Merge tag 'media/v4.12-1' of git://git.kernel.org/pub/scm/linux/kernel/git/mchehab/linux-media

Pull media updates from Mauro Carvalho Chehab:
 "Media updates for v4.12-rc1:

   - new driver to support mediatek jpeg in hardware codec

   - rc-lirc, s5p-cec and st-cec staging drivers got promoted

   - hardware histogram support for vsp1 driver

   - added Virtual Media Controller driver, to make easier to test the
     media controller

   - added a new CEC driver (rainshadow-cec)

   - removed two staging LIRC drivers for obscure hardware that are too
     obsolete

   - added support for Intel SR300 Depth camera

   - some improvements at CEC and RC core

   - lots of driver cleanups, improvements all over the tree

  With this series, we're finally getting rid of the LIRC staging
  driver. There's just one left (lirc_zilog), with require more care,
  as part of its functionality (IR RX) is already provided by another
  driver. Work in progress to convert it on the proper way"

* tag 'media/v4.12-1' of git://git.kernel.org/pub/scm/linux/kernel/git/mchehab/linux-media: (304 commits)
  [media] ov2640: print error if devm_*_optional*() fails
  [media] atmel-isc: Fix the static checker warning
  [media] ov2640: add support for MEDIA_BUS_FMT_YVYU8_2X8 and MEDIA_BUS_FMT_VYUY8_2X8
  [media] ov2640: fix vflip control
  [media] ov2640: fix duplicate width+height returning from ov2640_select_win()
  [media] ov2640: add missing write to size change preamble
  [media] ov2640: add information about DSP register 0xc7
  [media] ov2640: improve banding filter register definitions/documentation
  [media] ov2640: fix init sequence alignment
  [media] ov2640: make GPIOLIB an optional dependency
  [media] xc5000: fix spelling mistake: "calibration"
  [media] vidioc-queryctrl.rst: fix menu/int menu references
  [media] media-entity: only call dev_dbg_obj if mdev is not NULL
  [media] pixfmt-meta-vsp1-hgo.rst: remove spurious '-'
  [media] mtk-vcodec: avoid warnings because of empty macros
  [media] coda: bump maximum number of internal framebuffers to 17
  [media] media: mtk-vcodec: remove informative log
  [media] subdev-formats.rst: remove spurious '-'
  [media] dw2102: limit messages to buffer size
  [media] ttusb2: limit messages to buffer size
  ...

260 files changed, 14372 insertions, 2947 deletions

diff --git a/drivers/media/Kconfig b/drivers/media/Kconfig
index 3512316e7a46..b72edd27f880 100644
--- a/drivers/media/Kconfig
+++ b/drivers/media/Kconfig
@@ -81,23 +81,15 @@ config MEDIA_RC_SUPPORT
           Say Y when you have a TV or an IR device.
 
 config MEDIA_CEC_SUPPORT
-        bool "HDMI CEC support"
-        select MEDIA_CEC_EDID
-        ---help---
-          Enable support for HDMI CEC (Consumer Electronics Control),
-          which is an optional HDMI feature.
-
-          Say Y when you have an HDMI receiver, transmitter or a USB CEC
-          adapter that supports HDMI CEC.
+       bool "HDMI CEC support"
+       ---help---
+         Enable support for HDMI CEC (Consumer Electronics Control),
+         which is an optional HDMI feature.
 
-config MEDIA_CEC_DEBUG
-        bool "HDMI CEC debugfs interface"
-        depends on MEDIA_CEC_SUPPORT && DEBUG_FS
-        ---help---
-          Turns on the DebugFS interface for CEC devices.
+         Say Y when you have an HDMI receiver, transmitter or a USB CEC
+         adapter that supports HDMI CEC.
 
-config MEDIA_CEC_EDID
-        bool
+source "drivers/media/cec/Kconfig"
 
 #
 # Media controller
diff --git a/drivers/media/Makefile b/drivers/media/Makefile
index d87ccb8eeabe..523fea3648ad 100644
--- a/drivers/media/Makefile
+++ b/drivers/media/Makefile
@@ -2,16 +2,10 @@
 # Makefile for the kernel multimedia device drivers.
 #
 
-ifeq ($(CONFIG_MEDIA_CEC_EDID),y)
-  obj-$(CONFIG_MEDIA_SUPPORT) += cec-edid.o
-endif
-
-ifeq ($(CONFIG_MEDIA_CEC_SUPPORT),y)
-  obj-$(CONFIG_MEDIA_SUPPORT) += cec/
-endif
-
 media-objs      := media-device.o media-devnode.o media-entity.o
 
+obj-$(CONFIG_CEC_CORE) += cec/
+
 #
 # I2C drivers should come before other drivers, otherwise they'll fail
 # when compiled as builtin drivers
diff --git a/drivers/media/cec/Kconfig b/drivers/media/cec/Kconfig
new file mode 100644
index 000000000000..f944d93e3167
--- /dev/null
+++ b/drivers/media/cec/Kconfig
@@ -0,0 +1,19 @@
+config CEC_CORE
+        tristate
+        depends on MEDIA_CEC_SUPPORT
+        default y
+
+config MEDIA_CEC_NOTIFIER
+        bool
+
+config MEDIA_CEC_RC
+        bool "HDMI CEC RC integration"
+        depends on CEC_CORE && RC_CORE
+        ---help---
+          Pass on CEC remote control messages to the RC framework.
+
+config MEDIA_CEC_DEBUG
+        bool "HDMI CEC debugfs interface"
+        depends on CEC_CORE && DEBUG_FS
+        ---help---
+          Turns on the DebugFS interface for CEC devices.
diff --git a/drivers/media/cec/Makefile b/drivers/media/cec/Makefile
index d6686337275f..402a6c62a3e8 100644
--- a/drivers/media/cec/Makefile
+++ b/drivers/media/cec/Makefile
@@ -1,5 +1,7 @@
-cec-objs := cec-core.o cec-adap.o cec-api.o
+cec-objs := cec-core.o cec-adap.o cec-api.o cec-edid.o
 
-ifeq ($(CONFIG_MEDIA_CEC_SUPPORT),y)
-  obj-$(CONFIG_MEDIA_SUPPORT) += cec.o
+ifeq ($(CONFIG_MEDIA_CEC_NOTIFIER),y)
+  cec-objs += cec-notifier.o
 endif
+
+obj-$(CONFIG_CEC_CORE) += cec.o
diff --git a/drivers/media/cec/cec-adap.c b/drivers/media/cec/cec-adap.c
index ccda41c2c9e4..f5fe01c9da8a 100644
--- a/drivers/media/cec/cec-adap.c
+++ b/drivers/media/cec/cec-adap.c
@@ -300,6 +300,40 @@ static void cec_data_cancel(struct cec_data *data)
 }
 
 /*
+ * Flush all pending transmits and cancel any pending timeout work.
+ *
+ * This function is called with adap->lock held.
+ */
+static void cec_flush(struct cec_adapter *adap)
+{
+        struct cec_data *data, *n;
+
+        /*
+         * If the adapter is disabled, or we're asked to stop,
+         * then cancel any pending transmits.
+         */
+        while (!list_empty(&adap->transmit_queue)) {
+                data = list_first_entry(&adap->transmit_queue,
+                                        struct cec_data, list);
+                cec_data_cancel(data);
+        }
+        if (adap->transmitting)
+                cec_data_cancel(adap->transmitting);
+
+        /* Cancel the pending timeout work. */
+        list_for_each_entry_safe(data, n, &adap->wait_queue, list) {
+                if (cancel_delayed_work(&data->work))
+                        cec_data_cancel(data);
+                /*
+                 * If cancel_delayed_work returned false, then
+                 * the cec_wait_timeout function is running,
+                 * which will call cec_data_completed. So no
+                 * need to do anything special in that case.
+                 */
+        }
+}
+
+/*
  * Main CEC state machine
  *
  * Wait until the thread should be stopped, or we are not transmitting and
@@ -333,7 +367,6 @@ int cec_thread_func(void *_adap)
                          */
                         err = wait_event_interruptible_timeout(adap->kthread_waitq,
                                 kthread_should_stop() ||
-                                (!adap->is_configured && !adap->is_configuring) ||
                                 (!adap->transmitting &&
                                  !list_empty(&adap->transmit_queue)),
                                 msecs_to_jiffies(CEC_XFER_TIMEOUT_MS));
@@ -348,39 +381,8 @@ int cec_thread_func(void *_adap)
 
                 mutex_lock(&adap->lock);
 
-                if ((!adap->is_configured && !adap->is_configuring) ||
-                    kthread_should_stop()) {
-                        /*
-                         * If the adapter is disabled, or we're asked to stop,
-                         * then cancel any pending transmits.
-                         */
-                        while (!list_empty(&adap->transmit_queue)) {
-                                data = list_first_entry(&adap->transmit_queue,
-                                                        struct cec_data, list);
-                                cec_data_cancel(data);
-                        }
-                        if (adap->transmitting)
-                                cec_data_cancel(adap->transmitting);
-
-                        /*
-                         * Cancel the pending timeout work. We have to unlock
-                         * the mutex when flushing the work since
-                         * cec_wait_timeout() will take it. This is OK since
-                         * no new entries can be added to wait_queue as long
-                         * as adap->transmitting is NULL, which it is due to
-                         * the cec_data_cancel() above.
-                         */
-                        while (!list_empty(&adap->wait_queue)) {
-                                data = list_first_entry(&adap->wait_queue,
-                                                        struct cec_data, list);
-
-                                if (!cancel_delayed_work(&data->work)) {
-                                        mutex_unlock(&adap->lock);
-                                        flush_scheduled_work();
-                                        mutex_lock(&adap->lock);
-                                }
-                                cec_data_cancel(data);
-                        }
+                if (kthread_should_stop()) {
+                        cec_flush(adap);
                         goto unlock;
                 }
 
@@ -410,6 +412,7 @@ int cec_thread_func(void *_adap)
                                         struct cec_data, list);
                 list_del_init(&data->list);
                 adap->transmit_queue_sz--;
+
                 /* Make this the current transmitting message */
                 adap->transmitting = data;
 
@@ -603,17 +606,17 @@ int cec_transmit_msg_fh(struct cec_adapter *adap, struct cec_msg *msg,
 
         /* Sanity checks */
         if (msg->len == 0 || msg->len > CEC_MAX_MSG_SIZE) {
-                dprintk(1, "cec_transmit_msg: invalid length %d\n", msg->len);
+                dprintk(1, "%s: invalid length %d\n", __func__, msg->len);
                 return -EINVAL;
         }
         if (msg->timeout && msg->len == 1) {
-                dprintk(1, "cec_transmit_msg: can't reply for poll msg\n");
+                dprintk(1, "%s: can't reply for poll msg\n", __func__);
                 return -EINVAL;
         }
         memset(msg->msg + msg->len, 0, sizeof(msg->msg) - msg->len);
         if (msg->len == 1) {
                 if (cec_msg_destination(msg) == 0xf) {
-                        dprintk(1, "cec_transmit_msg: invalid poll message\n");
+                        dprintk(1, "%s: invalid poll message\n", __func__);
                         return -EINVAL;
                 }
                 if (cec_has_log_addr(adap, cec_msg_destination(msg))) {
@@ -634,20 +637,30 @@ int cec_transmit_msg_fh(struct cec_adapter *adap, struct cec_msg *msg,
         }
         if (msg->len > 1 && !cec_msg_is_broadcast(msg) &&
             cec_has_log_addr(adap, cec_msg_destination(msg))) {
-                dprintk(1, "cec_transmit_msg: destination is the adapter itself\n");
+                dprintk(1, "%s: destination is the adapter itself\n", __func__);
                 return -EINVAL;
         }
         if (msg->len > 1 && adap->is_configured &&
             !cec_has_log_addr(adap, cec_msg_initiator(msg))) {
-                dprintk(1, "cec_transmit_msg: initiator has unknown logical address %d\n",
-                        cec_msg_initiator(msg));
+                dprintk(1, "%s: initiator has unknown logical address %d\n",
+                        __func__, cec_msg_initiator(msg));
                 return -EINVAL;
         }
-        if (!adap->is_configured && !adap->is_configuring)
-                return -ENONET;
+        if (!adap->is_configured && !adap->is_configuring) {
+                if (msg->msg[0] != 0xf0) {
+                        dprintk(1, "%s: adapter is unconfigured\n", __func__);
+                        return -ENONET;
+                }
+                if (msg->reply) {
+                        dprintk(1, "%s: invalid msg->reply\n", __func__);
+                        return -EINVAL;
+                }
+        }
 
-        if (adap->transmit_queue_sz >= CEC_MAX_MSG_TX_QUEUE_SZ)
+        if (adap->transmit_queue_sz >= CEC_MAX_MSG_TX_QUEUE_SZ) {
+                dprintk(1, "%s: transmit queue full\n", __func__);
                 return -EBUSY;
+        }
 
         data = kzalloc(sizeof(*data), GFP_KERNEL);
         if (!data)
@@ -659,11 +672,11 @@ int cec_transmit_msg_fh(struct cec_adapter *adap, struct cec_msg *msg,
         }
 
         if (msg->timeout)
-                dprintk(2, "cec_transmit_msg: %*ph (wait for 0x%02x%s)\n",
-                        msg->len, msg->msg, msg->reply, !block ? ", nb" : "");
+                dprintk(2, "%s: %*ph (wait for 0x%02x%s)\n",
+                        __func__, msg->len, msg->msg, msg->reply, !block ? ", nb" : "");
         else
-                dprintk(2, "cec_transmit_msg: %*ph%s\n",
-                        msg->len, msg->msg, !block ? " (nb)" : "");
+                dprintk(2, "%s: %*ph%s\n",
+                        __func__, msg->len, msg->msg, !block ? " (nb)" : "");
 
         data->msg = *msg;
         data->fh = fh;
@@ -692,6 +705,7 @@ int cec_transmit_msg_fh(struct cec_adapter *adap, struct cec_msg *msg,
 
         if (fh)
                 list_add_tail(&data->xfer_list, &fh->xfer_list);
+
         list_add_tail(&data->list, &adap->transmit_queue);
         adap->transmit_queue_sz++;
         if (!adap->transmitting)
@@ -1117,6 +1131,7 @@ static void cec_adap_unconfigure(struct cec_adapter *adap)
         adap->is_configuring = false;
         adap->is_configured = false;
         memset(adap->phys_addrs, 0xff, sizeof(adap->phys_addrs));
+        cec_flush(adap);
         wake_up_interruptible(&adap->kthread_waitq);
         cec_post_state_event(adap);
 }
@@ -1348,19 +1363,30 @@ void __cec_s_phys_addr(struct cec_adapter *adap, u16 phys_addr, bool block)
                 /* Disabling monitor all mode should always succeed */
                 if (adap->monitor_all_cnt)
                         WARN_ON(call_op(adap, adap_monitor_all_enable, false));
-                WARN_ON(adap->ops->adap_enable(adap, false));
+                mutex_lock(&adap->devnode.lock);
+                if (list_empty(&adap->devnode.fhs))
+                        WARN_ON(adap->ops->adap_enable(adap, false));
+                mutex_unlock(&adap->devnode.lock);
                 if (phys_addr == CEC_PHYS_ADDR_INVALID)
                         return;
         }
 
-        if (adap->ops->adap_enable(adap, true))
+        mutex_lock(&adap->devnode.lock);
+        if (list_empty(&adap->devnode.fhs) &&
+            adap->ops->adap_enable(adap, true)) {
+                mutex_unlock(&adap->devnode.lock);
                 return;
+        }
 
         if (adap->monitor_all_cnt &&
             call_op(adap, adap_monitor_all_enable, true)) {
-                WARN_ON(adap->ops->adap_enable(adap, false));
+                if (list_empty(&adap->devnode.fhs))
+                        WARN_ON(adap->ops->adap_enable(adap, false));
+                mutex_unlock(&adap->devnode.lock);
                 return;
         }
+        mutex_unlock(&adap->devnode.lock);
+
         adap->phys_addr = phys_addr;
         cec_post_state_event(adap);
         if (adap->log_addrs.num_log_addrs)
@@ -1435,12 +1461,16 @@ int __cec_s_log_addrs(struct cec_adapter *adap,
          * within the correct range.
          */
         if (log_addrs->vendor_id != CEC_VENDOR_ID_NONE &&
-            (log_addrs->vendor_id & 0xff000000) != 0)
+            (log_addrs->vendor_id & 0xff000000) != 0) {
+                dprintk(1, "invalid vendor ID\n");
                 return -EINVAL;
+        }
 
         if (log_addrs->cec_version != CEC_OP_CEC_VERSION_1_4 &&
-            log_addrs->cec_version != CEC_OP_CEC_VERSION_2_0)
+            log_addrs->cec_version != CEC_OP_CEC_VERSION_2_0) {
+                dprintk(1, "invalid CEC version\n");
                 return -EINVAL;
+        }
 
         if (log_addrs->num_log_addrs > 1)
                 for (i = 0; i < log_addrs->num_log_addrs; i++)
@@ -1585,6 +1615,9 @@ static int cec_feature_abort_reason(struct cec_adapter *adap,
          */
         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);
@@ -1699,7 +1732,7 @@ static int cec_receive_notify(struct cec_adapter *adap, struct cec_msg *msg,
                     !(adap->log_addrs.flags & CEC_LOG_ADDRS_FL_ALLOW_RC_PASSTHRU))
                         break;
 
-#if IS_REACHABLE(CONFIG_RC_CORE)
+#ifdef CONFIG_MEDIA_CEC_RC
                 switch (msg->msg[2]) {
                 /*
                  * Play function, this message can have variable length
@@ -1736,7 +1769,7 @@ static int cec_receive_notify(struct cec_adapter *adap, struct cec_msg *msg,
                 if (!(adap->capabilities & CEC_CAP_RC) ||
                     !(adap->log_addrs.flags & CEC_LOG_ADDRS_FL_ALLOW_RC_PASSTHRU))
                         break;
-#if IS_REACHABLE(CONFIG_RC_CORE)
+#ifdef CONFIG_MEDIA_CEC_RC
                 rc_keyup(adap->rc);
 #endif
                 break;
diff --git a/drivers/media/cec/cec-api.c b/drivers/media/cec/cec-api.c
index 8950b6c9d6a9..0860fb458757 100644
--- a/drivers/media/cec/cec-api.c
+++ b/drivers/media/cec/cec-api.c
@@ -198,7 +198,11 @@ static long cec_transmit(struct cec_adapter *adap, struct cec_fh *fh,
                 return -EINVAL;
 
         mutex_lock(&adap->lock);
-        if (!adap->is_configured)
+        if (adap->log_addrs.num_log_addrs == 0)
+                err = -EPERM;
+        else if (adap->is_configuring)
+                err = -ENONET;
+        else if (!adap->is_configured && msg.msg[0] != 0xf0)
                 err = -ENONET;
         else if (cec_is_busy(adap, fh))
                 err = -EBUSY;
@@ -515,9 +519,18 @@ static int cec_open(struct inode *inode, struct file *filp)
                 return err;
         }
 
+        mutex_lock(&devnode->lock);
+        if (list_empty(&devnode->fhs) &&
+            adap->phys_addr == CEC_PHYS_ADDR_INVALID) {
+                err = adap->ops->adap_enable(adap, true);
+                if (err) {
+                        mutex_unlock(&devnode->lock);
+                        kfree(fh);
+                        return err;
+                }
+        }
         filp->private_data = fh;
 
-        mutex_lock(&devnode->lock);
         /* Queue up initial state events */
         ev_state.state_change.phys_addr = adap->phys_addr;
         ev_state.state_change.log_addr_mask = adap->log_addrs.log_addr_mask;
@@ -551,6 +564,10 @@ static int cec_release(struct inode *inode, struct file *filp)
 
         mutex_lock(&devnode->lock);
         list_del(&fh->list);
+        if (list_empty(&devnode->fhs) &&
+            adap->phys_addr == CEC_PHYS_ADDR_INVALID) {
+                WARN_ON(adap->ops->adap_enable(adap, false));
+        }
         mutex_unlock(&devnode->lock);
 
         /* Unhook pending transmits from this filehandle. */
diff --git a/drivers/media/cec/cec-core.c b/drivers/media/cec/cec-core.c
index 3163e038a364..f9ebff90f8eb 100644
--- a/drivers/media/cec/cec-core.c
+++ b/drivers/media/cec/cec-core.c
@@ -187,6 +187,24 @@ static void cec_devnode_unregister(struct cec_devnode *devnode)
         put_device(&devnode->dev);
 }
 
+#ifdef CONFIG_MEDIA_CEC_NOTIFIER
+static void cec_cec_notify(struct cec_adapter *adap, u16 pa)
+{
+        cec_s_phys_addr(adap, pa, false);
+}
+
+void cec_register_cec_notifier(struct cec_adapter *adap,
+                               struct cec_notifier *notifier)
+{
+        if (WARN_ON(!adap->devnode.registered))
+                return;
+
+        adap->notifier = notifier;
+        cec_notifier_register(adap->notifier, adap, cec_cec_notify);
+}
+EXPORT_SYMBOL_GPL(cec_register_cec_notifier);
+#endif
+
 struct cec_adapter *cec_allocate_adapter(const struct cec_adap_ops *ops,
                                          void *priv, const char *name, u32 caps,
                                          u8 available_las)
@@ -194,6 +212,10 @@ struct cec_adapter *cec_allocate_adapter(const struct cec_adap_ops *ops,
         struct cec_adapter *adap;
         int res;
 
+#ifndef CONFIG_MEDIA_CEC_RC
+        caps &= ~CEC_CAP_RC;
+#endif
+
         if (WARN_ON(!caps))
                 return ERR_PTR(-EINVAL);
         if (WARN_ON(!ops))
@@ -226,10 +248,10 @@ struct cec_adapter *cec_allocate_adapter(const struct cec_adap_ops *ops,
                 return ERR_PTR(res);
         }
 
+#ifdef CONFIG_MEDIA_CEC_RC
         if (!(caps & CEC_CAP_RC))
                 return adap;
 
-#if IS_REACHABLE(CONFIG_RC_CORE)
         /* Prepare the RC input device */
         adap->rc = rc_allocate_device(RC_DRIVER_SCANCODE);
         if (!adap->rc) {
@@ -256,8 +278,6 @@ struct cec_adapter *cec_allocate_adapter(const struct cec_adap_ops *ops,
         adap->rc->priv = adap;
         adap->rc->map_name = RC_MAP_CEC;
         adap->rc->timeout = MS_TO_NS(100);
-#else
-        adap->capabilities &= ~CEC_CAP_RC;
 #endif
         return adap;
 }
@@ -277,9 +297,9 @@ int cec_register_adapter(struct cec_adapter *adap,
         adap->owner = parent->driver->owner;
         adap->devnode.dev.parent = parent;
 
-#if IS_REACHABLE(CONFIG_RC_CORE)
-        adap->rc->dev.parent = parent;
+#ifdef CONFIG_MEDIA_CEC_RC
         if (adap->capabilities & CEC_CAP_RC) {
+                adap->rc->dev.parent = parent;
                 res = rc_register_device(adap->rc);
 
                 if (res) {
@@ -294,7 +314,7 @@ int cec_register_adapter(struct cec_adapter *adap,
 
         res = cec_devnode_register(&adap->devnode, adap->owner);
         if (res) {
-#if IS_REACHABLE(CONFIG_RC_CORE)
+#ifdef CONFIG_MEDIA_CEC_RC
                 /* Note: rc_unregister also calls rc_free */
                 rc_unregister_device(adap->rc);
                 adap->rc = NULL;
@@ -329,12 +349,16 @@ void cec_unregister_adapter(struct cec_adapter *adap)
         if (IS_ERR_OR_NULL(adap))
                 return;
 
-#if IS_REACHABLE(CONFIG_RC_CORE)
+#ifdef CONFIG_MEDIA_CEC_RC
         /* Note: rc_unregister also calls rc_free */
         rc_unregister_device(adap->rc);
         adap->rc = NULL;
 #endif
         debugfs_remove_recursive(adap->cec_dir);
+#ifdef CONFIG_MEDIA_CEC_NOTIFIER
+        if (adap->notifier)
+                cec_notifier_unregister(adap->notifier);
+#endif
         cec_devnode_unregister(&adap->devnode);
 }
 EXPORT_SYMBOL_GPL(cec_unregister_adapter);
@@ -349,7 +373,7 @@ void cec_delete_adapter(struct cec_adapter *adap)
         kthread_stop(adap->kthread);
         if (adap->kthread_config)
                 kthread_stop(adap->kthread_config);
-#if IS_REACHABLE(CONFIG_RC_CORE)
+#ifdef CONFIG_MEDIA_CEC_RC
         rc_free_device(adap->rc);
 #endif
         kfree(adap);
diff --git a/drivers/media/cec-edid.c b/drivers/media/cec/cec-edid.c
index 5719b991e340..38e3fec6152b 100644
--- a/drivers/media/cec-edid.c
+++ b/drivers/media/cec/cec-edid.c
@@ -20,7 +20,7 @@
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/types.h>
-#include <media/cec-edid.h>
+#include <media/cec.h>
 
 /*
  * This EDID is expected to be a CEA-861 compliant, which means that there are
@@ -165,7 +165,3 @@ int cec_phys_addr_validate(u16 phys_addr, u16 *parent, u16 *port)
         return 0;
 }
 EXPORT_SYMBOL_GPL(cec_phys_addr_validate);
-
-MODULE_AUTHOR("Hans Verkuil <hans.verkuil@cisco.com>");
-MODULE_DESCRIPTION("CEC EDID helper functions");
-MODULE_LICENSE("GPL");
diff --git a/drivers/media/cec/cec-notifier.c b/drivers/media/cec/cec-notifier.c
new file mode 100644
index 000000000000..74dc1c32080e
--- /dev/null
+++ b/drivers/media/cec/cec-notifier.c
@@ -0,0 +1,130 @@
+/*
+ * cec-notifier.c - notify CEC drivers of physical address changes
+ *
+ * Copyright 2016 Russell King <rmk+kernel@arm.linux.org.uk>
+ * Copyright 2016-2017 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/export.h>
+#include <linux/string.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <linux/kref.h>
+
+#include <media/cec.h>
+#include <media/cec-notifier.h>
+#include <drm/drm_edid.h>
+
+struct cec_notifier {
+        struct mutex lock;
+        struct list_head head;
+        struct kref kref;
+        struct device *dev;
+        struct cec_adapter *cec_adap;
+        void (*callback)(struct cec_adapter *adap, u16 pa);
+
+        u16 phys_addr;
+};
+
+static LIST_HEAD(cec_notifiers);
+static DEFINE_MUTEX(cec_notifiers_lock);
+
+struct cec_notifier *cec_notifier_get(struct device *dev)
+{
+        struct cec_notifier *n;
+
+        mutex_lock(&cec_notifiers_lock);
+        list_for_each_entry(n, &cec_notifiers, head) {
+                if (n->dev == dev) {
+                        kref_get(&n->kref);
+                        mutex_unlock(&cec_notifiers_lock);
+                        return n;
+                }
+        }
+        n = kzalloc(sizeof(*n), GFP_KERNEL);
+        if (!n)
+                goto unlock;
+        n->dev = dev;
+        n->phys_addr = CEC_PHYS_ADDR_INVALID;
+        mutex_init(&n->lock);
+        kref_init(&n->kref);
+        list_add_tail(&n->head, &cec_notifiers);
+unlock:
+        mutex_unlock(&cec_notifiers_lock);
+        return n;
+}
+EXPORT_SYMBOL_GPL(cec_notifier_get);
+
+static void cec_notifier_release(struct kref *kref)
+{
+        struct cec_notifier *n =
+                container_of(kref, struct cec_notifier, kref);
+
+        list_del(&n->head);
+        kfree(n);
+}
+
+void cec_notifier_put(struct cec_notifier *n)
+{
+        mutex_lock(&cec_notifiers_lock);
+        kref_put(&n->kref, cec_notifier_release);
+        mutex_unlock(&cec_notifiers_lock);
+}
+EXPORT_SYMBOL_GPL(cec_notifier_put);
+
+void cec_notifier_set_phys_addr(struct cec_notifier *n, u16 pa)
+{
+        mutex_lock(&n->lock);
+        n->phys_addr = pa;
+        if (n->callback)
+                n->callback(n->cec_adap, n->phys_addr);
+        mutex_unlock(&n->lock);
+}
+EXPORT_SYMBOL_GPL(cec_notifier_set_phys_addr);
+
+void cec_notifier_set_phys_addr_from_edid(struct cec_notifier *n,
+                                          const struct edid *edid)
+{
+        u16 pa = CEC_PHYS_ADDR_INVALID;
+
+        if (edid && edid->extensions)
+                pa = cec_get_edid_phys_addr((const u8 *)edid,
+                                EDID_LENGTH * (edid->extensions + 1), NULL);
+        cec_notifier_set_phys_addr(n, pa);
+}
+EXPORT_SYMBOL_GPL(cec_notifier_set_phys_addr_from_edid);
+
+void cec_notifier_register(struct cec_notifier *n,
+                           struct cec_adapter *adap,
+                           void (*callback)(struct cec_adapter *adap, u16 pa))
+{
+        kref_get(&n->kref);
+        mutex_lock(&n->lock);
+        n->cec_adap = adap;
+        n->callback = callback;
+        n->callback(adap, n->phys_addr);
+        mutex_unlock(&n->lock);
+}
+EXPORT_SYMBOL_GPL(cec_notifier_register);
+
+void cec_notifier_unregister(struct cec_notifier *n)
+{
+        mutex_lock(&n->lock);
+        n->callback = NULL;
+        mutex_unlock(&n->lock);
+        cec_notifier_put(n);
+}
+EXPORT_SYMBOL_GPL(cec_notifier_unregister);
diff --git a/drivers/media/common/b2c2/flexcop-fe-tuner.c b/drivers/media/common/b2c2/flexcop-fe-tuner.c
index 5f10151ecec9..7636606f0be5 100644
--- a/drivers/media/common/b2c2/flexcop-fe-tuner.c
+++ b/drivers/media/common/b2c2/flexcop-fe-tuner.c
@@ -473,7 +473,7 @@ static int airstar_atsc1_attach(struct flexcop_device *fc,
 
 /* AirStar ATSC 2nd generation */
 #if FE_SUPPORTED(NXT200X) && FE_SUPPORTED(PLL)
-static struct nxt200x_config samsung_tbmv_config = {
+static const struct nxt200x_config samsung_tbmv_config = {
         .demod_address = 0x0a,
 };
 
diff --git a/drivers/media/common/saa7146/saa7146_vbi.c b/drivers/media/common/saa7146/saa7146_vbi.c
index 49237518d65f..3553ac4cba5c 100644
--- a/drivers/media/common/saa7146/saa7146_vbi.c
+++ b/drivers/media/common/saa7146/saa7146_vbi.c
@@ -365,9 +365,8 @@ static void vbi_init(struct saa7146_dev *dev, struct saa7146_vv *vv)
 
         INIT_LIST_HEAD(&vv->vbi_dmaq.queue);
 
-        init_timer(&vv->vbi_dmaq.timeout);
-        vv->vbi_dmaq.timeout.function = saa7146_buffer_timeout;
-        vv->vbi_dmaq.timeout.data     = (unsigned long)(&vv->vbi_dmaq);
+        setup_timer(&vv->vbi_dmaq.timeout, saa7146_buffer_timeout,
+                    (unsigned long)(&vv->vbi_dmaq));
         vv->vbi_dmaq.dev              = dev;
 
         init_waitqueue_head(&vv->vbi_wq);
diff --git a/drivers/media/common/saa7146/saa7146_video.c b/drivers/media/common/saa7146/saa7146_video.c
index e034bcfcf757..b3b29d4f36ed 100644
--- a/drivers/media/common/saa7146/saa7146_video.c
+++ b/drivers/media/common/saa7146/saa7146_video.c
@@ -1201,9 +1201,8 @@ static void video_init(struct saa7146_dev *dev, struct saa7146_vv *vv)
 {
         INIT_LIST_HEAD(&vv->video_dmaq.queue);
 
-        init_timer(&vv->video_dmaq.timeout);
-        vv->video_dmaq.timeout.function = saa7146_buffer_timeout;
-        vv->video_dmaq.timeout.data     = (unsigned long)(&vv->video_dmaq);
+        setup_timer(&vv->video_dmaq.timeout, saa7146_buffer_timeout,
+                    (unsigned long)(&vv->video_dmaq));
         vv->video_dmaq.dev              = dev;
 
         /* set some default values */
diff --git a/drivers/media/common/tveeprom.c b/drivers/media/common/tveeprom.c
index 6e1020227f9f..ccf2d3b12aec 100644
--- a/drivers/media/common/tveeprom.c
+++ b/drivers/media/common/tveeprom.c
@@ -420,8 +420,8 @@ static int hasRadioTuner(int tunerType)
         return 0;
 }
 
-void tveeprom_hauppauge_analog(struct i2c_client *c, struct tveeprom *tvee,
-                                unsigned char *eeprom_data)
+void tveeprom_hauppauge_analog(struct tveeprom *tvee,
+                               unsigned char *eeprom_data)
 {
         /* ----------------------------------------------
         ** The hauppauge eeprom format is tagged
diff --git a/drivers/media/common/v4l2-tpg/v4l2-tpg-core.c b/drivers/media/common/v4l2-tpg/v4l2-tpg-core.c
index e47b46e2d26c..3dd22da7e17d 100644
--- a/drivers/media/common/v4l2-tpg/v4l2-tpg-core.c
+++ b/drivers/media/common/v4l2-tpg/v4l2-tpg-core.c
@@ -927,7 +927,14 @@ static void precalculate_color(struct tpg_data *tpg, int k)
                 y >>= 4;
                 cb >>= 4;
                 cr >>= 4;
-                if (tpg->real_quantization == V4L2_QUANTIZATION_LIM_RANGE) {
+                /*
+                 * XV601/709 use the header/footer margins to encode R', G'
+                 * and B' values outside the range [0-1]. So do not clamp
+                 * XV601/709 values.
+                 */
+                if (tpg->real_quantization == V4L2_QUANTIZATION_LIM_RANGE &&
+                    tpg->real_ycbcr_enc != V4L2_YCBCR_ENC_XV601 &&
+                    tpg->real_ycbcr_enc != V4L2_YCBCR_ENC_XV709) {
                         y = clamp(y, 16, 235);
                         cb = clamp(cb, 16, 240);
                         cr = clamp(cr, 16, 240);
diff --git a/drivers/media/dvb-core/dvb_ca_en50221.c b/drivers/media/dvb-core/dvb_ca_en50221.c
index 8d65028c7a74..d38bf9bce480 100644
--- a/drivers/media/dvb-core/dvb_ca_en50221.c
+++ b/drivers/media/dvb-core/dvb_ca_en50221.c
@@ -785,6 +785,29 @@ static int dvb_ca_en50221_write_data(struct dvb_ca_private *ca, int slot, u8 * b
                 goto exit;
         }
 
+        /*
+         * It may need some time for the CAM to settle down, or there might
+         * be a race condition between the CAM, writing HC and our last
+         * check for DA. This happens, if the CAM asserts DA, just after
+         * checking DA before we are setting HC. In this case it might be
+         * a bug in the CAM to keep the FR bit, the lower layer/HW
+         * communication requires a longer timeout or the CAM needs more
+         * time internally. But this happens in reality!
+         * We need to read the status from the HW again and do the same
+         * we did for the previous check for DA
+         */
+        status = ca->pub->read_cam_control(ca->pub, slot, CTRLIF_STATUS);
+        if (status < 0)
+                goto exit;
+
+        if (status & (STATUSREG_DA | STATUSREG_RE)) {
+                if (status & STATUSREG_DA)
+                        dvb_ca_en50221_thread_wakeup(ca);
+
+                status = -EAGAIN;
+                goto exit;
+        }
+
         /* send the amount of data */
         if ((status = ca->pub->write_cam_control(ca->pub, slot, CTRLIF_SIZE_HIGH, bytes_write >> 8)) != 0)
                 goto exit;
diff --git a/drivers/media/dvb-core/dvb_frontend.h b/drivers/media/dvb-core/dvb_frontend.h
index 482912d3b77a..907a05bde162 100644
--- a/drivers/media/dvb-core/dvb_frontend.h
+++ b/drivers/media/dvb-core/dvb_frontend.h
@@ -643,6 +643,8 @@ struct dtv_frontend_properties {
 /**
  * struct dvb_frontend - Frontend structure to be used on drivers.
  *
+ * @refcount:           refcount to keep track of struct dvb_frontend
+ *                      references
  * @ops:                embedded struct dvb_frontend_ops
  * @dvb:                pointer to struct dvb_adapter
  * @demodulator_priv:   demod private data
diff --git a/drivers/media/dvb-frontends/cxd2841er.c b/drivers/media/dvb-frontends/cxd2841er.c
index 614bfb3740f1..ce37dc2e89c7 100644
--- a/drivers/media/dvb-frontends/cxd2841er.c
+++ b/drivers/media/dvb-frontends/cxd2841er.c
@@ -3852,7 +3852,9 @@ static struct dvb_frontend_ops cxd2841er_t_c_ops = {
                         FE_CAN_MUTE_TS |
                         FE_CAN_2G_MODULATION,
                 .frequency_min = 42000000,
-                .frequency_max = 1002000000
+                .frequency_max = 1002000000,
+                .symbol_rate_min = 870000,
+                .symbol_rate_max = 11700000
         },
         .init = cxd2841er_init_tc,
         .sleep = cxd2841er_sleep_tc,
diff --git a/drivers/media/dvb-frontends/drxk_hard.c b/drivers/media/dvb-frontends/drxk_hard.c
index 7e1bbbaad625..b5ea9192a341 100644
--- a/drivers/media/dvb-frontends/drxk_hard.c
+++ b/drivers/media/dvb-frontends/drxk_hard.c
@@ -1904,7 +1904,9 @@ static int get_lock_status(struct drxk_state *state, u32 *p_lock_status)
                 status = get_dvbt_lock_status(state, p_lock_status);
                 break;
         default:
-                break;
+                pr_debug("Unsupported operation mode %d in %s\n",
+                        state->m_operation_mode, __func__);
+                return 0;
         }
 error:
         if (status < 0)
diff --git a/drivers/media/dvb-frontends/mn88472.c b/drivers/media/dvb-frontends/mn88472.c
index 29dd13b36c28..f6938f9607ac 100644
--- a/drivers/media/dvb-frontends/mn88472.c
+++ b/drivers/media/dvb-frontends/mn88472.c
@@ -28,8 +28,9 @@ static int mn88472_read_status(struct dvb_frontend *fe, enum fe_status *status)
         struct i2c_client *client = fe->demodulator_priv;
         struct mn88472_dev *dev = i2c_get_clientdata(client);
         struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-        int ret;
-        unsigned int utmp;
+        int ret, i, stmp;
+        unsigned int utmp, utmp1, utmp2;
+        u8 buf[5];
 
         if (!dev->active) {
                 ret = -EAGAIN;
@@ -77,6 +78,127 @@ static int mn88472_read_status(struct dvb_frontend *fe, enum fe_status *status)
                 goto err;
         }
 
+        /* Signal strength */
+        if (*status & FE_HAS_SIGNAL) {
+                for (i = 0; i < 2; i++) {
+                        ret = regmap_bulk_read(dev->regmap[2], 0x8e + i,
+                                               &buf[i], 1);
+                        if (ret)
+                                goto err;
+                }
+
+                utmp1 = buf[0] << 8 | buf[1] << 0 | buf[0] >> 2;
+                dev_dbg(&client->dev, "strength=%u\n", utmp1);
+
+                c->strength.stat[0].scale = FE_SCALE_RELATIVE;
+                c->strength.stat[0].uvalue = utmp1;
+        } else {
+                c->strength.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+        }
+
+        /* CNR */
+        if (*status & FE_HAS_VITERBI && c->delivery_system == SYS_DVBT) {
+                /* DVB-T CNR */
+                ret = regmap_bulk_read(dev->regmap[0], 0x9c, buf, 2);
+                if (ret)
+                        goto err;
+
+                utmp = buf[0] << 8 | buf[1] << 0;
+                if (utmp) {
+                        /* CNR[dB]: 10 * log10(65536 / value) + 2 */
+                        /* log10(65536) = 80807124, 0.2 = 3355443 */
+                        stmp = ((u64)80807124 - intlog10(utmp) + 3355443)
+                               * 10000 >> 24;
+
+                        dev_dbg(&client->dev, "cnr=%d value=%u\n", stmp, utmp);
+                } else {
+                        stmp = 0;
+                }
+
+                c->cnr.stat[0].svalue = stmp;
+                c->cnr.stat[0].scale = FE_SCALE_DECIBEL;
+        } else if (*status & FE_HAS_VITERBI &&
+                   c->delivery_system == SYS_DVBT2) {
+                /* DVB-T2 CNR */
+                for (i = 0; i < 3; i++) {
+                        ret = regmap_bulk_read(dev->regmap[2], 0xbc + i,
+                                               &buf[i], 1);
+                        if (ret)
+                                goto err;
+                }
+
+                utmp = buf[1] << 8 | buf[2] << 0;
+                utmp1 = (buf[0] >> 2) & 0x01; /* 0=SISO, 1=MISO */
+                if (utmp) {
+                        if (utmp1) {
+                                /* CNR[dB]: 10 * log10(16384 / value) - 6 */
+                                /* log10(16384) = 70706234, 0.6 = 10066330 */
+                                stmp = ((u64)70706234 - intlog10(utmp)
+                                       - 10066330) * 10000 >> 24;
+                                dev_dbg(&client->dev, "cnr=%d value=%u MISO\n",
+                                        stmp, utmp);
+                        } else {
+                                /* CNR[dB]: 10 * log10(65536 / value) + 2 */
+                                /* log10(65536) = 80807124, 0.2 = 3355443 */
+                                stmp = ((u64)80807124 - intlog10(utmp)
+                                       + 3355443) * 10000 >> 24;
+
+                                dev_dbg(&client->dev, "cnr=%d value=%u SISO\n",
+                                        stmp, utmp);
+                        }
+                } else {
+                        stmp = 0;
+                }
+
+                c->cnr.stat[0].svalue = stmp;
+                c->cnr.stat[0].scale = FE_SCALE_DECIBEL;
+        } else if (*status & FE_HAS_VITERBI &&
+                   c->delivery_system == SYS_DVBC_ANNEX_A) {
+                /* DVB-C CNR */
+                ret = regmap_bulk_read(dev->regmap[1], 0xa1, buf, 4);
+                if (ret)
+                        goto err;
+
+                utmp1 = buf[0] << 8 | buf[1] << 0; /* signal */
+                utmp2 = buf[2] << 8 | buf[3] << 0; /* noise */
+                if (utmp1 && utmp2) {
+                        /* CNR[dB]: 10 * log10(8 * (signal / noise)) */
+                        /* log10(8) = 15151336 */
+                        stmp = ((u64)15151336 + intlog10(utmp1)
+                               - intlog10(utmp2)) * 10000 >> 24;
+
+                        dev_dbg(&client->dev, "cnr=%d signal=%u noise=%u\n",
+                                stmp, utmp1, utmp2);
+                } else {
+                        stmp = 0;
+                }
+
+                c->cnr.stat[0].svalue = stmp;
+                c->cnr.stat[0].scale = FE_SCALE_DECIBEL;
+        } else {
+                c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+        }
+
+        /* PER */
+        if (*status & FE_HAS_SYNC) {
+                ret = regmap_bulk_read(dev->regmap[0], 0xe1, buf, 4);
+                if (ret)
+                        goto err;
+
+                utmp1 = buf[0] << 8 | buf[1] << 0;
+                utmp2 = buf[2] << 8 | buf[3] << 0;
+                dev_dbg(&client->dev, "block_error=%u block_count=%u\n",
+                        utmp1, utmp2);
+
+                c->block_error.stat[0].scale = FE_SCALE_COUNTER;
+                c->block_error.stat[0].uvalue += utmp1;
+                c->block_count.stat[0].scale = FE_SCALE_COUNTER;
+                c->block_count.stat[0].uvalue += utmp2;
+        } else {
+                c->block_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+                c->block_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+        }
+
         return 0;
 err:
         dev_dbg(&client->dev, "failed=%d\n", ret);
@@ -462,6 +584,7 @@ static int mn88472_probe(struct i2c_client *client,
 {
         struct mn88472_config *pdata = client->dev.platform_data;
         struct mn88472_dev *dev;
+        struct dtv_frontend_properties *c;
         int ret;
         unsigned int utmp;
         static const struct regmap_config regmap_config = {
@@ -547,6 +670,13 @@ static int mn88472_probe(struct i2c_client *client,
         *pdata->fe = &dev->fe;
         i2c_set_clientdata(client, dev);
 
+        /* Init stats to indicate which stats are supported */
+        c = &dev->fe.dtv_property_cache;
+        c->strength.len = 1;
+        c->cnr.len = 1;
+        c->block_error.len = 1;
+        c->block_count.len = 1;
+
         /* Setup callbacks */
         pdata->get_dvb_frontend = mn88472_get_dvb_frontend;
 
diff --git a/drivers/media/dvb-frontends/mn88472_priv.h b/drivers/media/dvb-frontends/mn88472_priv.h
index cdf2597a25d1..fb50f56ba30b 100644
--- a/drivers/media/dvb-frontends/mn88472_priv.h
+++ b/drivers/media/dvb-frontends/mn88472_priv.h
@@ -18,6 +18,7 @@
 #define MN88472_PRIV_H
 
 #include "dvb_frontend.h"
+#include "dvb_math.h"
 #include "mn88472.h"
 #include <linux/firmware.h>
 #include <linux/regmap.h>
diff --git a/drivers/media/dvb-frontends/si2168.c b/drivers/media/dvb-frontends/si2168.c
index 680ba06c29fb..172fc367ccaa 100644
--- a/drivers/media/dvb-frontends/si2168.c
+++ b/drivers/media/dvb-frontends/si2168.c
@@ -740,6 +740,9 @@ static int si2168_probe(struct i2c_client *client,
         case SI2168_CHIP_ID_B40:
                 dev->firmware_name = SI2168_B40_FIRMWARE;
                 break;
+        case SI2168_CHIP_ID_D60:
+                dev->firmware_name = SI2168_D60_FIRMWARE;
+                break;
         default:
                 dev_dbg(&client->dev, "unknown chip version Si21%d-%c%c%c\n",
                         cmd.args[2], cmd.args[1], cmd.args[3], cmd.args[4]);
@@ -827,3 +830,4 @@ MODULE_LICENSE("GPL");
 MODULE_FIRMWARE(SI2168_A20_FIRMWARE);
 MODULE_FIRMWARE(SI2168_A30_FIRMWARE);
 MODULE_FIRMWARE(SI2168_B40_FIRMWARE);
+MODULE_FIRMWARE(SI2168_D60_FIRMWARE);
diff --git a/drivers/media/dvb-frontends/si2168_priv.h b/drivers/media/dvb-frontends/si2168_priv.h
index 2fecac6231ff..737cf416fbb2 100644
--- a/drivers/media/dvb-frontends/si2168_priv.h
+++ b/drivers/media/dvb-frontends/si2168_priv.h
@@ -26,6 +26,7 @@
 #define SI2168_A20_FIRMWARE "dvb-demod-si2168-a20-01.fw"
 #define SI2168_A30_FIRMWARE "dvb-demod-si2168-a30-01.fw"
 #define SI2168_B40_FIRMWARE "dvb-demod-si2168-b40-01.fw"
+#define SI2168_D60_FIRMWARE "dvb-demod-si2168-d60-01.fw"
 #define SI2168_B40_FIRMWARE_FALLBACK "dvb-demod-si2168-02.fw"
 
 /* state struct */
@@ -38,6 +39,7 @@ struct si2168_dev {
         #define SI2168_CHIP_ID_A20 ('A' << 24 | 68 << 16 | '2' << 8 | '0' << 0)
         #define SI2168_CHIP_ID_A30 ('A' << 24 | 68 << 16 | '3' << 8 | '0' << 0)
         #define SI2168_CHIP_ID_B40 ('B' << 24 | 68 << 16 | '4' << 8 | '0' << 0)
+        #define SI2168_CHIP_ID_D60 ('D' << 24 | 68 << 16 | '6' << 8 | '0' << 0)
         unsigned int chip_id;
         unsigned int version;
         const char *firmware_name;
diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
index cee1dae6e014..fd181c99ce11 100644
--- a/drivers/media/i2c/Kconfig
+++ b/drivers/media/i2c/Kconfig
@@ -209,7 +209,6 @@ config VIDEO_ADV7604
         depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API
         depends on GPIOLIB || COMPILE_TEST
         select HDMI
-        select MEDIA_CEC_EDID
         ---help---
           Support for the Analog Devices ADV7604 video decoder.
 
@@ -221,7 +220,7 @@ config VIDEO_ADV7604
 
 config VIDEO_ADV7604_CEC
         bool "Enable Analog Devices ADV7604 CEC support"
-        depends on VIDEO_ADV7604 && MEDIA_CEC_SUPPORT
+        depends on VIDEO_ADV7604 && CEC_CORE
         ---help---
           When selected the adv7604 will support the optional
           HDMI CEC feature.
@@ -230,7 +229,6 @@ config VIDEO_ADV7842
         tristate "Analog Devices ADV7842 decoder"
         depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API
         select HDMI
-        select MEDIA_CEC_EDID
         ---help---
           Support for the Analog Devices ADV7842 video decoder.
 
@@ -242,7 +240,7 @@ config VIDEO_ADV7842
 
 config VIDEO_ADV7842_CEC
         bool "Enable Analog Devices ADV7842 CEC support"
-        depends on VIDEO_ADV7842 && MEDIA_CEC_SUPPORT
+        depends on VIDEO_ADV7842 && CEC_CORE
         ---help---
           When selected the adv7842 will support the optional
           HDMI CEC feature.
@@ -470,7 +468,6 @@ config VIDEO_ADV7511
         tristate "Analog Devices ADV7511 encoder"
         depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API
         select HDMI
-        select MEDIA_CEC_EDID
         ---help---
           Support for the Analog Devices ADV7511 video encoder.
 
@@ -481,7 +478,7 @@ config VIDEO_ADV7511
 
 config VIDEO_ADV7511_CEC
         bool "Enable Analog Devices ADV7511 CEC support"
-        depends on VIDEO_ADV7511 && MEDIA_CEC_SUPPORT
+        depends on VIDEO_ADV7511 && CEC_CORE
         ---help---
           When selected the adv7511 will support the optional
           HDMI CEC feature.
@@ -520,6 +517,17 @@ config VIDEO_APTINA_PLL
 config VIDEO_SMIAPP_PLL
         tristate
 
+config VIDEO_OV2640
+        tristate "OmniVision OV2640 sensor support"
+        depends on VIDEO_V4L2 && I2C
+        depends on MEDIA_CAMERA_SUPPORT
+        help
+          This is a Video4Linux2 sensor-level driver for the OmniVision
+          OV2640 camera.
+
+          To compile this driver as a module, choose M here: the
+          module will be called ov2640.
+
 config VIDEO_OV2659
         tristate "OmniVision OV2659 sensor support"
         depends on VIDEO_V4L2 && I2C
@@ -531,6 +539,29 @@ config VIDEO_OV2659
           To compile this driver as a module, choose M here: the
           module will be called ov2659.
 
+config VIDEO_OV5645
+        tristate "OmniVision OV5645 sensor support"
+        depends on OF
+        depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+        depends on MEDIA_CAMERA_SUPPORT
+        ---help---
+          This is a Video4Linux2 sensor-level driver for the OmniVision
+          OV5645 camera.
+
+          To compile this driver as a module, choose M here: the
+          module will be called ov5645.
+
+config VIDEO_OV5647
+        tristate "OmniVision OV5647 sensor support"
+        depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+        depends on MEDIA_CAMERA_SUPPORT
+        ---help---
+          This is a Video4Linux2 sensor-level driver for the OmniVision
+          OV5647 camera.
+
+          To compile this driver as a module, choose M here: the
+          module will be called ov5647.
+
 config VIDEO_OV7640
         tristate "OmniVision OV7640 sensor support"
         depends on I2C && VIDEO_V4L2
diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
index 5bc7bbeb5499..62323ec66be8 100644
--- a/drivers/media/i2c/Makefile
+++ b/drivers/media/i2c/Makefile
@@ -57,6 +57,9 @@ obj-$(CONFIG_VIDEO_VP27SMPX) += vp27smpx.o
 obj-$(CONFIG_VIDEO_SONY_BTF_MPX) += sony-btf-mpx.o
 obj-$(CONFIG_VIDEO_UPD64031A) += upd64031a.o
 obj-$(CONFIG_VIDEO_UPD64083) += upd64083.o
+obj-$(CONFIG_VIDEO_OV2640) += ov2640.o
+obj-$(CONFIG_VIDEO_OV5645) += ov5645.o
+obj-$(CONFIG_VIDEO_OV5647) += ov5647.o
 obj-$(CONFIG_VIDEO_OV7640) += ov7640.o
 obj-$(CONFIG_VIDEO_OV7670) += ov7670.o
 obj-$(CONFIG_VIDEO_OV9650) += ov9650.o
diff --git a/drivers/media/i2c/ad5820.c b/drivers/media/i2c/ad5820.c
index a9026a91855e..3d2a3c6b67d8 100644
--- a/drivers/media/i2c/ad5820.c
+++ b/drivers/media/i2c/ad5820.c
@@ -336,7 +336,7 @@ cleanup:
         return ret;
 }
 
-static int __exit ad5820_remove(struct i2c_client *client)
+static int ad5820_remove(struct i2c_client *client)
 {
         struct v4l2_subdev *subdev = i2c_get_clientdata(client);
         struct ad5820_device *coil = to_ad5820_device(subdev);
@@ -362,7 +362,7 @@ static struct i2c_driver ad5820_i2c_driver = {
                 .pm     = &ad5820_pm,
         },
         .probe          = ad5820_probe,
-        .remove         = __exit_p(ad5820_remove),
+        .remove         = ad5820_remove,
         .id_table       = ad5820_id_table,
 };
 
diff --git a/drivers/media/i2c/adv7511.c b/drivers/media/i2c/adv7511.c
index 8c9e28949ab1..ccc478605643 100644
--- a/drivers/media/i2c/adv7511.c
+++ b/drivers/media/i2c/adv7511.c
@@ -734,7 +734,7 @@ static int adv7511_s_power(struct v4l2_subdev *sd, int on)
 #if IS_ENABLED(CONFIG_VIDEO_ADV7511_CEC)
 static int adv7511_cec_adap_enable(struct cec_adapter *adap, bool enable)
 {
-        struct adv7511_state *state = adap->priv;
+        struct adv7511_state *state = cec_get_drvdata(adap);
         struct v4l2_subdev *sd = &state->sd;
 
         if (state->i2c_cec == NULL)
@@ -769,7 +769,7 @@ static int adv7511_cec_adap_enable(struct cec_adapter *adap, bool enable)
 
 static int adv7511_cec_adap_log_addr(struct cec_adapter *adap, u8 addr)
 {
-        struct adv7511_state *state = adap->priv;
+        struct adv7511_state *state = cec_get_drvdata(adap);
         struct v4l2_subdev *sd = &state->sd;
         unsigned int i, free_idx = ADV7511_MAX_ADDRS;
 
@@ -824,7 +824,7 @@ static int adv7511_cec_adap_log_addr(struct cec_adapter *adap, u8 addr)
 static int adv7511_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
                                      u32 signal_free_time, struct cec_msg *msg)
 {
-        struct adv7511_state *state = adap->priv;
+        struct adv7511_state *state = cec_get_drvdata(adap);
         struct v4l2_subdev *sd = &state->sd;
         u8 len = msg->len;
         unsigned int i;
diff --git a/drivers/media/i2c/adv7604.c b/drivers/media/i2c/adv7604.c
index d8bf435db86d..f1fa9cec489f 100644
--- a/drivers/media/i2c/adv7604.c
+++ b/drivers/media/i2c/adv7604.c
@@ -2050,7 +2050,7 @@ static void adv76xx_cec_isr(struct v4l2_subdev *sd, bool *handled)
 
 static int adv76xx_cec_adap_enable(struct cec_adapter *adap, bool enable)
 {
-        struct adv76xx_state *state = adap->priv;
+        struct adv76xx_state *state = cec_get_drvdata(adap);
         struct v4l2_subdev *sd = &state->sd;
 
         if (!state->cec_enabled_adap && enable) {
@@ -2080,7 +2080,7 @@ static int adv76xx_cec_adap_enable(struct cec_adapter *adap, bool enable)
 
 static int adv76xx_cec_adap_log_addr(struct cec_adapter *adap, u8 addr)
 {
-        struct adv76xx_state *state = adap->priv;
+        struct adv76xx_state *state = cec_get_drvdata(adap);
         struct v4l2_subdev *sd = &state->sd;
         unsigned int i, free_idx = ADV76XX_MAX_ADDRS;
 
@@ -2135,7 +2135,7 @@ static int adv76xx_cec_adap_log_addr(struct cec_adapter *adap, u8 addr)
 static int adv76xx_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
                                      u32 signal_free_time, struct cec_msg *msg)
 {
-        struct adv76xx_state *state = adap->priv;
+        struct adv76xx_state *state = cec_get_drvdata(adap);
         struct v4l2_subdev *sd = &state->sd;
         u8 len = msg->len;
         unsigned int i;
diff --git a/drivers/media/i2c/adv7842.c b/drivers/media/i2c/adv7842.c
index 2d61f0cc2b5b..303effda1a2e 100644
--- a/drivers/media/i2c/adv7842.c
+++ b/drivers/media/i2c/adv7842.c
@@ -2250,7 +2250,7 @@ static void adv7842_cec_isr(struct v4l2_subdev *sd, bool *handled)
 
 static int adv7842_cec_adap_enable(struct cec_adapter *adap, bool enable)
 {
-        struct adv7842_state *state = adap->priv;
+        struct adv7842_state *state = cec_get_drvdata(adap);
         struct v4l2_subdev *sd = &state->sd;
 
         if (!state->cec_enabled_adap && enable) {
@@ -2279,7 +2279,7 @@ static int adv7842_cec_adap_enable(struct cec_adapter *adap, bool enable)
 
 static int adv7842_cec_adap_log_addr(struct cec_adapter *adap, u8 addr)
 {
-        struct adv7842_state *state = adap->priv;
+        struct adv7842_state *state = cec_get_drvdata(adap);
         struct v4l2_subdev *sd = &state->sd;
         unsigned int i, free_idx = ADV7842_MAX_ADDRS;
 
@@ -2334,7 +2334,7 @@ static int adv7842_cec_adap_log_addr(struct cec_adapter *adap, u8 addr)
 static int adv7842_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
                                      u32 signal_free_time, struct cec_msg *msg)
 {
-        struct adv7842_state *state = adap->priv;
+        struct adv7842_state *state = cec_get_drvdata(adap);
         struct v4l2_subdev *sd = &state->sd;
         u8 len = msg->len;
         unsigned int i;
diff --git a/drivers/media/i2c/et8ek8/et8ek8_driver.c b/drivers/media/i2c/et8ek8/et8ek8_driver.c
index bec4a563a09c..6e313d5243a0 100644
--- a/drivers/media/i2c/et8ek8/et8ek8_driver.c
+++ b/drivers/media/i2c/et8ek8/et8ek8_driver.c
@@ -1485,6 +1485,7 @@ static const struct of_device_id et8ek8_of_table[] = {
         { .compatible = "toshiba,et8ek8" },
         { },
 };
+MODULE_DEVICE_TABLE(of, et8ek8_of_table);
 
 static const struct i2c_device_id et8ek8_id_table[] = {
         { ET8EK8_NAME, 0 },
@@ -1495,6 +1496,7 @@ MODULE_DEVICE_TABLE(i2c, et8ek8_id_table);
 static const struct dev_pm_ops et8ek8_pm_ops = {
         SET_SYSTEM_SLEEP_PM_OPS(et8ek8_suspend, et8ek8_resume)
 };
+MODULE_DEVICE_TABLE(of, et8ek8_of_table);
 
 static struct i2c_driver et8ek8_i2c_driver = {
         .driver         = {
diff --git a/drivers/media/i2c/soc_camera/ov2640.c b/drivers/media/i2c/ov2640.c
index 56de18263359..e6d0c1f64f0b 100644
--- a/drivers/media/i2c/soc_camera/ov2640.c
+++ b/drivers/media/i2c/ov2640.c
@@ -16,6 +16,7 @@
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/i2c.h>
+#include <linux/clk.h>
 #include <linux/slab.h>
 #include <linux/delay.h>
 #include <linux/gpio.h>
@@ -24,8 +25,7 @@
 #include <linux/v4l2-mediabus.h>
 #include <linux/videodev2.h>
 
-#include <media/soc_camera.h>
-#include <media/v4l2-clk.h>
+#include <media/v4l2-device.h>
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-image-sizes.h>
@@ -106,6 +106,10 @@
 #define   CTRL1_AWB_GAIN     0x04
 #define   CTRL1_LENC         0x02
 #define   CTRL1_PRE          0x01
+/*      REG 0xC7 (unknown name): affects Auto White Balance (AWB)
+ *        AWB_OFF            0x40
+ *        AWB_SIMPLE         0x10
+ *        AWB_ON             0x00       (Advanced AWB ?) */
 #define R_DVP_SP    0xD3 /* DVP output speed control */
 #define   R_DVP_SP_AUTO_MODE 0x80
 #define   R_DVP_SP_DVP_MASK  0x3F /* DVP PCLK = sysclk (48)/[6:0] (YUV0);
@@ -199,7 +203,7 @@
 #define   COM7_ZOOM_EN         0x04 /* Enable Zoom mode */
 #define   COM7_COLOR_BAR_TEST  0x02 /* Enable Color Bar Test Pattern */
 #define COM8        0x13 /* Common control 8 */
-#define   COM8_DEF             0xC0 /* Banding filter ON/OFF */
+#define   COM8_DEF             0xC0
 #define   COM8_BNDF_EN         0x20 /* Banding filter ON/OFF */
 #define   COM8_AGC_EN          0x04 /* AGC Auto/Manual control selection */
 #define   COM8_AEC_EN          0x01 /* Auto/Manual Exposure control */
@@ -248,8 +252,19 @@
 #define ZOOMS       0x49 /* Zoom: Vertical start point */
 #define COM22       0x4B /* Flash light control */
 #define COM25       0x4E /* For Banding operations */
+#define   COM25_50HZ_BANDING_AEC_MSBS_MASK      0xC0 /* 50Hz Bd. AEC 2 MSBs */
+#define   COM25_60HZ_BANDING_AEC_MSBS_MASK      0x30 /* 60Hz Bd. AEC 2 MSBs */
+#define   COM25_50HZ_BANDING_AEC_MSBS_SET(x)    VAL_SET(x, 0x3, 8, 6)
+#define   COM25_60HZ_BANDING_AEC_MSBS_SET(x)    VAL_SET(x, 0x3, 8, 4)
 #define BD50        0x4F /* 50Hz Banding AEC 8 LSBs */
+#define   BD50_50HZ_BANDING_AEC_LSBS_SET(x)     VAL_SET(x, 0xFF, 0, 0)
 #define BD60        0x50 /* 60Hz Banding AEC 8 LSBs */
+#define   BD60_60HZ_BANDING_AEC_LSBS_SET(x)     VAL_SET(x, 0xFF, 0, 0)
+#define REG5A       0x5A /* 50/60Hz Banding Maximum AEC Step */
+#define   BD50_MAX_AEC_STEP_MASK         0xF0 /* 50Hz Banding Max. AEC Step */
+#define   BD60_MAX_AEC_STEP_MASK         0x0F /* 60Hz Banding Max. AEC Step */
+#define   BD50_MAX_AEC_STEP_SET(x)       VAL_SET((x - 1), 0x0F, 0, 4)
+#define   BD60_MAX_AEC_STEP_SET(x)       VAL_SET((x - 1), 0x0F, 0, 0)
 #define REG5D       0x5D /* AVGsel[7:0],   16-zone average weight option */
 #define REG5E       0x5E /* AVGsel[15:8],  16-zone average weight option */
 #define REG5F       0x5F /* AVGsel[23:16], 16-zone average weight option */
@@ -282,12 +297,14 @@ struct ov2640_win_size {
 
 struct ov2640_priv {
         struct v4l2_subdev              subdev;
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        struct media_pad pad;
+#endif
         struct v4l2_ctrl_handler        hdl;
         u32     cfmt_code;
-        struct v4l2_clk                 *clk;
+        struct clk                      *clk;
         const struct ov2640_win_size    *win;
 
-        struct soc_camera_subdev_desc   ssdd_dt;
         struct gpio_desc *resetb_gpio;
         struct gpio_desc *pwdn_gpio;
 };
@@ -304,11 +321,11 @@ static const struct regval_list ov2640_init_regs[] = {
         { 0x2e,   0xdf },
         { BANK_SEL, BANK_SEL_SENS },
         { 0x3c,   0x32 },
-        { CLKRC, CLKRC_DIV_SET(1) },
-        { COM2, COM2_OCAP_Nx_SET(3) },
-        { REG04, REG04_DEF | REG04_HREF_EN },
-        { COM8,  COM8_DEF | COM8_BNDF_EN | COM8_AGC_EN | COM8_AEC_EN },
-        { COM9, COM9_AGC_GAIN_8x | 0x08},
+        { CLKRC,  CLKRC_DIV_SET(1) },
+        { COM2,   COM2_OCAP_Nx_SET(3) },
+        { REG04,  REG04_DEF | REG04_HREF_EN },
+        { COM8,   COM8_DEF | COM8_BNDF_EN | COM8_AGC_EN | COM8_AEC_EN },
+        { COM9,   COM9_AGC_GAIN_8x | 0x08},
         { 0x2c,   0x0c },
         { 0x33,   0x78 },
         { 0x3a,   0x33 },
@@ -353,25 +370,28 @@ static const struct regval_list ov2640_init_regs[] = {
         { 0x71,   0x94 },
         { 0x73,   0xc1 },
         { 0x3d,   0x34 },
-        { COM7, COM7_RES_UXGA | COM7_ZOOM_EN },
-        { 0x5a,   0x57 },
-        { BD50,   0xbb },
-        { BD60,   0x9c },
-        { BANK_SEL, BANK_SEL_DSP },
+        { COM7,   COM7_RES_UXGA | COM7_ZOOM_EN },
+        { REG5A,  BD50_MAX_AEC_STEP_SET(6)
+                   | BD60_MAX_AEC_STEP_SET(8) },                /* 0x57 */
+        { COM25,  COM25_50HZ_BANDING_AEC_MSBS_SET(0x0bb)
+                   | COM25_60HZ_BANDING_AEC_MSBS_SET(0x09c) },  /* 0x00 */
+        { BD50,   BD50_50HZ_BANDING_AEC_LSBS_SET(0x0bb) },      /* 0xbb */
+        { BD60,   BD60_60HZ_BANDING_AEC_LSBS_SET(0x09c) },      /* 0x9c */
+        { BANK_SEL,  BANK_SEL_DSP },
         { 0xe5,   0x7f },
-        { MC_BIST, MC_BIST_RESET | MC_BIST_BOOT_ROM_SEL },
+        { MC_BIST,  MC_BIST_RESET | MC_BIST_BOOT_ROM_SEL },
         { 0x41,   0x24 },
-        { RESET, RESET_JPEG | RESET_DVP },
+        { RESET,  RESET_JPEG | RESET_DVP },
         { 0x76,   0xff },
         { 0x33,   0xa0 },
         { 0x42,   0x20 },
         { 0x43,   0x18 },
         { 0x4c,   0x00 },
-        { CTRL3, CTRL3_BPC_EN | CTRL3_WPC_EN | 0x10 },
+        { CTRL3,  CTRL3_BPC_EN | CTRL3_WPC_EN | 0x10 },
         { 0x88,   0x3f },
         { 0xd7,   0x03 },
         { 0xd9,   0x10 },
-        { R_DVP_SP , R_DVP_SP_AUTO_MODE | 0x2 },
+        { R_DVP_SP,  R_DVP_SP_AUTO_MODE | 0x2 },
         { 0xc8,   0x08 },
         { 0xc9,   0x80 },
         { BPADDR, 0x00 },
@@ -433,7 +453,7 @@ static const struct regval_list ov2640_init_regs[] = {
         { 0xc5,   0x11 },
         { 0xc6,   0x51 },
         { 0xbf,   0x80 },
-        { 0xc7,   0x10 },
+        { 0xc7,   0x10 },       /* simple AWB */
         { 0xb6,   0x66 },
         { 0xb8,   0xA5 },
         { 0xb7,   0x64 },
@@ -480,6 +500,9 @@ static const struct regval_list ov2640_init_regs[] = {
 static const struct regval_list ov2640_size_change_preamble_regs[] = {
         { BANK_SEL, BANK_SEL_DSP },
         { RESET, RESET_DVP },
+        { SIZEL, SIZEL_HSIZE8_11_SET(UXGA_WIDTH) |
+                 SIZEL_HSIZE8_SET(UXGA_WIDTH) |
+                 SIZEL_VSIZE8_SET(UXGA_HEIGHT) },
         { HSIZE8, HSIZE8_SET(UXGA_WIDTH) },
         { VSIZE8, VSIZE8_SET(UXGA_HEIGHT) },
         { CTRL2, CTRL2_DCW_EN | CTRL2_SDE_EN |
@@ -611,6 +634,8 @@ static const struct regval_list ov2640_rgb565_le_regs[] = {
 static u32 ov2640_codes[] = {
         MEDIA_BUS_FMT_YUYV8_2X8,
         MEDIA_BUS_FMT_UYVY8_2X8,
+        MEDIA_BUS_FMT_YVYU8_2X8,
+        MEDIA_BUS_FMT_VYUY8_2X8,
         MEDIA_BUS_FMT_RGB565_2X8_BE,
         MEDIA_BUS_FMT_RGB565_2X8_LE,
 };
@@ -677,13 +702,8 @@ err:
 }
 
 /*
- * soc_camera_ops functions
+ * functions
  */
-static int ov2640_s_stream(struct v4l2_subdev *sd, int enable)
-{
-        return 0;
-}
-
 static int ov2640_s_ctrl(struct v4l2_ctrl *ctrl)
 {
         struct v4l2_subdev *sd =
@@ -698,8 +718,10 @@ static int ov2640_s_ctrl(struct v4l2_ctrl *ctrl)
 
         switch (ctrl->id) {
         case V4L2_CID_VFLIP:
-                val = ctrl->val ? REG04_VFLIP_IMG : 0x00;
-                return ov2640_mask_set(client, REG04, REG04_VFLIP_IMG, val);
+                val = ctrl->val ? REG04_VFLIP_IMG | REG04_VREF_EN : 0x00;
+                return ov2640_mask_set(client, REG04,
+                                       REG04_VFLIP_IMG | REG04_VREF_EN, val);
+                /* NOTE: REG04_VREF_EN: 1 line shift / even/odd line swap */
         case V4L2_CID_HFLIP:
                 val = ctrl->val ? REG04_HFLIP_IMG : 0x00;
                 return ov2640_mask_set(client, REG04, REG04_HFLIP_IMG, val);
@@ -743,41 +765,46 @@ static int ov2640_s_register(struct v4l2_subdev *sd,
 
 static int ov2640_s_power(struct v4l2_subdev *sd, int on)
 {
+#ifdef CONFIG_GPIOLIB
         struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct soc_camera_subdev_desc *ssdd = soc_camera_i2c_to_desc(client);
         struct ov2640_priv *priv = to_ov2640(client);
 
-        return soc_camera_set_power(&client->dev, ssdd, priv->clk, on);
+        if (priv->pwdn_gpio)
+                gpiod_direction_output(priv->pwdn_gpio, !on);
+        if (on && priv->resetb_gpio) {
+                /* Active the resetb pin to perform a reset pulse */
+                gpiod_direction_output(priv->resetb_gpio, 1);
+                usleep_range(3000, 5000);
+                gpiod_set_value(priv->resetb_gpio, 0);
+        }
+#endif
+        return 0;
 }
 
 /* Select the nearest higher resolution for capture */
-static const struct ov2640_win_size *ov2640_select_win(u32 *width, u32 *height)
+static const struct ov2640_win_size *ov2640_select_win(u32 width, u32 height)
 {
         int i, default_size = ARRAY_SIZE(ov2640_supported_win_sizes) - 1;
 
         for (i = 0; i < ARRAY_SIZE(ov2640_supported_win_sizes); i++) {
-                if (ov2640_supported_win_sizes[i].width  >= *width &&
-                    ov2640_supported_win_sizes[i].height >= *height) {
-                        *width = ov2640_supported_win_sizes[i].width;
-                        *height = ov2640_supported_win_sizes[i].height;
+                if (ov2640_supported_win_sizes[i].width  >= width &&
+                    ov2640_supported_win_sizes[i].height >= height)
                         return &ov2640_supported_win_sizes[i];
-                }
         }
 
-        *width = ov2640_supported_win_sizes[default_size].width;
-        *height = ov2640_supported_win_sizes[default_size].height;
         return &ov2640_supported_win_sizes[default_size];
 }
 
-static int ov2640_set_params(struct i2c_client *client, u32 *width, u32 *height,
-                             u32 code)
+static int ov2640_set_params(struct i2c_client *client,
+                             const struct ov2640_win_size *win, u32 code)
 {
         struct ov2640_priv       *priv = to_ov2640(client);
         const struct regval_list *selected_cfmt_regs;
+        u8 val;
         int ret;
 
         /* select win */
-        priv->win = ov2640_select_win(width, height);
+        priv->win = win;
 
         /* select format */
         priv->cfmt_code = 0;
@@ -794,10 +821,19 @@ static int ov2640_set_params(struct i2c_client *client, u32 *width, u32 *height,
                 dev_dbg(&client->dev, "%s: Selected cfmt YUYV (YUV422)", __func__);
                 selected_cfmt_regs = ov2640_yuyv_regs;
                 break;
-        default:
         case MEDIA_BUS_FMT_UYVY8_2X8:
+        default:
                 dev_dbg(&client->dev, "%s: Selected cfmt UYVY", __func__);
                 selected_cfmt_regs = ov2640_uyvy_regs;
+                break;
+        case MEDIA_BUS_FMT_YVYU8_2X8:
+                dev_dbg(&client->dev, "%s: Selected cfmt YVYU", __func__);
+                selected_cfmt_regs = ov2640_yuyv_regs;
+                break;
+        case MEDIA_BUS_FMT_VYUY8_2X8:
+                dev_dbg(&client->dev, "%s: Selected cfmt VYUY", __func__);
+                selected_cfmt_regs = ov2640_uyvy_regs;
+                break;
         }
 
         /* reset hardware */
@@ -830,10 +866,13 @@ static int ov2640_set_params(struct i2c_client *client, u32 *width, u32 *height,
         ret = ov2640_write_array(client, selected_cfmt_regs);
         if (ret < 0)
                 goto err;
+        val = (code == MEDIA_BUS_FMT_YVYU8_2X8)
+              || (code == MEDIA_BUS_FMT_VYUY8_2X8) ? CTRL0_VFIRST : 0x00;
+        ret = ov2640_mask_set(client, CTRL0, CTRL0_VFIRST, val);
+        if (ret < 0)
+                goto err;
 
         priv->cfmt_code = code;
-        *width = priv->win->width;
-        *height = priv->win->height;
 
         return 0;
 
@@ -857,25 +896,14 @@ static int ov2640_get_fmt(struct v4l2_subdev *sd,
                 return -EINVAL;
 
         if (!priv->win) {
-                u32 width = SVGA_WIDTH, height = SVGA_HEIGHT;
-                priv->win = ov2640_select_win(&width, &height);
+                priv->win = ov2640_select_win(SVGA_WIDTH, SVGA_HEIGHT);
                 priv->cfmt_code = MEDIA_BUS_FMT_UYVY8_2X8;
         }
 
         mf->width       = priv->win->width;
         mf->height      = priv->win->height;
         mf->code        = priv->cfmt_code;
-
-        switch (mf->code) {
-        case MEDIA_BUS_FMT_RGB565_2X8_BE:
-        case MEDIA_BUS_FMT_RGB565_2X8_LE:
-                mf->colorspace = V4L2_COLORSPACE_SRGB;
-                break;
-        default:
-        case MEDIA_BUS_FMT_YUYV8_2X8:
-        case MEDIA_BUS_FMT_UYVY8_2X8:
-                mf->colorspace = V4L2_COLORSPACE_JPEG;
-        }
+        mf->colorspace  = V4L2_COLORSPACE_SRGB;
         mf->field       = V4L2_FIELD_NONE;
 
         return 0;
@@ -887,32 +915,34 @@ static int ov2640_set_fmt(struct v4l2_subdev *sd,
 {
         struct v4l2_mbus_framefmt *mf = &format->format;
         struct i2c_client *client = v4l2_get_subdevdata(sd);
+        const struct ov2640_win_size *win;
 
         if (format->pad)
                 return -EINVAL;
 
-        /*
-         * select suitable win, but don't store it
-         */
-        ov2640_select_win(&mf->width, &mf->height);
+        /* select suitable win */
+        win = ov2640_select_win(mf->width, mf->height);
+        mf->width       = win->width;
+        mf->height      = win->height;
 
         mf->field       = V4L2_FIELD_NONE;
+        mf->colorspace  = V4L2_COLORSPACE_SRGB;
 
         switch (mf->code) {
         case MEDIA_BUS_FMT_RGB565_2X8_BE:
         case MEDIA_BUS_FMT_RGB565_2X8_LE:
-                mf->colorspace = V4L2_COLORSPACE_SRGB;
+        case MEDIA_BUS_FMT_YUYV8_2X8:
+        case MEDIA_BUS_FMT_UYVY8_2X8:
+        case MEDIA_BUS_FMT_YVYU8_2X8:
+        case MEDIA_BUS_FMT_VYUY8_2X8:
                 break;
         default:
                 mf->code = MEDIA_BUS_FMT_UYVY8_2X8;
-        case MEDIA_BUS_FMT_YUYV8_2X8:
-        case MEDIA_BUS_FMT_UYVY8_2X8:
-                mf->colorspace = V4L2_COLORSPACE_JPEG;
+                break;
         }
 
         if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
-                return ov2640_set_params(client, &mf->width,
-                                         &mf->height, mf->code);
+                return ov2640_set_params(client, win, mf->code);
         cfg->try_fmt = *mf;
         return 0;
 }
@@ -995,7 +1025,7 @@ static const struct v4l2_ctrl_ops ov2640_ctrl_ops = {
         .s_ctrl = ov2640_s_ctrl,
 };
 
-static struct v4l2_subdev_core_ops ov2640_subdev_core_ops = {
+static const struct v4l2_subdev_core_ops ov2640_subdev_core_ops = {
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register     = ov2640_g_register,
         .s_register     = ov2640_s_register,
@@ -1003,26 +1033,6 @@ static struct v4l2_subdev_core_ops ov2640_subdev_core_ops = {
         .s_power        = ov2640_s_power,
 };
 
-static int ov2640_g_mbus_config(struct v4l2_subdev *sd,
-                                struct v4l2_mbus_config *cfg)
-{
-        struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct soc_camera_subdev_desc *ssdd = soc_camera_i2c_to_desc(client);
-
-        cfg->flags = V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_MASTER |
-                V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_HIGH |
-                V4L2_MBUS_DATA_ACTIVE_HIGH;
-        cfg->type = V4L2_MBUS_PARALLEL;
-        cfg->flags = soc_camera_apply_board_flags(ssdd, cfg);
-
-        return 0;
-}
-
-static struct v4l2_subdev_video_ops ov2640_subdev_video_ops = {
-        .s_stream       = ov2640_s_stream,
-        .g_mbus_config  = ov2640_g_mbus_config,
-};
-
 static const struct v4l2_subdev_pad_ops ov2640_subdev_pad_ops = {
         .enum_mbus_code = ov2640_enum_mbus_code,
         .get_selection  = ov2640_get_selection,
@@ -1030,65 +1040,43 @@ static const struct v4l2_subdev_pad_ops ov2640_subdev_pad_ops = {
         .set_fmt        = ov2640_set_fmt,
 };
 
-static struct v4l2_subdev_ops ov2640_subdev_ops = {
+static const struct v4l2_subdev_ops ov2640_subdev_ops = {
         .core   = &ov2640_subdev_core_ops,
-        .video  = &ov2640_subdev_video_ops,
         .pad    = &ov2640_subdev_pad_ops,
 };
 
-/* OF probe functions */
-static int ov2640_hw_power(struct device *dev, int on)
-{
-        struct i2c_client *client = to_i2c_client(dev);
-        struct ov2640_priv *priv = to_ov2640(client);
-
-        dev_dbg(&client->dev, "%s: %s the camera\n",
-                        __func__, on ? "ENABLE" : "DISABLE");
-
-        if (priv->pwdn_gpio)
-                gpiod_direction_output(priv->pwdn_gpio, !on);
-
-        return 0;
-}
-
-static int ov2640_hw_reset(struct device *dev)
-{
-        struct i2c_client *client = to_i2c_client(dev);
-        struct ov2640_priv *priv = to_ov2640(client);
-
-        if (priv->resetb_gpio) {
-                /* Active the resetb pin to perform a reset pulse */
-                gpiod_direction_output(priv->resetb_gpio, 1);
-                usleep_range(3000, 5000);
-                gpiod_direction_output(priv->resetb_gpio, 0);
-        }
-
-        return 0;
-}
-
 static int ov2640_probe_dt(struct i2c_client *client,
                 struct ov2640_priv *priv)
 {
+        int ret;
+
         /* Request the reset GPIO deasserted */
         priv->resetb_gpio = devm_gpiod_get_optional(&client->dev, "resetb",
                         GPIOD_OUT_LOW);
+
         if (!priv->resetb_gpio)
                 dev_dbg(&client->dev, "resetb gpio is not assigned!\n");
-        else if (IS_ERR(priv->resetb_gpio))
-                return PTR_ERR(priv->resetb_gpio);
+
+        ret = PTR_ERR_OR_ZERO(priv->resetb_gpio);
+        if (ret && ret != -ENOSYS) {
+                dev_dbg(&client->dev,
+                        "Error %d while getting resetb gpio\n", ret);
+                return ret;
+        }
 
         /* Request the power down GPIO asserted */
         priv->pwdn_gpio = devm_gpiod_get_optional(&client->dev, "pwdn",
                         GPIOD_OUT_HIGH);
+
         if (!priv->pwdn_gpio)
                 dev_dbg(&client->dev, "pwdn gpio is not assigned!\n");
-        else if (IS_ERR(priv->pwdn_gpio))
-                return PTR_ERR(priv->pwdn_gpio);
 
-        /* Initialize the soc_camera_subdev_desc */
-        priv->ssdd_dt.power = ov2640_hw_power;
-        priv->ssdd_dt.reset = ov2640_hw_reset;
-        client->dev.platform_data = &priv->ssdd_dt;
+        ret = PTR_ERR_OR_ZERO(priv->pwdn_gpio);
+        if (ret && ret != -ENOSYS) {
+                dev_dbg(&client->dev,
+                        "Error %d while getting pwdn gpio\n", ret);
+                return ret;
+        }
 
         return 0;
 }
@@ -1100,7 +1088,6 @@ static int ov2640_probe(struct i2c_client *client,
                         const struct i2c_device_id *did)
 {
         struct ov2640_priv      *priv;
-        struct soc_camera_subdev_desc *ssdd = soc_camera_i2c_to_desc(client);
         struct i2c_adapter      *adapter = to_i2c_adapter(client->dev.parent);
         int                     ret;
 
@@ -1117,23 +1104,19 @@ static int ov2640_probe(struct i2c_client *client,
                 return -ENOMEM;
         }
 
-        priv->clk = v4l2_clk_get(&client->dev, "xvclk");
-        if (IS_ERR(priv->clk))
-                return -EPROBE_DEFER;
-
-        if (!ssdd && !client->dev.of_node) {
-                dev_err(&client->dev, "Missing platform_data for driver\n");
-                ret = -EINVAL;
-                goto err_clk;
+        if (client->dev.of_node) {
+                priv->clk = devm_clk_get(&client->dev, "xvclk");
+                if (IS_ERR(priv->clk))
+                        return -EPROBE_DEFER;
+                clk_prepare_enable(priv->clk);
         }
 
-        if (!ssdd) {
-                ret = ov2640_probe_dt(client, priv);
-                if (ret)
-                        goto err_clk;
-        }
+        ret = ov2640_probe_dt(client, priv);
+        if (ret)
+                goto err_clk;
 
         v4l2_i2c_subdev_init(&priv->subdev, client, &ov2640_subdev_ops);
+        priv->subdev.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
         v4l2_ctrl_handler_init(&priv->hdl, 2);
         v4l2_ctrl_new_std(&priv->hdl, &ov2640_ctrl_ops,
                         V4L2_CID_VFLIP, 0, 1, 1, 0);
@@ -1142,8 +1125,15 @@ static int ov2640_probe(struct i2c_client *client,
         priv->subdev.ctrl_handler = &priv->hdl;
         if (priv->hdl.error) {
                 ret = priv->hdl.error;
-                goto err_clk;
+                goto err_hdl;
         }
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        priv->pad.flags = MEDIA_PAD_FL_SOURCE;
+        priv->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+        ret = media_entity_pads_init(&priv->subdev.entity, 1, &priv->pad);
+        if (ret < 0)
+                goto err_hdl;
+#endif
 
         ret = ov2640_video_probe(client);
         if (ret < 0)
@@ -1158,9 +1148,13 @@ static int ov2640_probe(struct i2c_client *client,
         return 0;
 
 err_videoprobe:
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        media_entity_cleanup(&priv->subdev.entity);
+#endif
+err_hdl:
         v4l2_ctrl_handler_free(&priv->hdl);
 err_clk:
-        v4l2_clk_put(priv->clk);
+        clk_disable_unprepare(priv->clk);
         return ret;
 }
 
@@ -1169,9 +1163,12 @@ static int ov2640_remove(struct i2c_client *client)
         struct ov2640_priv       *priv = to_ov2640(client);
 
         v4l2_async_unregister_subdev(&priv->subdev);
-        v4l2_clk_put(priv->clk);
-        v4l2_device_unregister_subdev(&priv->subdev);
         v4l2_ctrl_handler_free(&priv->hdl);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        media_entity_cleanup(&priv->subdev.entity);
+#endif
+        v4l2_device_unregister_subdev(&priv->subdev);
+        clk_disable_unprepare(priv->clk);
         return 0;
 }
 
@@ -1199,6 +1196,6 @@ static struct i2c_driver ov2640_i2c_driver = {
 
 module_i2c_driver(ov2640_i2c_driver);
 
-MODULE_DESCRIPTION("SoC Camera driver for Omni Vision 2640 sensor");
+MODULE_DESCRIPTION("Driver for Omni Vision 2640 sensor");
 MODULE_AUTHOR("Alberto Panizzo");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov5645.c b/drivers/media/i2c/ov5645.c
new file mode 100644
index 000000000000..57bd591ea54b
--- /dev/null
+++ b/drivers/media/i2c/ov5645.c
@@ -0,0 +1,1345 @@
+/*
+ * Driver for the OV5645 camera sensor.
+ *
+ * Copyright (c) 2011-2015, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2015 By Tech Design S.L. All Rights Reserved.
+ * Copyright (C) 2012-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * Based on:
+ * - the OV5645 driver from QC msm-3.10 kernel on codeaurora.org:
+ *   https://us.codeaurora.org/cgit/quic/la/kernel/msm-3.10/tree/drivers/
+ *       media/platform/msm/camera_v2/sensor/ov5645.c?h=LA.BR.1.2.4_rb1.41
+ * - the OV5640 driver posted on linux-media:
+ *   https://www.mail-archive.com/linux-media%40vger.kernel.org/msg92671.html
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/bitops.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-of.h>
+#include <media/v4l2-subdev.h>
+
+#define OV5645_VOLTAGE_ANALOG               2800000
+#define OV5645_VOLTAGE_DIGITAL_CORE         1500000
+#define OV5645_VOLTAGE_DIGITAL_IO           1800000
+
+#define OV5645_SYSTEM_CTRL0             0x3008
+#define         OV5645_SYSTEM_CTRL0_START       0x02
+#define         OV5645_SYSTEM_CTRL0_STOP        0x42
+#define OV5645_CHIP_ID_HIGH             0x300a
+#define         OV5645_CHIP_ID_HIGH_BYTE        0x56
+#define OV5645_CHIP_ID_LOW              0x300b
+#define         OV5645_CHIP_ID_LOW_BYTE         0x45
+#define OV5645_AWB_MANUAL_CONTROL       0x3406
+#define         OV5645_AWB_MANUAL_ENABLE        BIT(0)
+#define OV5645_AEC_PK_MANUAL            0x3503
+#define         OV5645_AEC_MANUAL_ENABLE        BIT(0)
+#define         OV5645_AGC_MANUAL_ENABLE        BIT(1)
+#define OV5645_TIMING_TC_REG20          0x3820
+#define         OV5645_SENSOR_VFLIP             BIT(1)
+#define         OV5645_ISP_VFLIP                BIT(2)
+#define OV5645_TIMING_TC_REG21          0x3821
+#define         OV5645_SENSOR_MIRROR            BIT(1)
+#define OV5645_PRE_ISP_TEST_SETTING_1   0x503d
+#define         OV5645_TEST_PATTERN_MASK        0x3
+#define         OV5645_SET_TEST_PATTERN(x)      ((x) & OV5645_TEST_PATTERN_MASK)
+#define         OV5645_TEST_PATTERN_ENABLE      BIT(7)
+#define OV5645_SDE_SAT_U                0x5583
+#define OV5645_SDE_SAT_V                0x5584
+
+struct reg_value {
+        u16 reg;
+        u8 val;
+};
+
+struct ov5645_mode_info {
+        u32 width;
+        u32 height;
+        const struct reg_value *data;
+        u32 data_size;
+};
+
+struct ov5645 {
+        struct i2c_client *i2c_client;
+        struct device *dev;
+        struct v4l2_subdev sd;
+        struct media_pad pad;
+        struct v4l2_of_endpoint ep;
+        struct v4l2_mbus_framefmt fmt;
+        struct v4l2_rect crop;
+        struct clk *xclk;
+
+        struct regulator *io_regulator;
+        struct regulator *core_regulator;
+        struct regulator *analog_regulator;
+
+        const struct ov5645_mode_info *current_mode;
+
+        struct v4l2_ctrl_handler ctrls;
+
+        /* Cached register values */
+        u8 aec_pk_manual;
+        u8 timing_tc_reg20;
+        u8 timing_tc_reg21;
+
+        struct mutex power_lock; /* lock to protect power state */
+        int power_count;
+
+        struct gpio_desc *enable_gpio;
+        struct gpio_desc *rst_gpio;
+};
+
+static inline struct ov5645 *to_ov5645(struct v4l2_subdev *sd)
+{
+        return container_of(sd, struct ov5645, sd);
+}
+
+static const struct reg_value ov5645_global_init_setting[] = {
+        { 0x3103, 0x11 },
+        { 0x3008, 0x82 },
+        { 0x3008, 0x42 },
+        { 0x3103, 0x03 },
+        { 0x3503, 0x07 },
+        { 0x3002, 0x1c },
+        { 0x3006, 0xc3 },
+        { 0x300e, 0x45 },
+        { 0x3017, 0x00 },
+        { 0x3018, 0x00 },
+        { 0x302e, 0x0b },
+        { 0x3037, 0x13 },
+        { 0x3108, 0x01 },
+        { 0x3611, 0x06 },
+        { 0x3500, 0x00 },
+        { 0x3501, 0x01 },
+        { 0x3502, 0x00 },
+        { 0x350a, 0x00 },
+        { 0x350b, 0x3f },
+        { 0x3620, 0x33 },
+        { 0x3621, 0xe0 },
+        { 0x3622, 0x01 },
+        { 0x3630, 0x2e },
+        { 0x3631, 0x00 },
+        { 0x3632, 0x32 },
+        { 0x3633, 0x52 },
+        { 0x3634, 0x70 },
+        { 0x3635, 0x13 },
+        { 0x3636, 0x03 },
+        { 0x3703, 0x5a },
+        { 0x3704, 0xa0 },
+        { 0x3705, 0x1a },
+        { 0x3709, 0x12 },
+        { 0x370b, 0x61 },
+        { 0x370f, 0x10 },
+        { 0x3715, 0x78 },
+        { 0x3717, 0x01 },
+        { 0x371b, 0x20 },
+        { 0x3731, 0x12 },
+        { 0x3901, 0x0a },
+        { 0x3905, 0x02 },
+        { 0x3906, 0x10 },
+        { 0x3719, 0x86 },
+        { 0x3810, 0x00 },
+        { 0x3811, 0x10 },
+        { 0x3812, 0x00 },
+        { 0x3821, 0x01 },
+        { 0x3824, 0x01 },
+        { 0x3826, 0x03 },
+        { 0x3828, 0x08 },
+        { 0x3a19, 0xf8 },
+        { 0x3c01, 0x34 },
+        { 0x3c04, 0x28 },
+        { 0x3c05, 0x98 },
+        { 0x3c07, 0x07 },
+        { 0x3c09, 0xc2 },
+        { 0x3c0a, 0x9c },
+        { 0x3c0b, 0x40 },
+        { 0x3c01, 0x34 },
+        { 0x4001, 0x02 },
+        { 0x4514, 0x00 },
+        { 0x4520, 0xb0 },
+        { 0x460b, 0x37 },
+        { 0x460c, 0x20 },
+        { 0x4818, 0x01 },
+        { 0x481d, 0xf0 },
+        { 0x481f, 0x50 },
+        { 0x4823, 0x70 },
+        { 0x4831, 0x14 },
+        { 0x5000, 0xa7 },
+        { 0x5001, 0x83 },
+        { 0x501d, 0x00 },
+        { 0x501f, 0x00 },
+        { 0x503d, 0x00 },
+        { 0x505c, 0x30 },
+        { 0x5181, 0x59 },
+        { 0x5183, 0x00 },
+        { 0x5191, 0xf0 },
+        { 0x5192, 0x03 },
+        { 0x5684, 0x10 },
+        { 0x5685, 0xa0 },
+        { 0x5686, 0x0c },
+        { 0x5687, 0x78 },
+        { 0x5a00, 0x08 },
+        { 0x5a21, 0x00 },
+        { 0x5a24, 0x00 },
+        { 0x3008, 0x02 },
+        { 0x3503, 0x00 },
+        { 0x5180, 0xff },
+        { 0x5181, 0xf2 },
+        { 0x5182, 0x00 },
+        { 0x5183, 0x14 },
+        { 0x5184, 0x25 },
+        { 0x5185, 0x24 },
+        { 0x5186, 0x09 },
+        { 0x5187, 0x09 },
+        { 0x5188, 0x0a },
+        { 0x5189, 0x75 },
+        { 0x518a, 0x52 },
+        { 0x518b, 0xea },
+        { 0x518c, 0xa8 },
+        { 0x518d, 0x42 },
+        { 0x518e, 0x38 },
+        { 0x518f, 0x56 },
+        { 0x5190, 0x42 },
+        { 0x5191, 0xf8 },
+        { 0x5192, 0x04 },
+        { 0x5193, 0x70 },
+        { 0x5194, 0xf0 },
+        { 0x5195, 0xf0 },
+        { 0x5196, 0x03 },
+        { 0x5197, 0x01 },
+        { 0x5198, 0x04 },
+        { 0x5199, 0x12 },
+        { 0x519a, 0x04 },
+        { 0x519b, 0x00 },
+        { 0x519c, 0x06 },
+        { 0x519d, 0x82 },
+        { 0x519e, 0x38 },
+        { 0x5381, 0x1e },
+        { 0x5382, 0x5b },
+        { 0x5383, 0x08 },
+        { 0x5384, 0x0a },
+        { 0x5385, 0x7e },
+        { 0x5386, 0x88 },
+        { 0x5387, 0x7c },
+        { 0x5388, 0x6c },
+        { 0x5389, 0x10 },
+        { 0x538a, 0x01 },
+        { 0x538b, 0x98 },
+        { 0x5300, 0x08 },
+        { 0x5301, 0x30 },
+        { 0x5302, 0x10 },
+        { 0x5303, 0x00 },
+        { 0x5304, 0x08 },
+        { 0x5305, 0x30 },
+        { 0x5306, 0x08 },
+        { 0x5307, 0x16 },
+        { 0x5309, 0x08 },
+        { 0x530a, 0x30 },
+        { 0x530b, 0x04 },
+        { 0x530c, 0x06 },
+        { 0x5480, 0x01 },
+        { 0x5481, 0x08 },
+        { 0x5482, 0x14 },
+        { 0x5483, 0x28 },
+        { 0x5484, 0x51 },
+        { 0x5485, 0x65 },
+        { 0x5486, 0x71 },
+        { 0x5487, 0x7d },
+        { 0x5488, 0x87 },
+        { 0x5489, 0x91 },
+        { 0x548a, 0x9a },
+        { 0x548b, 0xaa },
+        { 0x548c, 0xb8 },
+        { 0x548d, 0xcd },
+        { 0x548e, 0xdd },
+        { 0x548f, 0xea },
+        { 0x5490, 0x1d },
+        { 0x5580, 0x02 },
+        { 0x5583, 0x40 },
+        { 0x5584, 0x10 },
+        { 0x5589, 0x10 },
+        { 0x558a, 0x00 },
+        { 0x558b, 0xf8 },
+        { 0x5800, 0x3f },
+        { 0x5801, 0x16 },
+        { 0x5802, 0x0e },
+        { 0x5803, 0x0d },
+        { 0x5804, 0x17 },
+        { 0x5805, 0x3f },
+        { 0x5806, 0x0b },
+        { 0x5807, 0x06 },
+        { 0x5808, 0x04 },
+        { 0x5809, 0x04 },
+        { 0x580a, 0x06 },
+        { 0x580b, 0x0b },
+        { 0x580c, 0x09 },
+        { 0x580d, 0x03 },
+        { 0x580e, 0x00 },
+        { 0x580f, 0x00 },
+        { 0x5810, 0x03 },
+        { 0x5811, 0x08 },
+        { 0x5812, 0x0a },
+        { 0x5813, 0x03 },
+        { 0x5814, 0x00 },
+        { 0x5815, 0x00 },
+        { 0x5816, 0x04 },
+        { 0x5817, 0x09 },
+        { 0x5818, 0x0f },
+        { 0x5819, 0x08 },
+        { 0x581a, 0x06 },
+        { 0x581b, 0x06 },
+        { 0x581c, 0x08 },
+        { 0x581d, 0x0c },
+        { 0x581e, 0x3f },
+        { 0x581f, 0x1e },
+        { 0x5820, 0x12 },
+        { 0x5821, 0x13 },
+        { 0x5822, 0x21 },
+        { 0x5823, 0x3f },
+        { 0x5824, 0x68 },
+        { 0x5825, 0x28 },
+        { 0x5826, 0x2c },
+        { 0x5827, 0x28 },
+        { 0x5828, 0x08 },
+        { 0x5829, 0x48 },
+        { 0x582a, 0x64 },
+        { 0x582b, 0x62 },
+        { 0x582c, 0x64 },
+        { 0x582d, 0x28 },
+        { 0x582e, 0x46 },
+        { 0x582f, 0x62 },
+        { 0x5830, 0x60 },
+        { 0x5831, 0x62 },
+        { 0x5832, 0x26 },
+        { 0x5833, 0x48 },
+        { 0x5834, 0x66 },
+        { 0x5835, 0x44 },
+        { 0x5836, 0x64 },
+        { 0x5837, 0x28 },
+        { 0x5838, 0x66 },
+        { 0x5839, 0x48 },
+        { 0x583a, 0x2c },
+        { 0x583b, 0x28 },
+        { 0x583c, 0x26 },
+        { 0x583d, 0xae },
+        { 0x5025, 0x00 },
+        { 0x3a0f, 0x30 },
+        { 0x3a10, 0x28 },
+        { 0x3a1b, 0x30 },
+        { 0x3a1e, 0x26 },
+        { 0x3a11, 0x60 },
+        { 0x3a1f, 0x14 },
+        { 0x0601, 0x02 },
+        { 0x3008, 0x42 },
+        { 0x3008, 0x02 }
+};
+
+static const struct reg_value ov5645_setting_sxga[] = {
+        { 0x3612, 0xa9 },
+        { 0x3614, 0x50 },
+        { 0x3618, 0x00 },
+        { 0x3034, 0x18 },
+        { 0x3035, 0x21 },
+        { 0x3036, 0x70 },
+        { 0x3600, 0x09 },
+        { 0x3601, 0x43 },
+        { 0x3708, 0x66 },
+        { 0x370c, 0xc3 },
+        { 0x3800, 0x00 },
+        { 0x3801, 0x00 },
+        { 0x3802, 0x00 },
+        { 0x3803, 0x06 },
+        { 0x3804, 0x0a },
+        { 0x3805, 0x3f },
+        { 0x3806, 0x07 },
+        { 0x3807, 0x9d },
+        { 0x3808, 0x05 },
+        { 0x3809, 0x00 },
+        { 0x380a, 0x03 },
+        { 0x380b, 0xc0 },
+        { 0x380c, 0x07 },
+        { 0x380d, 0x68 },
+        { 0x380e, 0x03 },
+        { 0x380f, 0xd8 },
+        { 0x3813, 0x06 },
+        { 0x3814, 0x31 },
+        { 0x3815, 0x31 },
+        { 0x3820, 0x47 },
+        { 0x3a02, 0x03 },
+        { 0x3a03, 0xd8 },
+        { 0x3a08, 0x01 },
+        { 0x3a09, 0xf8 },
+        { 0x3a0a, 0x01 },
+        { 0x3a0b, 0xa4 },
+        { 0x3a0e, 0x02 },
+        { 0x3a0d, 0x02 },
+        { 0x3a14, 0x03 },
+        { 0x3a15, 0xd8 },
+        { 0x3a18, 0x00 },
+        { 0x4004, 0x02 },
+        { 0x4005, 0x18 },
+        { 0x4300, 0x32 },
+        { 0x4202, 0x00 }
+};
+
+static const struct reg_value ov5645_setting_1080p[] = {
+        { 0x3612, 0xab },
+        { 0x3614, 0x50 },
+        { 0x3618, 0x04 },
+        { 0x3034, 0x18 },
+        { 0x3035, 0x11 },
+        { 0x3036, 0x54 },
+        { 0x3600, 0x08 },
+        { 0x3601, 0x33 },
+        { 0x3708, 0x63 },
+        { 0x370c, 0xc0 },
+        { 0x3800, 0x01 },
+        { 0x3801, 0x50 },
+        { 0x3802, 0x01 },
+        { 0x3803, 0xb2 },
+        { 0x3804, 0x08 },
+        { 0x3805, 0xef },
+        { 0x3806, 0x05 },
+        { 0x3807, 0xf1 },
+        { 0x3808, 0x07 },
+        { 0x3809, 0x80 },
+        { 0x380a, 0x04 },
+        { 0x380b, 0x38 },
+        { 0x380c, 0x09 },
+        { 0x380d, 0xc4 },
+        { 0x380e, 0x04 },
+        { 0x380f, 0x60 },
+        { 0x3813, 0x04 },
+        { 0x3814, 0x11 },
+        { 0x3815, 0x11 },
+        { 0x3820, 0x47 },
+        { 0x4514, 0x88 },
+        { 0x3a02, 0x04 },
+        { 0x3a03, 0x60 },
+        { 0x3a08, 0x01 },
+        { 0x3a09, 0x50 },
+        { 0x3a0a, 0x01 },
+        { 0x3a0b, 0x18 },
+        { 0x3a0e, 0x03 },
+        { 0x3a0d, 0x04 },
+        { 0x3a14, 0x04 },
+        { 0x3a15, 0x60 },
+        { 0x3a18, 0x00 },
+        { 0x4004, 0x06 },
+        { 0x4005, 0x18 },
+        { 0x4300, 0x32 },
+        { 0x4202, 0x00 },
+        { 0x4837, 0x0b }
+};
+
+static const struct reg_value ov5645_setting_full[] = {
+        { 0x3612, 0xab },
+        { 0x3614, 0x50 },
+        { 0x3618, 0x04 },
+        { 0x3034, 0x18 },
+        { 0x3035, 0x11 },
+        { 0x3036, 0x54 },
+        { 0x3600, 0x08 },
+        { 0x3601, 0x33 },
+        { 0x3708, 0x63 },
+        { 0x370c, 0xc0 },
+        { 0x3800, 0x00 },
+        { 0x3801, 0x00 },
+        { 0x3802, 0x00 },
+        { 0x3803, 0x00 },
+        { 0x3804, 0x0a },
+        { 0x3805, 0x3f },
+        { 0x3806, 0x07 },
+        { 0x3807, 0x9f },
+        { 0x3808, 0x0a },
+        { 0x3809, 0x20 },
+        { 0x380a, 0x07 },
+        { 0x380b, 0x98 },
+        { 0x380c, 0x0b },
+        { 0x380d, 0x1c },
+        { 0x380e, 0x07 },
+        { 0x380f, 0xb0 },
+        { 0x3813, 0x06 },
+        { 0x3814, 0x11 },
+        { 0x3815, 0x11 },
+        { 0x3820, 0x47 },
+        { 0x4514, 0x88 },
+        { 0x3a02, 0x07 },
+        { 0x3a03, 0xb0 },
+        { 0x3a08, 0x01 },
+        { 0x3a09, 0x27 },
+        { 0x3a0a, 0x00 },
+        { 0x3a0b, 0xf6 },
+        { 0x3a0e, 0x06 },
+        { 0x3a0d, 0x08 },
+        { 0x3a14, 0x07 },
+        { 0x3a15, 0xb0 },
+        { 0x3a18, 0x01 },
+        { 0x4004, 0x06 },
+        { 0x4005, 0x18 },
+        { 0x4300, 0x32 },
+        { 0x4837, 0x0b },
+        { 0x4202, 0x00 }
+};
+
+static const struct ov5645_mode_info ov5645_mode_info_data[] = {
+        {
+                .width = 1280,
+                .height = 960,
+                .data = ov5645_setting_sxga,
+                .data_size = ARRAY_SIZE(ov5645_setting_sxga)
+        },
+        {
+                .width = 1920,
+                .height = 1080,
+                .data = ov5645_setting_1080p,
+                .data_size = ARRAY_SIZE(ov5645_setting_1080p)
+        },
+        {
+                .width = 2592,
+                .height = 1944,
+                .data = ov5645_setting_full,
+                .data_size = ARRAY_SIZE(ov5645_setting_full)
+        },
+};
+
+static int ov5645_regulators_enable(struct ov5645 *ov5645)
+{
+        int ret;
+
+        ret = regulator_enable(ov5645->io_regulator);
+        if (ret < 0) {
+                dev_err(ov5645->dev, "set io voltage failed\n");
+                return ret;
+        }
+
+        ret = regulator_enable(ov5645->analog_regulator);
+        if (ret) {
+                dev_err(ov5645->dev, "set analog voltage failed\n");
+                goto err_disable_io;
+        }
+
+        ret = regulator_enable(ov5645->core_regulator);
+        if (ret) {
+                dev_err(ov5645->dev, "set core voltage failed\n");
+                goto err_disable_analog;
+        }
+
+        return 0;
+
+err_disable_analog:
+        regulator_disable(ov5645->analog_regulator);
+err_disable_io:
+        regulator_disable(ov5645->io_regulator);
+
+        return ret;
+}
+
+static void ov5645_regulators_disable(struct ov5645 *ov5645)
+{
+        int ret;
+
+        ret = regulator_disable(ov5645->core_regulator);
+        if (ret < 0)
+                dev_err(ov5645->dev, "core regulator disable failed\n");
+
+        ret = regulator_disable(ov5645->analog_regulator);
+        if (ret < 0)
+                dev_err(ov5645->dev, "analog regulator disable failed\n");
+
+        ret = regulator_disable(ov5645->io_regulator);
+        if (ret < 0)
+                dev_err(ov5645->dev, "io regulator disable failed\n");
+}
+
+static int ov5645_write_reg(struct ov5645 *ov5645, u16 reg, u8 val)
+{
+        u8 regbuf[3];
+        int ret;
+
+        regbuf[0] = reg >> 8;
+        regbuf[1] = reg & 0xff;
+        regbuf[2] = val;
+
+        ret = i2c_master_send(ov5645->i2c_client, regbuf, 3);
+        if (ret < 0)
+                dev_err(ov5645->dev, "%s: write reg error %d: reg=%x, val=%x\n",
+                        __func__, ret, reg, val);
+
+        return ret;
+}
+
+static int ov5645_read_reg(struct ov5645 *ov5645, u16 reg, u8 *val)
+{
+        u8 regbuf[2];
+        int ret;
+
+        regbuf[0] = reg >> 8;
+        regbuf[1] = reg & 0xff;
+
+        ret = i2c_master_send(ov5645->i2c_client, regbuf, 2);
+        if (ret < 0) {
+                dev_err(ov5645->dev, "%s: write reg error %d: reg=%x\n",
+                        __func__, ret, reg);
+                return ret;
+        }
+
+        ret = i2c_master_recv(ov5645->i2c_client, val, 1);
+        if (ret < 0) {
+                dev_err(ov5645->dev, "%s: read reg error %d: reg=%x\n",
+                        __func__, ret, reg);
+                return ret;
+        }
+
+        return 0;
+}
+
+static int ov5645_set_aec_mode(struct ov5645 *ov5645, u32 mode)
+{
+        u8 val = ov5645->aec_pk_manual;
+        int ret;
+
+        if (mode == V4L2_EXPOSURE_AUTO)
+                val &= ~OV5645_AEC_MANUAL_ENABLE;
+        else /* V4L2_EXPOSURE_MANUAL */
+                val |= OV5645_AEC_MANUAL_ENABLE;
+
+        ret = ov5645_write_reg(ov5645, OV5645_AEC_PK_MANUAL, val);
+        if (!ret)
+                ov5645->aec_pk_manual = val;
+
+        return ret;
+}
+
+static int ov5645_set_agc_mode(struct ov5645 *ov5645, u32 enable)
+{
+        u8 val = ov5645->aec_pk_manual;
+        int ret;
+
+        if (enable)
+                val &= ~OV5645_AGC_MANUAL_ENABLE;
+        else
+                val |= OV5645_AGC_MANUAL_ENABLE;
+
+        ret = ov5645_write_reg(ov5645, OV5645_AEC_PK_MANUAL, val);
+        if (!ret)
+                ov5645->aec_pk_manual = val;
+
+        return ret;
+}
+
+static int ov5645_set_register_array(struct ov5645 *ov5645,
+                                     const struct reg_value *settings,
+                                     unsigned int num_settings)
+{
+        unsigned int i;
+        int ret;
+
+        for (i = 0; i < num_settings; ++i, ++settings) {
+                ret = ov5645_write_reg(ov5645, settings->reg, settings->val);
+                if (ret < 0)
+                        return ret;
+        }
+
+        return 0;
+}
+
+static int ov5645_set_power_on(struct ov5645 *ov5645)
+{
+        int ret;
+
+        ret = ov5645_regulators_enable(ov5645);
+        if (ret < 0) {
+                return ret;
+        }
+
+        ret = clk_prepare_enable(ov5645->xclk);
+        if (ret < 0) {
+                dev_err(ov5645->dev, "clk prepare enable failed\n");
+                ov5645_regulators_disable(ov5645);
+                return ret;
+        }
+
+        usleep_range(5000, 15000);
+        gpiod_set_value_cansleep(ov5645->enable_gpio, 1);
+
+        usleep_range(1000, 2000);
+        gpiod_set_value_cansleep(ov5645->rst_gpio, 0);
+
+        msleep(20);
+
+        return 0;
+}
+
+static void ov5645_set_power_off(struct ov5645 *ov5645)
+{
+        gpiod_set_value_cansleep(ov5645->rst_gpio, 1);
+        gpiod_set_value_cansleep(ov5645->enable_gpio, 0);
+        clk_disable_unprepare(ov5645->xclk);
+        ov5645_regulators_disable(ov5645);
+}
+
+static int ov5645_s_power(struct v4l2_subdev *sd, int on)
+{
+        struct ov5645 *ov5645 = to_ov5645(sd);
+        int ret = 0;
+
+        mutex_lock(&ov5645->power_lock);
+
+        /* If the power count is modified from 0 to != 0 or from != 0 to 0,
+         * update the power state.
+         */
+        if (ov5645->power_count == !on) {
+                if (on) {
+                        ret = ov5645_set_power_on(ov5645);
+                        if (ret < 0)
+                                goto exit;
+
+                        ret = ov5645_set_register_array(ov5645,
+                                        ov5645_global_init_setting,
+                                        ARRAY_SIZE(ov5645_global_init_setting));
+                        if (ret < 0) {
+                                dev_err(ov5645->dev,
+                                        "could not set init registers\n");
+                                ov5645_set_power_off(ov5645);
+                                goto exit;
+                        }
+
+                        ret = ov5645_write_reg(ov5645, OV5645_SYSTEM_CTRL0,
+                                               OV5645_SYSTEM_CTRL0_STOP);
+                        if (ret < 0) {
+                                ov5645_set_power_off(ov5645);
+                                goto exit;
+                        }
+                } else {
+                        ov5645_set_power_off(ov5645);
+                }
+        }
+
+        /* Update the power count. */
+        ov5645->power_count += on ? 1 : -1;
+        WARN_ON(ov5645->power_count < 0);
+
+exit:
+        mutex_unlock(&ov5645->power_lock);
+
+        return ret;
+}
+
+static int ov5645_set_saturation(struct ov5645 *ov5645, s32 value)
+{
+        u32 reg_value = (value * 0x10) + 0x40;
+        int ret;
+
+        ret = ov5645_write_reg(ov5645, OV5645_SDE_SAT_U, reg_value);
+        if (ret < 0)
+                return ret;
+
+        return ov5645_write_reg(ov5645, OV5645_SDE_SAT_V, reg_value);
+}
+
+static int ov5645_set_hflip(struct ov5645 *ov5645, s32 value)
+{
+        u8 val = ov5645->timing_tc_reg21;
+        int ret;
+
+        if (value == 0)
+                val &= ~(OV5645_SENSOR_MIRROR);
+        else
+                val |= (OV5645_SENSOR_MIRROR);
+
+        ret = ov5645_write_reg(ov5645, OV5645_TIMING_TC_REG21, val);
+        if (!ret)
+                ov5645->timing_tc_reg21 = val;
+
+        return ret;
+}
+
+static int ov5645_set_vflip(struct ov5645 *ov5645, s32 value)
+{
+        u8 val = ov5645->timing_tc_reg20;
+        int ret;
+
+        if (value == 0)
+                val |= (OV5645_SENSOR_VFLIP | OV5645_ISP_VFLIP);
+        else
+                val &= ~(OV5645_SENSOR_VFLIP | OV5645_ISP_VFLIP);
+
+        ret = ov5645_write_reg(ov5645, OV5645_TIMING_TC_REG20, val);
+        if (!ret)
+                ov5645->timing_tc_reg20 = val;
+
+        return ret;
+}
+
+static int ov5645_set_test_pattern(struct ov5645 *ov5645, s32 value)
+{
+        u8 val = 0;
+
+        if (value) {
+                val = OV5645_SET_TEST_PATTERN(value - 1);
+                val |= OV5645_TEST_PATTERN_ENABLE;
+        }
+
+        return ov5645_write_reg(ov5645, OV5645_PRE_ISP_TEST_SETTING_1, val);
+}
+
+static const char * const ov5645_test_pattern_menu[] = {
+        "Disabled",
+        "Vertical Color Bars",
+        "Pseudo-Random Data",
+        "Color Square",
+        "Black Image",
+};
+
+static int ov5645_set_awb(struct ov5645 *ov5645, s32 enable_auto)
+{
+        u8 val = 0;
+
+        if (!enable_auto)
+                val = OV5645_AWB_MANUAL_ENABLE;
+
+        return ov5645_write_reg(ov5645, OV5645_AWB_MANUAL_CONTROL, val);
+}
+
+static int ov5645_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct ov5645 *ov5645 = container_of(ctrl->handler,
+                                             struct ov5645, ctrls);
+        int ret;
+
+        mutex_lock(&ov5645->power_lock);
+        if (!ov5645->power_count) {
+                mutex_unlock(&ov5645->power_lock);
+                return 0;
+        }
+
+        switch (ctrl->id) {
+        case V4L2_CID_SATURATION:
+                ret = ov5645_set_saturation(ov5645, ctrl->val);
+                break;
+        case V4L2_CID_AUTO_WHITE_BALANCE:
+                ret = ov5645_set_awb(ov5645, ctrl->val);
+                break;
+        case V4L2_CID_AUTOGAIN:
+                ret = ov5645_set_agc_mode(ov5645, ctrl->val);
+                break;
+        case V4L2_CID_EXPOSURE_AUTO:
+                ret = ov5645_set_aec_mode(ov5645, ctrl->val);
+                break;
+        case V4L2_CID_TEST_PATTERN:
+                ret = ov5645_set_test_pattern(ov5645, ctrl->val);
+                break;
+        case V4L2_CID_HFLIP:
+                ret = ov5645_set_hflip(ov5645, ctrl->val);
+                break;
+        case V4L2_CID_VFLIP:
+                ret = ov5645_set_vflip(ov5645, ctrl->val);
+                break;
+        default:
+                ret = -EINVAL;
+                break;
+        }
+
+        mutex_unlock(&ov5645->power_lock);
+
+        return ret;
+}
+
+static struct v4l2_ctrl_ops ov5645_ctrl_ops = {
+        .s_ctrl = ov5645_s_ctrl,
+};
+
+static int ov5645_enum_mbus_code(struct v4l2_subdev *sd,
+                                 struct v4l2_subdev_pad_config *cfg,
+                                 struct v4l2_subdev_mbus_code_enum *code)
+{
+        if (code->index > 0)
+                return -EINVAL;
+
+        code->code = MEDIA_BUS_FMT_UYVY8_2X8;
+
+        return 0;
+}
+
+static int ov5645_enum_frame_size(struct v4l2_subdev *subdev,
+                                  struct v4l2_subdev_pad_config *cfg,
+                                  struct v4l2_subdev_frame_size_enum *fse)
+{
+        if (fse->code != MEDIA_BUS_FMT_UYVY8_2X8)
+                return -EINVAL;
+
+        if (fse->index >= ARRAY_SIZE(ov5645_mode_info_data))
+                return -EINVAL;
+
+        fse->min_width = ov5645_mode_info_data[fse->index].width;
+        fse->max_width = ov5645_mode_info_data[fse->index].width;
+        fse->min_height = ov5645_mode_info_data[fse->index].height;
+        fse->max_height = ov5645_mode_info_data[fse->index].height;
+
+        return 0;
+}
+
+static struct v4l2_mbus_framefmt *
+__ov5645_get_pad_format(struct ov5645 *ov5645,
+                        struct v4l2_subdev_pad_config *cfg,
+                        unsigned int pad,
+                        enum v4l2_subdev_format_whence which)
+{
+        switch (which) {
+        case V4L2_SUBDEV_FORMAT_TRY:
+                return v4l2_subdev_get_try_format(&ov5645->sd, cfg, pad);
+        case V4L2_SUBDEV_FORMAT_ACTIVE:
+                return &ov5645->fmt;
+        default:
+                return NULL;
+        }
+}
+
+static int ov5645_get_format(struct v4l2_subdev *sd,
+                             struct v4l2_subdev_pad_config *cfg,
+                             struct v4l2_subdev_format *format)
+{
+        struct ov5645 *ov5645 = to_ov5645(sd);
+
+        format->format = *__ov5645_get_pad_format(ov5645, cfg, format->pad,
+                                                  format->which);
+        return 0;
+}
+
+static struct v4l2_rect *
+__ov5645_get_pad_crop(struct ov5645 *ov5645, struct v4l2_subdev_pad_config *cfg,
+                      unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+        switch (which) {
+        case V4L2_SUBDEV_FORMAT_TRY:
+                return v4l2_subdev_get_try_crop(&ov5645->sd, cfg, pad);
+        case V4L2_SUBDEV_FORMAT_ACTIVE:
+                return &ov5645->crop;
+        default:
+                return NULL;
+        }
+}
+
+static const struct ov5645_mode_info *
+ov5645_find_nearest_mode(unsigned int width, unsigned int height)
+{
+        int i;
+
+        for (i = ARRAY_SIZE(ov5645_mode_info_data) - 1; i >= 0; i--) {
+                if (ov5645_mode_info_data[i].width <= width &&
+                    ov5645_mode_info_data[i].height <= height)
+                        break;
+        }
+
+        if (i < 0)
+                i = 0;
+
+        return &ov5645_mode_info_data[i];
+}
+
+static int ov5645_set_format(struct v4l2_subdev *sd,
+                             struct v4l2_subdev_pad_config *cfg,
+                             struct v4l2_subdev_format *format)
+{
+        struct ov5645 *ov5645 = to_ov5645(sd);
+        struct v4l2_mbus_framefmt *__format;
+        struct v4l2_rect *__crop;
+        const struct ov5645_mode_info *new_mode;
+
+        __crop = __ov5645_get_pad_crop(ov5645, cfg, format->pad,
+                        format->which);
+
+        new_mode = ov5645_find_nearest_mode(format->format.width,
+                                            format->format.height);
+        __crop->width = new_mode->width;
+        __crop->height = new_mode->height;
+
+        if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+                ov5645->current_mode = new_mode;
+
+        __format = __ov5645_get_pad_format(ov5645, cfg, format->pad,
+                        format->which);
+        __format->width = __crop->width;
+        __format->height = __crop->height;
+        __format->code = MEDIA_BUS_FMT_UYVY8_2X8;
+        __format->field = V4L2_FIELD_NONE;
+        __format->colorspace = V4L2_COLORSPACE_SRGB;
+
+        format->format = *__format;
+
+        return 0;
+}
+
+static int ov5645_entity_init_cfg(struct v4l2_subdev *subdev,
+                                  struct v4l2_subdev_pad_config *cfg)
+{
+        struct v4l2_subdev_format fmt = { 0 };
+
+        fmt.which = cfg ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+        fmt.format.width = 1920;
+        fmt.format.height = 1080;
+
+        ov5645_set_format(subdev, cfg, &fmt);
+
+        return 0;
+}
+
+static int ov5645_get_selection(struct v4l2_subdev *sd,
+                           struct v4l2_subdev_pad_config *cfg,
+                           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, cfg, sel->pad,
+                                        sel->which);
+        return 0;
+}
+
+static int ov5645_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+        struct ov5645 *ov5645 = to_ov5645(subdev);
+        int ret;
+
+        if (enable) {
+                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);
+                        return ret;
+                }
+                ret = v4l2_ctrl_handler_setup(&ov5645->ctrls);
+                if (ret < 0) {
+                        dev_err(ov5645->dev, "could not sync v4l2 controls\n");
+                        return ret;
+                }
+                ret = ov5645_write_reg(ov5645, OV5645_SYSTEM_CTRL0,
+                                       OV5645_SYSTEM_CTRL0_START);
+                if (ret < 0)
+                        return ret;
+        } else {
+                ret = ov5645_write_reg(ov5645, OV5645_SYSTEM_CTRL0,
+                                       OV5645_SYSTEM_CTRL0_STOP);
+                if (ret < 0)
+                        return ret;
+        }
+
+        return 0;
+}
+
+static const struct v4l2_subdev_core_ops ov5645_core_ops = {
+        .s_power = ov5645_s_power,
+};
+
+static const struct v4l2_subdev_video_ops ov5645_video_ops = {
+        .s_stream = ov5645_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov5645_subdev_pad_ops = {
+        .init_cfg = ov5645_entity_init_cfg,
+        .enum_mbus_code = ov5645_enum_mbus_code,
+        .enum_frame_size = ov5645_enum_frame_size,
+        .get_fmt = ov5645_get_format,
+        .set_fmt = ov5645_set_format,
+        .get_selection = ov5645_get_selection,
+};
+
+static const struct v4l2_subdev_ops ov5645_subdev_ops = {
+        .core = &ov5645_core_ops,
+        .video = &ov5645_video_ops,
+        .pad = &ov5645_subdev_pad_ops,
+};
+
+static int ov5645_probe(struct i2c_client *client,
+                        const struct i2c_device_id *id)
+{
+        struct device *dev = &client->dev;
+        struct device_node *endpoint;
+        struct ov5645 *ov5645;
+        u8 chip_id_high, chip_id_low;
+        u32 xclk_freq;
+        int ret;
+
+        ov5645 = devm_kzalloc(dev, sizeof(struct ov5645), GFP_KERNEL);
+        if (!ov5645)
+                return -ENOMEM;
+
+        ov5645->i2c_client = client;
+        ov5645->dev = dev;
+
+        endpoint = of_graph_get_next_endpoint(dev->of_node, NULL);
+        if (!endpoint) {
+                dev_err(dev, "endpoint node not found\n");
+                return -EINVAL;
+        }
+
+        ret = v4l2_of_parse_endpoint(endpoint, &ov5645->ep);
+        if (ret < 0) {
+                dev_err(dev, "parsing endpoint node failed\n");
+                return ret;
+        }
+
+        of_node_put(endpoint);
+
+        if (ov5645->ep.bus_type != V4L2_MBUS_CSI2) {
+                dev_err(dev, "invalid bus type, must be CSI2\n");
+                return -EINVAL;
+        }
+
+        /* get system clock (xclk) */
+        ov5645->xclk = devm_clk_get(dev, "xclk");
+        if (IS_ERR(ov5645->xclk)) {
+                dev_err(dev, "could not get xclk");
+                return PTR_ERR(ov5645->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 (xclk_freq != 23880000) {
+                dev_err(dev, "external clock frequency %u is not supported\n",
+                        xclk_freq);
+                return -EINVAL;
+        }
+
+        ret = clk_set_rate(ov5645->xclk, xclk_freq);
+        if (ret) {
+                dev_err(dev, "could not set xclk frequency\n");
+                return ret;
+        }
+
+        ov5645->io_regulator = devm_regulator_get(dev, "vdddo");
+        if (IS_ERR(ov5645->io_regulator)) {
+                dev_err(dev, "cannot get io regulator\n");
+                return PTR_ERR(ov5645->io_regulator);
+        }
+
+        ret = regulator_set_voltage(ov5645->io_regulator,
+                                    OV5645_VOLTAGE_DIGITAL_IO,
+                                    OV5645_VOLTAGE_DIGITAL_IO);
+        if (ret < 0) {
+                dev_err(dev, "cannot set io voltage\n");
+                return ret;
+        }
+
+        ov5645->core_regulator = devm_regulator_get(dev, "vddd");
+        if (IS_ERR(ov5645->core_regulator)) {
+                dev_err(dev, "cannot get core regulator\n");
+                return PTR_ERR(ov5645->core_regulator);
+        }
+
+        ret = regulator_set_voltage(ov5645->core_regulator,
+                                    OV5645_VOLTAGE_DIGITAL_CORE,
+                                    OV5645_VOLTAGE_DIGITAL_CORE);
+        if (ret < 0) {
+                dev_err(dev, "cannot set core voltage\n");
+                return ret;
+        }
+
+        ov5645->analog_regulator = devm_regulator_get(dev, "vdda");
+        if (IS_ERR(ov5645->analog_regulator)) {
+                dev_err(dev, "cannot get analog regulator\n");
+                return PTR_ERR(ov5645->analog_regulator);
+        }
+
+        ret = regulator_set_voltage(ov5645->analog_regulator,
+                                    OV5645_VOLTAGE_ANALOG,
+                                    OV5645_VOLTAGE_ANALOG);
+        if (ret < 0) {
+                dev_err(dev, "cannot set analog voltage\n");
+                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);
+        }
+
+        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);
+
+        v4l2_ctrl_handler_init(&ov5645->ctrls, 7);
+        v4l2_ctrl_new_std(&ov5645->ctrls, &ov5645_ctrl_ops,
+                          V4L2_CID_SATURATION, -4, 4, 1, 0);
+        v4l2_ctrl_new_std(&ov5645->ctrls, &ov5645_ctrl_ops,
+                          V4L2_CID_HFLIP, 0, 1, 1, 0);
+        v4l2_ctrl_new_std(&ov5645->ctrls, &ov5645_ctrl_ops,
+                          V4L2_CID_VFLIP, 0, 1, 1, 0);
+        v4l2_ctrl_new_std(&ov5645->ctrls, &ov5645_ctrl_ops,
+                          V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+        v4l2_ctrl_new_std(&ov5645->ctrls, &ov5645_ctrl_ops,
+                          V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
+        v4l2_ctrl_new_std_menu(&ov5645->ctrls, &ov5645_ctrl_ops,
+                               V4L2_CID_EXPOSURE_AUTO, V4L2_EXPOSURE_MANUAL,
+                               0, V4L2_EXPOSURE_AUTO);
+        v4l2_ctrl_new_std_menu_items(&ov5645->ctrls, &ov5645_ctrl_ops,
+                                     V4L2_CID_TEST_PATTERN,
+                                     ARRAY_SIZE(ov5645_test_pattern_menu) - 1,
+                                     0, 0, ov5645_test_pattern_menu);
+
+        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;
+                goto free_ctrl;
+        }
+
+        v4l2_i2c_subdev_init(&ov5645->sd, client, &ov5645_subdev_ops);
+        ov5645->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+        ov5645->pad.flags = MEDIA_PAD_FL_SOURCE;
+        ov5645->sd.dev = &client->dev;
+
+        ret = media_entity_pads_init(&ov5645->sd.entity, 1, &ov5645->pad);
+        if (ret < 0) {
+                dev_err(dev, "could not register media entity\n");
+                goto free_ctrl;
+        }
+
+        ret = ov5645_s_power(&ov5645->sd, true);
+        if (ret < 0) {
+                dev_err(dev, "could not power up OV5645\n");
+                goto free_entity;
+        }
+
+        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;
+                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;
+                goto power_down;
+        }
+
+        dev_info(dev, "OV5645 detected at address 0x%02x\n", client->addr);
+
+        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;
+                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;
+                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;
+                goto power_down;
+        }
+
+        ov5645_s_power(&ov5645->sd, false);
+
+        ret = v4l2_async_register_subdev(&ov5645->sd);
+        if (ret < 0) {
+                dev_err(dev, "could not register v4l2 device\n");
+                goto free_entity;
+        }
+
+        ov5645_entity_init_cfg(&ov5645->sd, NULL);
+
+        return 0;
+
+power_down:
+        ov5645_s_power(&ov5645->sd, false);
+free_entity:
+        media_entity_cleanup(&ov5645->sd.entity);
+free_ctrl:
+        v4l2_ctrl_handler_free(&ov5645->ctrls);
+        mutex_destroy(&ov5645->power_lock);
+
+        return ret;
+}
+
+static int ov5645_remove(struct i2c_client *client)
+{
+        struct v4l2_subdev *sd = i2c_get_clientdata(client);
+        struct ov5645 *ov5645 = to_ov5645(sd);
+
+        v4l2_async_unregister_subdev(&ov5645->sd);
+        media_entity_cleanup(&ov5645->sd.entity);
+        v4l2_ctrl_handler_free(&ov5645->ctrls);
+        mutex_destroy(&ov5645->power_lock);
+
+        return 0;
+}
+
+static const struct i2c_device_id ov5645_id[] = {
+        { "ov5645", 0 },
+        {}
+};
+MODULE_DEVICE_TABLE(i2c, ov5645_id);
+
+static const struct of_device_id ov5645_of_match[] = {
+        { .compatible = "ovti,ov5645" },
+        { /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ov5645_of_match);
+
+static struct i2c_driver ov5645_i2c_driver = {
+        .driver = {
+                .of_match_table = of_match_ptr(ov5645_of_match),
+                .name  = "ov5645",
+        },
+        .probe  = ov5645_probe,
+        .remove = ov5645_remove,
+        .id_table = ov5645_id,
+};
+
+module_i2c_driver(ov5645_i2c_driver);
+
+MODULE_DESCRIPTION("Omnivision OV5645 Camera Driver");
+MODULE_AUTHOR("Todor Tomov <todor.tomov@linaro.org>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov5647.c b/drivers/media/i2c/ov5647.c
new file mode 100644
index 000000000000..f57a0b354cf6
--- /dev/null
+++ b/drivers/media/i2c/ov5647.c
@@ -0,0 +1,634 @@
+/*
+ * A V4L2 driver for OmniVision OV5647 cameras.
+ *
+ * Based on Samsung S5K6AAFX SXGA 1/6" 1.3M CMOS Image Sensor driver
+ * Copyright (C) 2011 Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * Based on Omnivision OV7670 Camera Driver
+ * Copyright (C) 2006-7 Jonathan Corbet <corbet@lwn.net>
+ *
+ * Copyright (C) 2016, Synopsys, Inc.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed .as is. WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-image-sizes.h>
+#include <media/v4l2-mediabus.h>
+#include <media/v4l2-of.h>
+
+#define SENSOR_NAME "ov5647"
+
+#define OV5647_SW_RESET         0x0103
+#define OV5647_REG_CHIPID_H     0x300A
+#define OV5647_REG_CHIPID_L     0x300B
+
+#define REG_TERM 0xfffe
+#define VAL_TERM 0xfe
+#define REG_DLY  0xffff
+
+#define OV5647_ROW_START                0x01
+#define OV5647_ROW_START_MIN            0
+#define OV5647_ROW_START_MAX            2004
+#define OV5647_ROW_START_DEF            54
+
+#define OV5647_COLUMN_START             0x02
+#define OV5647_COLUMN_START_MIN         0
+#define OV5647_COLUMN_START_MAX         2750
+#define OV5647_COLUMN_START_DEF         16
+
+#define OV5647_WINDOW_HEIGHT            0x03
+#define OV5647_WINDOW_HEIGHT_MIN        2
+#define OV5647_WINDOW_HEIGHT_MAX        2006
+#define OV5647_WINDOW_HEIGHT_DEF        1944
+
+#define OV5647_WINDOW_WIDTH             0x04
+#define OV5647_WINDOW_WIDTH_MIN         2
+#define OV5647_WINDOW_WIDTH_MAX         2752
+#define OV5647_WINDOW_WIDTH_DEF         2592
+
+struct regval_list {
+        u16 addr;
+        u8 data;
+};
+
+struct ov5647 {
+        struct v4l2_subdev              sd;
+        struct media_pad                pad;
+        struct mutex                    lock;
+        struct v4l2_mbus_framefmt       format;
+        unsigned int                    width;
+        unsigned int                    height;
+        int                             power_count;
+        struct clk                      *xclk;
+};
+
+static inline struct ov5647 *to_state(struct v4l2_subdev *sd)
+{
+        return container_of(sd, struct ov5647, sd);
+}
+
+static struct regval_list sensor_oe_disable_regs[] = {
+        {0x3000, 0x00},
+        {0x3001, 0x00},
+        {0x3002, 0x00},
+};
+
+static struct regval_list sensor_oe_enable_regs[] = {
+        {0x3000, 0x0f},
+        {0x3001, 0xff},
+        {0x3002, 0xe4},
+};
+
+static struct regval_list ov5647_640x480[] = {
+        {0x0100, 0x00},
+        {0x0103, 0x01},
+        {0x3034, 0x08},
+        {0x3035, 0x21},
+        {0x3036, 0x46},
+        {0x303c, 0x11},
+        {0x3106, 0xf5},
+        {0x3821, 0x07},
+        {0x3820, 0x41},
+        {0x3827, 0xec},
+        {0x370c, 0x0f},
+        {0x3612, 0x59},
+        {0x3618, 0x00},
+        {0x5000, 0x06},
+        {0x5001, 0x01},
+        {0x5002, 0x41},
+        {0x5003, 0x08},
+        {0x5a00, 0x08},
+        {0x3000, 0x00},
+        {0x3001, 0x00},
+        {0x3002, 0x00},
+        {0x3016, 0x08},
+        {0x3017, 0xe0},
+        {0x3018, 0x44},
+        {0x301c, 0xf8},
+        {0x301d, 0xf0},
+        {0x3a18, 0x00},
+        {0x3a19, 0xf8},
+        {0x3c01, 0x80},
+        {0x3b07, 0x0c},
+        {0x380c, 0x07},
+        {0x380d, 0x68},
+        {0x380e, 0x03},
+        {0x380f, 0xd8},
+        {0x3814, 0x31},
+        {0x3815, 0x31},
+        {0x3708, 0x64},
+        {0x3709, 0x52},
+        {0x3808, 0x02},
+        {0x3809, 0x80},
+        {0x380a, 0x01},
+        {0x380b, 0xE0},
+        {0x3801, 0x00},
+        {0x3802, 0x00},
+        {0x3803, 0x00},
+        {0x3804, 0x0a},
+        {0x3805, 0x3f},
+        {0x3806, 0x07},
+        {0x3807, 0xa1},
+        {0x3811, 0x08},
+        {0x3813, 0x02},
+        {0x3630, 0x2e},
+        {0x3632, 0xe2},
+        {0x3633, 0x23},
+        {0x3634, 0x44},
+        {0x3636, 0x06},
+        {0x3620, 0x64},
+        {0x3621, 0xe0},
+        {0x3600, 0x37},
+        {0x3704, 0xa0},
+        {0x3703, 0x5a},
+        {0x3715, 0x78},
+        {0x3717, 0x01},
+        {0x3731, 0x02},
+        {0x370b, 0x60},
+        {0x3705, 0x1a},
+        {0x3f05, 0x02},
+        {0x3f06, 0x10},
+        {0x3f01, 0x0a},
+        {0x3a08, 0x01},
+        {0x3a09, 0x27},
+        {0x3a0a, 0x00},
+        {0x3a0b, 0xf6},
+        {0x3a0d, 0x04},
+        {0x3a0e, 0x03},
+        {0x3a0f, 0x58},
+        {0x3a10, 0x50},
+        {0x3a1b, 0x58},
+        {0x3a1e, 0x50},
+        {0x3a11, 0x60},
+        {0x3a1f, 0x28},
+        {0x4001, 0x02},
+        {0x4004, 0x02},
+        {0x4000, 0x09},
+        {0x4837, 0x24},
+        {0x4050, 0x6e},
+        {0x4051, 0x8f},
+        {0x0100, 0x01},
+};
+
+static int ov5647_write(struct v4l2_subdev *sd, u16 reg, u8 val)
+{
+        int ret;
+        unsigned char data[3] = { reg >> 8, reg & 0xff, val};
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+        ret = i2c_master_send(client, data, 3);
+        if (ret < 0)
+                dev_dbg(&client->dev, "%s: i2c write error, reg: %x\n",
+                                __func__, reg);
+
+        return ret;
+}
+
+static int ov5647_read(struct v4l2_subdev *sd, u16 reg, u8 *val)
+{
+        int ret;
+        unsigned char data_w[2] = { reg >> 8, reg & 0xff };
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+        ret = i2c_master_send(client, data_w, 2);
+        if (ret < 0) {
+                dev_dbg(&client->dev, "%s: i2c write error, reg: %x\n",
+                        __func__, reg);
+                return ret;
+        }
+
+        ret = i2c_master_recv(client, val, 1);
+        if (ret < 0)
+                dev_dbg(&client->dev, "%s: i2c read error, reg: %x\n",
+                                __func__, reg);
+
+        return ret;
+}
+
+static int ov5647_write_array(struct v4l2_subdev *sd,
+                                struct regval_list *regs, int array_size)
+{
+        int i, ret;
+
+        for (i = 0; i < array_size; i++) {
+                ret = ov5647_write(sd, regs[i].addr, regs[i].data);
+                if (ret < 0)
+                        return ret;
+        }
+
+        return 0;
+}
+
+static int ov5647_set_virtual_channel(struct v4l2_subdev *sd, int channel)
+{
+        u8 channel_id;
+        int ret;
+
+        ret = ov5647_read(sd, 0x4814, &channel_id);
+        if (ret < 0)
+                return ret;
+
+        channel_id &= ~(3 << 6);
+        return ov5647_write(sd, 0x4814, channel_id | (channel << 6));
+}
+
+static int ov5647_stream_on(struct v4l2_subdev *sd)
+{
+        int ret;
+
+        ret = ov5647_write(sd, 0x4202, 0x00);
+        if (ret < 0)
+                return ret;
+
+        return ov5647_write(sd, 0x300D, 0x00);
+}
+
+static int ov5647_stream_off(struct v4l2_subdev *sd)
+{
+        int ret;
+
+        ret = ov5647_write(sd, 0x4202, 0x0f);
+        if (ret < 0)
+                return ret;
+
+        return ov5647_write(sd, 0x300D, 0x01);
+}
+
+static int set_sw_standby(struct v4l2_subdev *sd, bool standby)
+{
+        int ret;
+        u8 rdval;
+
+        ret = ov5647_read(sd, 0x0100, &rdval);
+        if (ret < 0)
+                return ret;
+
+        if (standby)
+                rdval &= ~0x01;
+        else
+                rdval |= 0x01;
+
+        return ov5647_write(sd, 0x0100, rdval);
+}
+
+static int __sensor_init(struct v4l2_subdev *sd)
+{
+        int ret;
+        u8 resetval, rdval;
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+        ret = ov5647_read(sd, 0x0100, &rdval);
+        if (ret < 0)
+                return ret;
+
+        ret = ov5647_write_array(sd, ov5647_640x480,
+                                        ARRAY_SIZE(ov5647_640x480));
+        if (ret < 0) {
+                dev_err(&client->dev, "write sensor default regs error\n");
+                return ret;
+        }
+
+        ret = ov5647_set_virtual_channel(sd, 0);
+        if (ret < 0)
+                return ret;
+
+        ret = ov5647_read(sd, 0x0100, &resetval);
+        if (ret < 0)
+                return ret;
+
+        if (!(resetval & 0x01)) {
+                dev_err(&client->dev, "Device was in SW standby");
+                ret = ov5647_write(sd, 0x0100, 0x01);
+                if (ret < 0)
+                        return ret;
+        }
+
+        return ov5647_write(sd, 0x4800, 0x04);
+}
+
+static int ov5647_sensor_power(struct v4l2_subdev *sd, int on)
+{
+        int ret = 0;
+        struct ov5647 *ov5647 = to_state(sd);
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+        mutex_lock(&ov5647->lock);
+
+        if (on && !ov5647->power_count) {
+                dev_dbg(&client->dev, "OV5647 power on\n");
+
+                ret = clk_prepare_enable(ov5647->xclk);
+                if (ret < 0) {
+                        dev_err(&client->dev, "clk prepare enable failed\n");
+                        goto out;
+                }
+
+                ret = ov5647_write_array(sd, sensor_oe_enable_regs,
+                                ARRAY_SIZE(sensor_oe_enable_regs));
+                if (ret < 0) {
+                        clk_disable_unprepare(ov5647->xclk);
+                        dev_err(&client->dev,
+                                "write sensor_oe_enable_regs error\n");
+                        goto out;
+                }
+
+                ret = __sensor_init(sd);
+                if (ret < 0) {
+                        clk_disable_unprepare(ov5647->xclk);
+                        dev_err(&client->dev,
+                                "Camera not available, check Power\n");
+                        goto out;
+                }
+        } else if (!on && ov5647->power_count == 1) {
+                dev_dbg(&client->dev, "OV5647 power off\n");
+
+                ret = ov5647_write_array(sd, sensor_oe_disable_regs,
+                                ARRAY_SIZE(sensor_oe_disable_regs));
+
+                if (ret < 0)
+                        dev_dbg(&client->dev, "disable oe failed\n");
+
+                ret = set_sw_standby(sd, true);
+
+                if (ret < 0)
+                        dev_dbg(&client->dev, "soft stby failed\n");
+
+                clk_disable_unprepare(ov5647->xclk);
+        }
+
+        /* Update the power count. */
+        ov5647->power_count += on ? 1 : -1;
+        WARN_ON(ov5647->power_count < 0);
+
+out:
+        mutex_unlock(&ov5647->lock);
+
+        return ret;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov5647_sensor_get_register(struct v4l2_subdev *sd,
+                                struct v4l2_dbg_register *reg)
+{
+        u8 val;
+        int ret;
+
+        ret = ov5647_read(sd, reg->reg & 0xff, &val);
+        if (ret < 0)
+                return ret;
+
+        reg->val = val;
+        reg->size = 1;
+
+        return 0;
+}
+
+static int ov5647_sensor_set_register(struct v4l2_subdev *sd,
+                                const struct v4l2_dbg_register *reg)
+{
+        return ov5647_write(sd, reg->reg & 0xff, reg->val & 0xff);
+}
+#endif
+
+/**
+ * @short Subdev core operations registration
+ */
+static const struct v4l2_subdev_core_ops ov5647_subdev_core_ops = {
+        .s_power                = ov5647_sensor_power,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+        .g_register             = ov5647_sensor_get_register,
+        .s_register             = ov5647_sensor_set_register,
+#endif
+};
+
+static int ov5647_s_stream(struct v4l2_subdev *sd, int enable)
+{
+        if (enable)
+                return ov5647_stream_on(sd);
+        else
+                return ov5647_stream_off(sd);
+}
+
+static const struct v4l2_subdev_video_ops ov5647_subdev_video_ops = {
+        .s_stream =             ov5647_s_stream,
+};
+
+static int ov5647_enum_mbus_code(struct v4l2_subdev *sd,
+                                struct v4l2_subdev_pad_config *cfg,
+                                struct v4l2_subdev_mbus_code_enum *code)
+{
+        if (code->index > 0)
+                return -EINVAL;
+
+        code->code = MEDIA_BUS_FMT_SBGGR8_1X8;
+
+        return 0;
+}
+
+static const struct v4l2_subdev_pad_ops ov5647_subdev_pad_ops = {
+        .enum_mbus_code = ov5647_enum_mbus_code,
+};
+
+static const struct v4l2_subdev_ops ov5647_subdev_ops = {
+        .core           = &ov5647_subdev_core_ops,
+        .video          = &ov5647_subdev_video_ops,
+        .pad            = &ov5647_subdev_pad_ops,
+};
+
+static int ov5647_detect(struct v4l2_subdev *sd)
+{
+        u8 read;
+        int ret;
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+        ret = ov5647_write(sd, OV5647_SW_RESET, 0x01);
+        if (ret < 0)
+                return ret;
+
+        ret = ov5647_read(sd, OV5647_REG_CHIPID_H, &read);
+        if (ret < 0)
+                return ret;
+
+        if (read != 0x56) {
+                dev_err(&client->dev, "ID High expected 0x56 got %x", read);
+                return -ENODEV;
+        }
+
+        ret = ov5647_read(sd, OV5647_REG_CHIPID_L, &read);
+        if (ret < 0)
+                return ret;
+
+        if (read != 0x47) {
+                dev_err(&client->dev, "ID Low expected 0x47 got %x", read);
+                return -ENODEV;
+        }
+
+        return ov5647_write(sd, OV5647_SW_RESET, 0x00);
+}
+
+static int ov5647_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+        struct v4l2_mbus_framefmt *format =
+                                v4l2_subdev_get_try_format(sd, fh->pad, 0);
+        struct v4l2_rect *crop =
+                                v4l2_subdev_get_try_crop(sd, fh->pad, 0);
+
+        crop->left = OV5647_COLUMN_START_DEF;
+        crop->top = OV5647_ROW_START_DEF;
+        crop->width = OV5647_WINDOW_WIDTH_DEF;
+        crop->height = OV5647_WINDOW_HEIGHT_DEF;
+
+        format->code = MEDIA_BUS_FMT_SBGGR8_1X8;
+
+        format->width = OV5647_WINDOW_WIDTH_DEF;
+        format->height = OV5647_WINDOW_HEIGHT_DEF;
+        format->field = V4L2_FIELD_NONE;
+        format->colorspace = V4L2_COLORSPACE_SRGB;
+
+        return 0;
+}
+
+static const struct v4l2_subdev_internal_ops ov5647_subdev_internal_ops = {
+        .open = ov5647_open,
+};
+
+static int ov5647_parse_dt(struct device_node *np)
+{
+        struct v4l2_of_endpoint bus_cfg;
+        struct device_node *ep;
+
+        int ret;
+
+        ep = of_graph_get_next_endpoint(np, NULL);
+        if (!ep)
+                return -EINVAL;
+
+        ret = v4l2_of_parse_endpoint(ep, &bus_cfg);
+
+        of_node_put(ep);
+        return ret;
+}
+
+static int ov5647_probe(struct i2c_client *client,
+                        const struct i2c_device_id *id)
+{
+        struct device *dev = &client->dev;
+        struct ov5647 *sensor;
+        int ret;
+        struct v4l2_subdev *sd;
+        struct device_node *np = client->dev.of_node;
+        u32 xclk_freq;
+
+        sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+        if (!sensor)
+                return -ENOMEM;
+
+        if (IS_ENABLED(CONFIG_OF) && np) {
+                ret = ov5647_parse_dt(np);
+                if (ret) {
+                        dev_err(dev, "DT parsing error: %d\n", ret);
+                        return ret;
+                }
+        }
+
+        /* get system clock (xclk) */
+        sensor->xclk = devm_clk_get(dev, NULL);
+        if (IS_ERR(sensor->xclk)) {
+                dev_err(dev, "could not get xclk");
+                return PTR_ERR(sensor->xclk);
+        }
+
+        xclk_freq = clk_get_rate(sensor->xclk);
+        if (xclk_freq != 25000000) {
+                dev_err(dev, "Unsupported clock frequency: %u\n", xclk_freq);
+                return -EINVAL;
+        }
+
+        mutex_init(&sensor->lock);
+
+        sd = &sensor->sd;
+        v4l2_i2c_subdev_init(sd, client, &ov5647_subdev_ops);
+        sensor->sd.internal_ops = &ov5647_subdev_internal_ops;
+        sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+        sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+        sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+        ret = media_entity_pads_init(&sd->entity, 1, &sensor->pad);
+        if (ret < 0)
+                goto mutex_remove;
+
+        ret = ov5647_detect(sd);
+        if (ret < 0)
+                goto error;
+
+        ret = v4l2_async_register_subdev(sd);
+        if (ret < 0)
+                goto error;
+
+        dev_dbg(dev, "OmniVision OV5647 camera driver probed\n");
+        return 0;
+error:
+        media_entity_cleanup(&sd->entity);
+mutex_remove:
+        mutex_destroy(&sensor->lock);
+        return ret;
+}
+
+static int ov5647_remove(struct i2c_client *client)
+{
+        struct v4l2_subdev *sd = i2c_get_clientdata(client);
+        struct ov5647 *ov5647 = to_state(sd);
+
+        v4l2_async_unregister_subdev(&ov5647->sd);
+        media_entity_cleanup(&ov5647->sd.entity);
+        v4l2_device_unregister_subdev(sd);
+        mutex_destroy(&ov5647->lock);
+
+        return 0;
+}
+
+static const struct i2c_device_id ov5647_id[] = {
+        { "ov5647", 0 },
+        { }
+};
+MODULE_DEVICE_TABLE(i2c, ov5647_id);
+
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id ov5647_of_match[] = {
+        { .compatible = "ovti,ov5647" },
+        { /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, ov5647_of_match);
+#endif
+
+static struct i2c_driver ov5647_driver = {
+        .driver = {
+                .of_match_table = of_match_ptr(ov5647_of_match),
+                .name   = SENSOR_NAME,
+        },
+        .probe          = ov5647_probe,
+        .remove         = ov5647_remove,
+        .id_table       = ov5647_id,
+};
+
+module_i2c_driver(ov5647_driver);
+
+MODULE_AUTHOR("Ramiro Oliveira <roliveir@synopsys.com>");
+MODULE_DESCRIPTION("A low-level driver for OmniVision ov5647 sensors");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov7670.c b/drivers/media/i2c/ov7670.c
index 56cfb5ca9c95..7270c68ed18a 100644
--- a/drivers/media/i2c/ov7670.c
+++ b/drivers/media/i2c/ov7670.c
@@ -10,12 +10,15 @@
  * This file may be distributed under the terms of the GNU General
  * Public License, version 2.
  */
+#include <linux/clk.h>
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/i2c.h>
 #include <linux/delay.h>
 #include <linux/videodev2.h>
+#include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-mediabus.h>
@@ -227,6 +230,9 @@ struct ov7670_info {
                 struct v4l2_ctrl *hue;
         };
         struct ov7670_format_struct *fmt;  /* Current format */
+        struct clk *clk;
+        struct gpio_desc *resetb_gpio;
+        struct gpio_desc *pwdn_gpio;
         int min_width;                  /* Filter out smaller sizes */
         int min_height;                 /* Filter out smaller sizes */
         int clock_speed;                /* External clock speed (MHz) */
@@ -589,8 +595,6 @@ static int ov7670_init(struct v4l2_subdev *sd, u32 val)
         return ov7670_write_array(sd, ov7670_default_regs);
 }
 
-
-
 static int ov7670_detect(struct v4l2_subdev *sd)
 {
         unsigned char v;
@@ -1046,7 +1050,6 @@ static int ov7670_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
         if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
                 return -EINVAL;
 
-        memset(cp, 0, sizeof(struct v4l2_captureparm));
         cp->capability = V4L2_CAP_TIMEPERFRAME;
         info->devtype->get_framerate(sd, &cp->timeperframe);
 
@@ -1061,9 +1064,8 @@ static int ov7670_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
 
         if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
                 return -EINVAL;
-        if (cp->extendedmode != 0)
-                return -EINVAL;
 
+        cp->capability = V4L2_CAP_TIMEPERFRAME;
         return info->devtype->set_framerate(sd, tpf);
 }
 
@@ -1549,6 +1551,27 @@ static const struct ov7670_devtype ov7670_devdata[] = {
         },
 };
 
+static int ov7670_init_gpio(struct i2c_client *client, struct ov7670_info *info)
+{
+        info->pwdn_gpio = devm_gpiod_get_optional(&client->dev, "powerdown",
+                        GPIOD_OUT_LOW);
+        if (IS_ERR(info->pwdn_gpio)) {
+                dev_info(&client->dev, "can't get %s GPIO\n", "powerdown");
+                return PTR_ERR(info->pwdn_gpio);
+        }
+
+        info->resetb_gpio = devm_gpiod_get_optional(&client->dev, "reset",
+                        GPIOD_OUT_LOW);
+        if (IS_ERR(info->resetb_gpio)) {
+                dev_info(&client->dev, "can't get %s GPIO\n", "reset");
+                return PTR_ERR(info->resetb_gpio);
+        }
+
+        usleep_range(3000, 5000);
+
+        return 0;
+}
+
 static int ov7670_probe(struct i2c_client *client,
                         const struct i2c_device_id *id)
 {
@@ -1589,13 +1612,28 @@ static int ov7670_probe(struct i2c_client *client,
                         info->pclk_hb_disable = true;
         }
 
+        info->clk = devm_clk_get(&client->dev, "xclk");
+        if (IS_ERR(info->clk))
+                return -EPROBE_DEFER;
+        clk_prepare_enable(info->clk);
+
+        ret = ov7670_init_gpio(client, info);
+        if (ret)
+                goto clk_disable;
+
+        info->clock_speed = clk_get_rate(info->clk) / 1000000;
+        if (info->clock_speed < 10 || info->clock_speed > 48) {
+                ret = -EINVAL;
+                goto clk_disable;
+        }
+
         /* Make sure it's an ov7670 */
         ret = ov7670_detect(sd);
         if (ret) {
                 v4l_dbg(1, debug, client,
                         "chip found @ 0x%x (%s) is not an ov7670 chip.\n",
                         client->addr << 1, client->adapter->name);
-                return ret;
+                goto clk_disable;
         }
         v4l_info(client, "chip found @ 0x%02x (%s)\n",
                         client->addr << 1, client->adapter->name);
@@ -1636,10 +1674,9 @@ static int ov7670_probe(struct i2c_client *client,
                         V4L2_EXPOSURE_AUTO);
         sd->ctrl_handler = &info->hdl;
         if (info->hdl.error) {
-                int err = info->hdl.error;
+                ret = info->hdl.error;
 
-                v4l2_ctrl_handler_free(&info->hdl);
-                return err;
+                goto hdl_free;
         }
         /*
          * We have checked empirically that hw allows to read back the gain
@@ -1651,7 +1688,17 @@ static int ov7670_probe(struct i2c_client *client,
         v4l2_ctrl_cluster(2, &info->saturation);
         v4l2_ctrl_handler_setup(&info->hdl);
 
+        ret = v4l2_async_register_subdev(&info->sd);
+        if (ret < 0)
+                goto hdl_free;
+
         return 0;
+
+hdl_free:
+        v4l2_ctrl_handler_free(&info->hdl);
+clk_disable:
+        clk_disable_unprepare(info->clk);
+        return ret;
 }
 
 
@@ -1662,6 +1709,7 @@ static int ov7670_remove(struct i2c_client *client)
 
         v4l2_device_unregister_subdev(sd);
         v4l2_ctrl_handler_free(&info->hdl);
+        clk_disable_unprepare(info->clk);
         return 0;
 }
 
@@ -1672,9 +1720,18 @@ static const struct i2c_device_id ov7670_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, ov7670_id);
 
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id ov7670_of_match[] = {
+        { .compatible = "ovti,ov7670", },
+        { /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, ov7670_of_match);
+#endif
+
 static struct i2c_driver ov7670_driver = {
         .driver = {
                 .name   = "ov7670",
+                .of_match_table = of_match_ptr(ov7670_of_match),
         },
         .probe          = ov7670_probe,
         .remove         = ov7670_remove,
diff --git a/drivers/media/i2c/soc_camera/Kconfig b/drivers/media/i2c/soc_camera/Kconfig
index 7704bcf5cc25..96859f37cb1c 100644
--- a/drivers/media/i2c/soc_camera/Kconfig
+++ b/drivers/media/i2c/soc_camera/Kconfig
@@ -41,12 +41,6 @@ config SOC_CAMERA_MT9V022
         help
           This driver supports MT9V022 cameras from Micron
 
-config SOC_CAMERA_OV2640
-        tristate "ov2640 camera support"
-        depends on SOC_CAMERA && I2C
-        help
-          This is a ov2640 camera driver
-
 config SOC_CAMERA_OV5642
         tristate "ov5642 camera support"
         depends on SOC_CAMERA && I2C
diff --git a/drivers/media/i2c/soc_camera/Makefile b/drivers/media/i2c/soc_camera/Makefile
index 6f994f9353a0..974bdb721dbe 100644
--- a/drivers/media/i2c/soc_camera/Makefile
+++ b/drivers/media/i2c/soc_camera/Makefile
@@ -3,7 +3,6 @@ obj-$(CONFIG_SOC_CAMERA_MT9M001)	+= mt9m001.o
 obj-$(CONFIG_SOC_CAMERA_MT9T031)        += mt9t031.o
 obj-$(CONFIG_SOC_CAMERA_MT9T112)        += mt9t112.o
 obj-$(CONFIG_SOC_CAMERA_MT9V022)        += mt9v022.o
-obj-$(CONFIG_SOC_CAMERA_OV2640)         += ov2640.o
 obj-$(CONFIG_SOC_CAMERA_OV5642)         += ov5642.o
 obj-$(CONFIG_SOC_CAMERA_OV6650)         += ov6650.o
 obj-$(CONFIG_SOC_CAMERA_OV772X)         += ov772x.o
diff --git a/drivers/media/i2c/soc_camera/imx074.c b/drivers/media/i2c/soc_camera/imx074.c
index 05b55cfe8147..77f1e0243d6e 100644
--- a/drivers/media/i2c/soc_camera/imx074.c
+++ b/drivers/media/i2c/soc_camera/imx074.c
@@ -180,7 +180,7 @@ static int imx074_set_fmt(struct v4l2_subdev *sd,
         mf->field       = V4L2_FIELD_NONE;
 
         if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
-                priv->fmt = imx074_find_datafmt(mf->code);
+                priv->fmt = fmt;
         else
                 cfg->try_fmt = *mf;
 
@@ -271,12 +271,12 @@ static int imx074_g_mbus_config(struct v4l2_subdev *sd,
         return 0;
 }
 
-static struct v4l2_subdev_video_ops imx074_subdev_video_ops = {
+static const struct v4l2_subdev_video_ops imx074_subdev_video_ops = {
         .s_stream       = imx074_s_stream,
         .g_mbus_config  = imx074_g_mbus_config,
 };
 
-static struct v4l2_subdev_core_ops imx074_subdev_core_ops = {
+static const struct v4l2_subdev_core_ops imx074_subdev_core_ops = {
         .s_power        = imx074_s_power,
 };
 
@@ -287,7 +287,7 @@ static const struct v4l2_subdev_pad_ops imx074_subdev_pad_ops = {
         .set_fmt        = imx074_set_fmt,
 };
 
-static struct v4l2_subdev_ops imx074_subdev_ops = {
+static const struct v4l2_subdev_ops imx074_subdev_ops = {
         .core   = &imx074_subdev_core_ops,
         .video  = &imx074_subdev_video_ops,
         .pad    = &imx074_subdev_pad_ops,
diff --git a/drivers/media/i2c/soc_camera/mt9m001.c b/drivers/media/i2c/soc_camera/mt9m001.c
index 3d6378d4491c..1bfb0d53059e 100644
--- a/drivers/media/i2c/soc_camera/mt9m001.c
+++ b/drivers/media/i2c/soc_camera/mt9m001.c
@@ -278,6 +278,7 @@ static int mt9m001_get_fmt(struct v4l2_subdev *sd,
 }
 
 static int mt9m001_s_fmt(struct v4l2_subdev *sd,
+                         const struct mt9m001_datafmt *fmt,
                          struct v4l2_mbus_framefmt *mf)
 {
         struct i2c_client *client = v4l2_get_subdevdata(sd);
@@ -297,9 +298,8 @@ static int mt9m001_s_fmt(struct v4l2_subdev *sd,
         if (!ret) {
                 mf->width       = mt9m001->rect.width;
                 mf->height      = mt9m001->rect.height;
-                mt9m001->fmt    = mt9m001_find_datafmt(mf->code,
-                                        mt9m001->fmts, mt9m001->num_fmts);
-                mf->colorspace  = mt9m001->fmt->colorspace;
+                mt9m001->fmt    = fmt;
+                mf->colorspace  = fmt->colorspace;
         }
 
         return ret;
@@ -335,7 +335,7 @@ static int mt9m001_set_fmt(struct v4l2_subdev *sd,
         mf->colorspace  = fmt->colorspace;
 
         if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
-                return mt9m001_s_fmt(sd, mf);
+                return mt9m001_s_fmt(sd, fmt, mf);
         cfg->try_fmt = *mf;
         return 0;
 }
@@ -574,7 +574,7 @@ static const struct v4l2_ctrl_ops mt9m001_ctrl_ops = {
         .s_ctrl = mt9m001_s_ctrl,
 };
 
-static struct v4l2_subdev_core_ops mt9m001_subdev_core_ops = {
+static const struct v4l2_subdev_core_ops mt9m001_subdev_core_ops = {
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register     = mt9m001_g_register,
         .s_register     = mt9m001_s_register,
@@ -630,7 +630,7 @@ static int mt9m001_s_mbus_config(struct v4l2_subdev *sd,
         return bps == 10 ? 0 : -EINVAL;
 }
 
-static struct v4l2_subdev_video_ops mt9m001_subdev_video_ops = {
+static const struct v4l2_subdev_video_ops mt9m001_subdev_video_ops = {
         .s_stream       = mt9m001_s_stream,
         .g_mbus_config  = mt9m001_g_mbus_config,
         .s_mbus_config  = mt9m001_s_mbus_config,
@@ -648,7 +648,7 @@ static const struct v4l2_subdev_pad_ops mt9m001_subdev_pad_ops = {
         .set_fmt        = mt9m001_set_fmt,
 };
 
-static struct v4l2_subdev_ops mt9m001_subdev_ops = {
+static const struct v4l2_subdev_ops mt9m001_subdev_ops = {
         .core   = &mt9m001_subdev_core_ops,
         .video  = &mt9m001_subdev_video_ops,
         .sensor = &mt9m001_subdev_sensor_ops,
diff --git a/drivers/media/i2c/soc_camera/mt9t031.c b/drivers/media/i2c/soc_camera/mt9t031.c
index 3aa5569065ad..714fb3555b34 100644
--- a/drivers/media/i2c/soc_camera/mt9t031.c
+++ b/drivers/media/i2c/soc_camera/mt9t031.c
@@ -679,7 +679,7 @@ static const struct v4l2_ctrl_ops mt9t031_ctrl_ops = {
         .s_ctrl = mt9t031_s_ctrl,
 };
 
-static struct v4l2_subdev_core_ops mt9t031_subdev_core_ops = {
+static const struct v4l2_subdev_core_ops mt9t031_subdev_core_ops = {
         .s_power        = mt9t031_s_power,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register     = mt9t031_g_register,
@@ -726,7 +726,7 @@ static int mt9t031_s_mbus_config(struct v4l2_subdev *sd,
                 return reg_set(client, MT9T031_PIXEL_CLOCK_CONTROL, 0x8000);
 }
 
-static struct v4l2_subdev_video_ops mt9t031_subdev_video_ops = {
+static const struct v4l2_subdev_video_ops mt9t031_subdev_video_ops = {
         .s_stream       = mt9t031_s_stream,
         .g_mbus_config  = mt9t031_g_mbus_config,
         .s_mbus_config  = mt9t031_s_mbus_config,
@@ -744,7 +744,7 @@ static const struct v4l2_subdev_pad_ops mt9t031_subdev_pad_ops = {
         .set_fmt        = mt9t031_set_fmt,
 };
 
-static struct v4l2_subdev_ops mt9t031_subdev_ops = {
+static const struct v4l2_subdev_ops mt9t031_subdev_ops = {
         .core   = &mt9t031_subdev_core_ops,
         .video  = &mt9t031_subdev_video_ops,
         .sensor = &mt9t031_subdev_sensor_ops,
diff --git a/drivers/media/i2c/soc_camera/mt9t112.c b/drivers/media/i2c/soc_camera/mt9t112.c
index 2ef22241ec14..297d22ebcb18 100644
--- a/drivers/media/i2c/soc_camera/mt9t112.c
+++ b/drivers/media/i2c/soc_camera/mt9t112.c
@@ -773,7 +773,7 @@ static int mt9t112_s_power(struct v4l2_subdev *sd, int on)
         return soc_camera_set_power(&client->dev, ssdd, priv->clk, on);
 }
 
-static struct v4l2_subdev_core_ops mt9t112_subdev_core_ops = {
+static const struct v4l2_subdev_core_ops mt9t112_subdev_core_ops = {
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register     = mt9t112_g_register,
         .s_register     = mt9t112_s_register,
@@ -1031,7 +1031,7 @@ static int mt9t112_s_mbus_config(struct v4l2_subdev *sd,
         return 0;
 }
 
-static struct v4l2_subdev_video_ops mt9t112_subdev_video_ops = {
+static const struct v4l2_subdev_video_ops mt9t112_subdev_video_ops = {
         .s_stream       = mt9t112_s_stream,
         .g_mbus_config  = mt9t112_g_mbus_config,
         .s_mbus_config  = mt9t112_s_mbus_config,
@@ -1048,7 +1048,7 @@ static const struct v4l2_subdev_pad_ops mt9t112_subdev_pad_ops = {
 /************************************************************************
                         i2c driver
 ************************************************************************/
-static struct v4l2_subdev_ops mt9t112_subdev_ops = {
+static const struct v4l2_subdev_ops mt9t112_subdev_ops = {
         .core   = &mt9t112_subdev_core_ops,
         .video  = &mt9t112_subdev_video_ops,
         .pad    = &mt9t112_subdev_pad_ops,
diff --git a/drivers/media/i2c/soc_camera/mt9v022.c b/drivers/media/i2c/soc_camera/mt9v022.c
index 6a14ab5e4f2d..762f06919329 100644
--- a/drivers/media/i2c/soc_camera/mt9v022.c
+++ b/drivers/media/i2c/soc_camera/mt9v022.c
@@ -403,6 +403,7 @@ static int mt9v022_get_fmt(struct v4l2_subdev *sd,
 }
 
 static int mt9v022_s_fmt(struct v4l2_subdev *sd,
+                         const struct mt9v022_datafmt *fmt,
                          struct v4l2_mbus_framefmt *mf)
 {
         struct i2c_client *client = v4l2_get_subdevdata(sd);
@@ -441,9 +442,8 @@ static int mt9v022_s_fmt(struct v4l2_subdev *sd,
         if (!ret) {
                 mf->width       = mt9v022->rect.width;
                 mf->height      = mt9v022->rect.height;
-                mt9v022->fmt    = mt9v022_find_datafmt(mf->code,
-                                        mt9v022->fmts, mt9v022->num_fmts);
-                mf->colorspace  = mt9v022->fmt->colorspace;
+                mt9v022->fmt    = fmt;
+                mf->colorspace  = fmt->colorspace;
         }
 
         return ret;
@@ -478,7 +478,7 @@ static int mt9v022_set_fmt(struct v4l2_subdev *sd,
         mf->colorspace  = fmt->colorspace;
 
         if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
-                return mt9v022_s_fmt(sd, mf);
+                return mt9v022_s_fmt(sd, fmt, mf);
         cfg->try_fmt = *mf;
         return 0;
 }
@@ -770,7 +770,7 @@ static const struct v4l2_ctrl_ops mt9v022_ctrl_ops = {
         .s_ctrl = mt9v022_s_ctrl,
 };
 
-static struct v4l2_subdev_core_ops mt9v022_subdev_core_ops = {
+static const struct v4l2_subdev_core_ops mt9v022_subdev_core_ops = {
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register     = mt9v022_g_register,
         .s_register     = mt9v022_s_register,
@@ -858,7 +858,7 @@ static int mt9v022_s_mbus_config(struct v4l2_subdev *sd,
         return 0;
 }
 
-static struct v4l2_subdev_video_ops mt9v022_subdev_video_ops = {
+static const struct v4l2_subdev_video_ops mt9v022_subdev_video_ops = {
         .s_stream       = mt9v022_s_stream,
         .g_mbus_config  = mt9v022_g_mbus_config,
         .s_mbus_config  = mt9v022_s_mbus_config,
@@ -876,7 +876,7 @@ static const struct v4l2_subdev_pad_ops mt9v022_subdev_pad_ops = {
         .set_fmt        = mt9v022_set_fmt,
 };
 
-static struct v4l2_subdev_ops mt9v022_subdev_ops = {
+static const struct v4l2_subdev_ops mt9v022_subdev_ops = {
         .core   = &mt9v022_subdev_core_ops,
         .video  = &mt9v022_subdev_video_ops,
         .sensor = &mt9v022_subdev_sensor_ops,
diff --git a/drivers/media/i2c/soc_camera/ov5642.c b/drivers/media/i2c/soc_camera/ov5642.c
index 3d185bd622a3..39f420db9c70 100644
--- a/drivers/media/i2c/soc_camera/ov5642.c
+++ b/drivers/media/i2c/soc_camera/ov5642.c
@@ -811,7 +811,7 @@ static int ov5642_set_fmt(struct v4l2_subdev *sd,
         mf->field       = V4L2_FIELD_NONE;
 
         if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
-                priv->fmt = ov5642_find_datafmt(mf->code);
+                priv->fmt = fmt;
         else
                 cfg->try_fmt = *mf;
         return 0;
@@ -943,7 +943,7 @@ static int ov5642_s_power(struct v4l2_subdev *sd, int on)
         return ret;
 }
 
-static struct v4l2_subdev_video_ops ov5642_subdev_video_ops = {
+static const struct v4l2_subdev_video_ops ov5642_subdev_video_ops = {
         .g_mbus_config  = ov5642_g_mbus_config,
 };
 
@@ -955,7 +955,7 @@ static const struct v4l2_subdev_pad_ops ov5642_subdev_pad_ops = {
         .set_fmt        = ov5642_set_fmt,
 };
 
-static struct v4l2_subdev_core_ops ov5642_subdev_core_ops = {
+static const struct v4l2_subdev_core_ops ov5642_subdev_core_ops = {
         .s_power        = ov5642_s_power,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register     = ov5642_get_register,
@@ -963,7 +963,7 @@ static struct v4l2_subdev_core_ops ov5642_subdev_core_ops = {
 #endif
 };
 
-static struct v4l2_subdev_ops ov5642_subdev_ops = {
+static const struct v4l2_subdev_ops ov5642_subdev_ops = {
         .core   = &ov5642_subdev_core_ops,
         .video  = &ov5642_subdev_video_ops,
         .pad    = &ov5642_subdev_pad_ops,
@@ -1063,9 +1063,18 @@ static const struct i2c_device_id ov5642_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, ov5642_id);
 
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id ov5642_of_match[] = {
+        { .compatible = "ovti,ov5642" },
+        { },
+};
+MODULE_DEVICE_TABLE(of, ov5642_of_match);
+#endif
+
 static struct i2c_driver ov5642_i2c_driver = {
         .driver = {
                 .name = "ov5642",
+                .of_match_table = of_match_ptr(ov5642_of_match),
         },
         .probe          = ov5642_probe,
         .remove         = ov5642_remove,
diff --git a/drivers/media/i2c/soc_camera/ov6650.c b/drivers/media/i2c/soc_camera/ov6650.c
index 4bf2995e1cb8..dbd6d92c589f 100644
--- a/drivers/media/i2c/soc_camera/ov6650.c
+++ b/drivers/media/i2c/soc_camera/ov6650.c
@@ -885,7 +885,7 @@ static const struct v4l2_ctrl_ops ov6550_ctrl_ops = {
         .s_ctrl = ov6550_s_ctrl,
 };
 
-static struct v4l2_subdev_core_ops ov6650_core_ops = {
+static const struct v4l2_subdev_core_ops ov6650_core_ops = {
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register             = ov6650_get_register,
         .s_register             = ov6650_set_register,
@@ -942,7 +942,7 @@ static int ov6650_s_mbus_config(struct v4l2_subdev *sd,
         return ret;
 }
 
-static struct v4l2_subdev_video_ops ov6650_video_ops = {
+static const struct v4l2_subdev_video_ops ov6650_video_ops = {
         .s_stream       = ov6650_s_stream,
         .g_parm         = ov6650_g_parm,
         .s_parm         = ov6650_s_parm,
@@ -958,7 +958,7 @@ static const struct v4l2_subdev_pad_ops ov6650_pad_ops = {
         .set_fmt        = ov6650_set_fmt,
 };
 
-static struct v4l2_subdev_ops ov6650_subdev_ops = {
+static const struct v4l2_subdev_ops ov6650_subdev_ops = {
         .core   = &ov6650_core_ops,
         .video  = &ov6650_video_ops,
         .pad    = &ov6650_pad_ops,
@@ -1033,7 +1033,7 @@ static int ov6650_probe(struct i2c_client *client,
         priv->code        = MEDIA_BUS_FMT_YUYV8_2X8;
         priv->colorspace  = V4L2_COLORSPACE_JPEG;
 
-        priv->clk = v4l2_clk_get(&client->dev, "mclk");
+        priv->clk = v4l2_clk_get(&client->dev, NULL);
         if (IS_ERR(priv->clk)) {
                 ret = PTR_ERR(priv->clk);
                 goto eclkget;
diff --git a/drivers/media/i2c/soc_camera/ov772x.c b/drivers/media/i2c/soc_camera/ov772x.c
index 985a3672b243..0f7b9d1b9c57 100644
--- a/drivers/media/i2c/soc_camera/ov772x.c
+++ b/drivers/media/i2c/soc_camera/ov772x.c
@@ -894,38 +894,15 @@ static int ov772x_get_fmt(struct v4l2_subdev *sd,
         return 0;
 }
 
-static int ov772x_s_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
-{
-        struct ov772x_priv *priv = to_ov772x(sd);
-        const struct ov772x_color_format *cfmt;
-        const struct ov772x_win_size *win;
-        int ret;
-
-        ov772x_select_params(mf, &cfmt, &win);
-
-        ret = ov772x_set_params(priv, cfmt, win);
-        if (ret < 0)
-                return ret;
-
-        priv->win = win;
-        priv->cfmt = cfmt;
-
-        mf->code = cfmt->code;
-        mf->width = win->rect.width;
-        mf->height = win->rect.height;
-        mf->field = V4L2_FIELD_NONE;
-        mf->colorspace = cfmt->colorspace;
-
-        return 0;
-}
-
 static int ov772x_set_fmt(struct v4l2_subdev *sd,
                 struct v4l2_subdev_pad_config *cfg,
                 struct v4l2_subdev_format *format)
 {
+        struct ov772x_priv *priv = to_ov772x(sd);
         struct v4l2_mbus_framefmt *mf = &format->format;
         const struct ov772x_color_format *cfmt;
         const struct ov772x_win_size *win;
+        int ret;
 
         if (format->pad)
                 return -EINVAL;
@@ -938,9 +915,17 @@ static int ov772x_set_fmt(struct v4l2_subdev *sd,
         mf->field = V4L2_FIELD_NONE;
         mf->colorspace = cfmt->colorspace;
 
-        if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
-                return ov772x_s_fmt(sd, mf);
-        cfg->try_fmt = *mf;
+        if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+                cfg->try_fmt = *mf;
+                return 0;
+        }
+
+        ret = ov772x_set_params(priv, cfmt, win);
+        if (ret < 0)
+                return ret;
+
+        priv->win = win;
+        priv->cfmt = cfmt;
         return 0;
 }
 
@@ -993,7 +978,7 @@ static const struct v4l2_ctrl_ops ov772x_ctrl_ops = {
         .s_ctrl = ov772x_s_ctrl,
 };
 
-static struct v4l2_subdev_core_ops ov772x_subdev_core_ops = {
+static const struct v4l2_subdev_core_ops ov772x_subdev_core_ops = {
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register     = ov772x_g_register,
         .s_register     = ov772x_s_register,
@@ -1027,7 +1012,7 @@ static int ov772x_g_mbus_config(struct v4l2_subdev *sd,
         return 0;
 }
 
-static struct v4l2_subdev_video_ops ov772x_subdev_video_ops = {
+static const struct v4l2_subdev_video_ops ov772x_subdev_video_ops = {
         .s_stream       = ov772x_s_stream,
         .g_mbus_config  = ov772x_g_mbus_config,
 };
@@ -1039,7 +1024,7 @@ static const struct v4l2_subdev_pad_ops ov772x_subdev_pad_ops = {
         .set_fmt        = ov772x_set_fmt,
 };
 
-static struct v4l2_subdev_ops ov772x_subdev_ops = {
+static const struct v4l2_subdev_ops ov772x_subdev_ops = {
         .core   = &ov772x_subdev_core_ops,
         .video  = &ov772x_subdev_video_ops,
         .pad    = &ov772x_subdev_pad_ops,
diff --git a/drivers/media/i2c/soc_camera/ov9640.c b/drivers/media/i2c/soc_camera/ov9640.c
index 65085a235128..0146d1f7aacb 100644
--- a/drivers/media/i2c/soc_camera/ov9640.c
+++ b/drivers/media/i2c/soc_camera/ov9640.c
@@ -486,11 +486,8 @@ static int ov9640_s_fmt(struct v4l2_subdev *sd,
 {
         struct i2c_client *client = v4l2_get_subdevdata(sd);
         struct ov9640_reg_alt alts = {0};
-        enum v4l2_colorspace cspace;
-        u32 code = mf->code;
         int ret;
 
-        ov9640_res_roundup(&mf->width, &mf->height);
         ov9640_alter_regs(mf->code, &alts);
 
         ov9640_reset(client);
@@ -499,24 +496,7 @@ static int ov9640_s_fmt(struct v4l2_subdev *sd,
         if (ret)
                 return ret;
 
-        switch (code) {
-        case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
-        case MEDIA_BUS_FMT_RGB565_2X8_LE:
-                cspace = V4L2_COLORSPACE_SRGB;
-                break;
-        default:
-                code = MEDIA_BUS_FMT_UYVY8_2X8;
-        case MEDIA_BUS_FMT_UYVY8_2X8:
-                cspace = V4L2_COLORSPACE_JPEG;
-        }
-
-        ret = ov9640_write_regs(client, mf->width, code, &alts);
-        if (!ret) {
-                mf->code        = code;
-                mf->colorspace  = cspace;
-        }
-
-        return ret;
+        return ov9640_write_regs(client, mf->width, mf->code, &alts);
 }
 
 static int ov9640_set_fmt(struct v4l2_subdev *sd,
@@ -539,8 +519,10 @@ static int ov9640_set_fmt(struct v4l2_subdev *sd,
                 break;
         default:
                 mf->code = MEDIA_BUS_FMT_UYVY8_2X8;
+                /* fall through */
         case MEDIA_BUS_FMT_UYVY8_2X8:
                 mf->colorspace = V4L2_COLORSPACE_JPEG;
+                break;
         }
 
         if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
@@ -637,7 +619,7 @@ static const struct v4l2_ctrl_ops ov9640_ctrl_ops = {
         .s_ctrl = ov9640_s_ctrl,
 };
 
-static struct v4l2_subdev_core_ops ov9640_core_ops = {
+static const struct v4l2_subdev_core_ops ov9640_core_ops = {
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register             = ov9640_get_register,
         .s_register             = ov9640_set_register,
@@ -661,7 +643,7 @@ static int ov9640_g_mbus_config(struct v4l2_subdev *sd,
         return 0;
 }
 
-static struct v4l2_subdev_video_ops ov9640_video_ops = {
+static const struct v4l2_subdev_video_ops ov9640_video_ops = {
         .s_stream       = ov9640_s_stream,
         .g_mbus_config  = ov9640_g_mbus_config,
 };
@@ -672,7 +654,7 @@ static const struct v4l2_subdev_pad_ops ov9640_pad_ops = {
         .set_fmt        = ov9640_set_fmt,
 };
 
-static struct v4l2_subdev_ops ov9640_subdev_ops = {
+static const struct v4l2_subdev_ops ov9640_subdev_ops = {
         .core   = &ov9640_core_ops,
         .video  = &ov9640_video_ops,
         .pad    = &ov9640_pad_ops,
diff --git a/drivers/media/i2c/soc_camera/ov9740.c b/drivers/media/i2c/soc_camera/ov9740.c
index f11f76cdacad..cc07b7ae5407 100644
--- a/drivers/media/i2c/soc_camera/ov9740.c
+++ b/drivers/media/i2c/soc_camera/ov9740.c
@@ -673,20 +673,8 @@ static int ov9740_s_fmt(struct v4l2_subdev *sd,
 {
         struct i2c_client *client = v4l2_get_subdevdata(sd);
         struct ov9740_priv *priv = to_ov9740(sd);
-        enum v4l2_colorspace cspace;
-        u32 code = mf->code;
         int ret;
 
-        ov9740_res_roundup(&mf->width, &mf->height);
-
-        switch (code) {
-        case MEDIA_BUS_FMT_YUYV8_2X8:
-                cspace = V4L2_COLORSPACE_SRGB;
-                break;
-        default:
-                return -EINVAL;
-        }
-
         ret = ov9740_reg_write_array(client, ov9740_defaults,
                                      ARRAY_SIZE(ov9740_defaults));
         if (ret < 0)
@@ -696,11 +684,7 @@ static int ov9740_s_fmt(struct v4l2_subdev *sd,
         if (ret < 0)
                 return ret;
 
-        mf->code        = code;
-        mf->colorspace  = cspace;
-
-        memcpy(&priv->current_mf, mf, sizeof(struct v4l2_mbus_framefmt));
-
+        priv->current_mf = *mf;
         return ret;
 }
 
@@ -907,12 +891,12 @@ static int ov9740_g_mbus_config(struct v4l2_subdev *sd,
         return 0;
 }
 
-static struct v4l2_subdev_video_ops ov9740_video_ops = {
+static const struct v4l2_subdev_video_ops ov9740_video_ops = {
         .s_stream       = ov9740_s_stream,
         .g_mbus_config  = ov9740_g_mbus_config,
 };
 
-static struct v4l2_subdev_core_ops ov9740_core_ops = {
+static const struct v4l2_subdev_core_ops ov9740_core_ops = {
         .s_power                = ov9740_s_power,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register             = ov9740_get_register,
@@ -926,7 +910,7 @@ static const struct v4l2_subdev_pad_ops ov9740_pad_ops = {
         .set_fmt        = ov9740_set_fmt,
 };
 
-static struct v4l2_subdev_ops ov9740_subdev_ops = {
+static const struct v4l2_subdev_ops ov9740_subdev_ops = {
         .core   = &ov9740_core_ops,
         .video  = &ov9740_video_ops,
         .pad    = &ov9740_pad_ops,
diff --git a/drivers/media/i2c/soc_camera/rj54n1cb0c.c b/drivers/media/i2c/soc_camera/rj54n1cb0c.c
index bc8ec59a3fbd..02398d0bc649 100644
--- a/drivers/media/i2c/soc_camera/rj54n1cb0c.c
+++ b/drivers/media/i2c/soc_camera/rj54n1cb0c.c
@@ -1213,7 +1213,7 @@ static const struct v4l2_ctrl_ops rj54n1_ctrl_ops = {
         .s_ctrl = rj54n1_s_ctrl,
 };
 
-static struct v4l2_subdev_core_ops rj54n1_subdev_core_ops = {
+static const struct v4l2_subdev_core_ops rj54n1_subdev_core_ops = {
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register     = rj54n1_g_register,
         .s_register     = rj54n1_s_register,
@@ -1251,7 +1251,7 @@ static int rj54n1_s_mbus_config(struct v4l2_subdev *sd,
                 return reg_write(client, RJ54N1_OUT_SIGPO, 0);
 }
 
-static struct v4l2_subdev_video_ops rj54n1_subdev_video_ops = {
+static const struct v4l2_subdev_video_ops rj54n1_subdev_video_ops = {
         .s_stream       = rj54n1_s_stream,
         .g_mbus_config  = rj54n1_g_mbus_config,
         .s_mbus_config  = rj54n1_s_mbus_config,
@@ -1265,7 +1265,7 @@ static const struct v4l2_subdev_pad_ops rj54n1_subdev_pad_ops = {
         .set_fmt        = rj54n1_set_fmt,
 };
 
-static struct v4l2_subdev_ops rj54n1_subdev_ops = {
+static const struct v4l2_subdev_ops rj54n1_subdev_ops = {
         .core   = &rj54n1_subdev_core_ops,
         .video  = &rj54n1_subdev_video_ops,
         .pad    = &rj54n1_subdev_pad_ops,
diff --git a/drivers/media/i2c/soc_camera/tw9910.c b/drivers/media/i2c/soc_camera/tw9910.c
index c9c49ed707b8..bdb5e0a431e9 100644
--- a/drivers/media/i2c/soc_camera/tw9910.c
+++ b/drivers/media/i2c/soc_camera/tw9910.c
@@ -837,7 +837,7 @@ done:
         return ret;
 }
 
-static struct v4l2_subdev_core_ops tw9910_subdev_core_ops = {
+static const struct v4l2_subdev_core_ops tw9910_subdev_core_ops = {
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register     = tw9910_g_register,
         .s_register     = tw9910_s_register,
@@ -901,7 +901,7 @@ static int tw9910_g_tvnorms(struct v4l2_subdev *sd, v4l2_std_id *norm)
         return 0;
 }
 
-static struct v4l2_subdev_video_ops tw9910_subdev_video_ops = {
+static const struct v4l2_subdev_video_ops tw9910_subdev_video_ops = {
         .s_std          = tw9910_s_std,
         .g_std          = tw9910_g_std,
         .s_stream       = tw9910_s_stream,
@@ -917,7 +917,7 @@ static const struct v4l2_subdev_pad_ops tw9910_subdev_pad_ops = {
         .set_fmt        = tw9910_set_fmt,
 };
 
-static struct v4l2_subdev_ops tw9910_subdev_ops = {
+static const struct v4l2_subdev_ops tw9910_subdev_ops = {
         .core   = &tw9910_subdev_core_ops,
         .video  = &tw9910_subdev_video_ops,
         .pad    = &tw9910_subdev_pad_ops,
diff --git a/drivers/media/i2c/tc358743.c b/drivers/media/i2c/tc358743.c
index f569a05fe105..acef4eca269f 100644
--- a/drivers/media/i2c/tc358743.c
+++ b/drivers/media/i2c/tc358743.c
@@ -194,57 +194,61 @@ static void i2c_wr(struct v4l2_subdev *sd, u16 reg, u8 *values, u32 n)
         }
 }
 
-static u8 i2c_rd8(struct v4l2_subdev *sd, u16 reg)
+static noinline u32 i2c_rdreg(struct v4l2_subdev *sd, u16 reg, u32 n)
 {
-        u8 val;
+        __le32 val = 0;
+
+        i2c_rd(sd, reg, (u8 __force *)&val, n);
 
-        i2c_rd(sd, reg, &val, 1);
+        return le32_to_cpu(val);
+}
+
+static noinline void i2c_wrreg(struct v4l2_subdev *sd, u16 reg, u32 val, u32 n)
+{
+        __le32 raw = cpu_to_le32(val);
 
-        return val;
+        i2c_wr(sd, reg, (u8 __force *)&raw, n);
+}
+
+static u8 i2c_rd8(struct v4l2_subdev *sd, u16 reg)
+{
+        return i2c_rdreg(sd, reg, 1);
 }
 
 static void i2c_wr8(struct v4l2_subdev *sd, u16 reg, u8 val)
 {
-        i2c_wr(sd, reg, &val, 1);
+        i2c_wrreg(sd, reg, val, 1);
 }
 
 static void i2c_wr8_and_or(struct v4l2_subdev *sd, u16 reg,
                 u8 mask, u8 val)
 {
-        i2c_wr8(sd, reg, (i2c_rd8(sd, reg) & mask) | val);
+        i2c_wrreg(sd, reg, (i2c_rdreg(sd, reg, 2) & mask) | val, 2);
 }
 
 static u16 i2c_rd16(struct v4l2_subdev *sd, u16 reg)
 {
-        u16 val;
-
-        i2c_rd(sd, reg, (u8 *)&val, 2);
-
-        return val;
+        return i2c_rdreg(sd, reg, 2);
 }
 
 static void i2c_wr16(struct v4l2_subdev *sd, u16 reg, u16 val)
 {
-        i2c_wr(sd, reg, (u8 *)&val, 2);
+        i2c_wrreg(sd, reg, val, 2);
 }
 
 static void i2c_wr16_and_or(struct v4l2_subdev *sd, u16 reg, u16 mask, u16 val)
 {
-        i2c_wr16(sd, reg, (i2c_rd16(sd, reg) & mask) | val);
+        i2c_wrreg(sd, reg, (i2c_rdreg(sd, reg, 2) & mask) | val, 2);
 }
 
 static u32 i2c_rd32(struct v4l2_subdev *sd, u16 reg)
 {
-        u32 val;
-
-        i2c_rd(sd, reg, (u8 *)&val, 4);
-
-        return val;
+        return i2c_rdreg(sd, reg, 4);
 }
 
 static void i2c_wr32(struct v4l2_subdev *sd, u16 reg, u32 val)
 {
-        i2c_wr(sd, reg, (u8 *)&val, 4);
+        i2c_wrreg(sd, reg, val, 4);
 }
 
 /* --------------- STATUS --------------- */
@@ -1227,7 +1231,7 @@ static int tc358743_g_register(struct v4l2_subdev *sd,
 
         reg->size = tc358743_get_reg_size(reg->reg);
 
-        i2c_rd(sd, reg->reg, (u8 *)&reg->val, reg->size);
+        reg->val = i2c_rdreg(sd, reg->reg, reg->size);
 
         return 0;
 }
@@ -1253,7 +1257,7 @@ static int tc358743_s_register(struct v4l2_subdev *sd,
             reg->reg == BCAPS)
                 return 0;
 
-        i2c_wr(sd, (u16)reg->reg, (u8 *)&reg->val,
+        i2c_wrreg(sd, (u16)reg->reg, reg->val,
                         tc358743_get_reg_size(reg->reg));
 
         return 0;
@@ -1459,6 +1463,10 @@ static int tc358743_g_mbus_config(struct v4l2_subdev *sd,
 static int tc358743_s_stream(struct v4l2_subdev *sd, int enable)
 {
         enable_stream(sd, enable);
+        if (!enable) {
+                /* Put all lanes in PL-11 state (STOPSTATE) */
+                tc358743_set_csi(sd);
+        }
 
         return 0;
 }
@@ -1951,9 +1959,18 @@ static struct i2c_device_id tc358743_id[] = {
 
 MODULE_DEVICE_TABLE(i2c, tc358743_id);
 
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id tc358743_of_match[] = {
+        { .compatible = "toshiba,tc358743" },
+        {},
+};
+MODULE_DEVICE_TABLE(of, tc358743_of_match);
+#endif
+
 static struct i2c_driver tc358743_driver = {
         .driver = {
                 .name = "tc358743",
+                .of_match_table = of_match_ptr(tc358743_of_match),
         },
         .probe = tc358743_probe,
         .remove = tc358743_remove,
diff --git a/drivers/media/i2c/tvp5150.c b/drivers/media/i2c/tvp5150.c
index 48646a7f3fb0..04e96b3057bb 100644
--- a/drivers/media/i2c/tvp5150.c
+++ b/drivers/media/i2c/tvp5150.c
@@ -865,13 +865,13 @@ static int tvp5150_fill_fmt(struct v4l2_subdev *sd,
         struct v4l2_mbus_framefmt *f;
         struct tvp5150 *decoder = to_tvp5150(sd);
 
-        if (!format || format->pad)
+        if (!format || (format->pad != DEMOD_PAD_VID_OUT))
                 return -EINVAL;
 
         f = &format->format;
 
         f->width = decoder->rect.width;
-        f->height = decoder->rect.height / 2;
+        f->height = decoder->rect.height;
 
         f->code = MEDIA_BUS_FMT_UYVY8_2X8;
         f->field = V4L2_FIELD_ALTERNATE;
diff --git a/drivers/media/media-entity.c b/drivers/media/media-entity.c
index 5640ca29da8c..bc44193efa47 100644
--- a/drivers/media/media-entity.c
+++ b/drivers/media/media-entity.c
@@ -199,12 +199,12 @@ void media_gobj_create(struct media_device *mdev,
 
 void media_gobj_destroy(struct media_gobj *gobj)
 {
-        dev_dbg_obj(__func__, gobj);
-
         /* Do nothing if the object is not linked. */
         if (gobj->mdev == NULL)
                 return;
 
+        dev_dbg_obj(__func__, gobj);
+
         gobj->mdev->topology_version++;
 
         /* Remove the object from mdev list */
diff --git a/drivers/media/pci/bt8xx/bttv-cards.c b/drivers/media/pci/bt8xx/bttv-cards.c
index a1b0f3193bc0..5cc42b426715 100644
--- a/drivers/media/pci/bt8xx/bttv-cards.c
+++ b/drivers/media/pci/bt8xx/bttv-cards.c
@@ -3717,7 +3717,7 @@ static void hauppauge_eeprom(struct bttv *btv)
 {
         struct tveeprom tv;
 
-        tveeprom_hauppauge_analog(&btv->i2c_client, &tv, eeprom_data);
+        tveeprom_hauppauge_analog(&tv, eeprom_data);
         btv->tuner_type = tv.tuner_type;
         btv->has_radio  = tv.has_radio;
 
diff --git a/drivers/media/pci/bt8xx/bttv-driver.c b/drivers/media/pci/bt8xx/bttv-driver.c
index fb4aefbcc8f8..ed319f18ba48 100644
--- a/drivers/media/pci/bt8xx/bttv-driver.c
+++ b/drivers/media/pci/bt8xx/bttv-driver.c
@@ -4043,9 +4043,7 @@ static int bttv_probe(struct pci_dev *dev, const struct pci_device_id *pci_id)
         INIT_LIST_HEAD(&btv->capture);
         INIT_LIST_HEAD(&btv->vcapture);
 
-        init_timer(&btv->timeout);
-        btv->timeout.function = bttv_irq_timeout;
-        btv->timeout.data     = (unsigned long)btv;
+        setup_timer(&btv->timeout, bttv_irq_timeout, (unsigned long)btv);
 
         btv->i2c_rc = -1;
         btv->tuner_type  = UNSET;
diff --git a/drivers/media/pci/cx18/cx18-driver.c b/drivers/media/pci/cx18/cx18-driver.c
index 206db81ef78e..8bce49cdad46 100644
--- a/drivers/media/pci/cx18/cx18-driver.c
+++ b/drivers/media/pci/cx18/cx18-driver.c
@@ -339,7 +339,7 @@ void cx18_read_eeprom(struct cx18 *cx, struct tveeprom *tv)
         case CX18_CARD_HVR_1600_ESMT:
         case CX18_CARD_HVR_1600_SAMSUNG:
         case CX18_CARD_HVR_1600_S5H1411:
-                tveeprom_hauppauge_analog(c, tv, eedata);
+                tveeprom_hauppauge_analog(tv, eedata);
                 break;
         case CX18_CARD_YUAN_MPC718:
         case CX18_CARD_GOTVIEW_PCI_DVD3:
diff --git a/drivers/media/pci/cx18/cx18-streams.c b/drivers/media/pci/cx18/cx18-streams.c
index 7c9381448966..3c45e0071530 100644
--- a/drivers/media/pci/cx18/cx18-streams.c
+++ b/drivers/media/pci/cx18/cx18-streams.c
@@ -282,9 +282,7 @@ static void cx18_stream_init(struct cx18 *cx, int type)
         INIT_WORK(&s->out_work_order, cx18_out_work_handler);
 
         INIT_LIST_HEAD(&s->vb_capture);
-        s->vb_timeout.function = cx18_vb_timeout;
-        s->vb_timeout.data = (unsigned long)s;
-        init_timer(&s->vb_timeout);
+        setup_timer(&s->vb_timeout, cx18_vb_timeout, (unsigned long)s);
         spin_lock_init(&s->vb_lock);
         if (type == CX18_ENC_STREAM_TYPE_YUV) {
                 spin_lock_init(&s->vbuf_q_lock);
diff --git a/drivers/media/pci/cx23885/cx23885-cards.c b/drivers/media/pci/cx23885/cx23885-cards.c
index 0350f13c5a9f..9e39aea85df6 100644
--- a/drivers/media/pci/cx23885/cx23885-cards.c
+++ b/drivers/media/pci/cx23885/cx23885-cards.c
@@ -1143,8 +1143,7 @@ static void hauppauge_eeprom(struct cx23885_dev *dev, u8 *eeprom_data)
 {
         struct tveeprom tv;
 
-        tveeprom_hauppauge_analog(&dev->i2c_bus[0].i2c_client, &tv,
-                eeprom_data);
+        tveeprom_hauppauge_analog(&tv, eeprom_data);
 
         /* Make sure we support the board model */
         switch (tv.model) {
diff --git a/drivers/media/pci/cx88/cx88-cards.c b/drivers/media/pci/cx88/cx88-cards.c
index cdfbde277b8b..73cc7a67a8bc 100644
--- a/drivers/media/pci/cx88/cx88-cards.c
+++ b/drivers/media/pci/cx88/cx88-cards.c
@@ -2854,7 +2854,7 @@ static void hauppauge_eeprom(struct cx88_core *core, u8 *eeprom_data)
 {
         struct tveeprom tv;
 
-        tveeprom_hauppauge_analog(&core->i2c_client, &tv, eeprom_data);
+        tveeprom_hauppauge_analog(&tv, eeprom_data);
         core->board.tuner_type = tv.tuner_type;
         core->tuner_formats = tv.tuner_formats;
         core->board.radio.type = tv.has_radio ? CX88_RADIO : 0;
@@ -3670,7 +3670,7 @@ struct cx88_core *cx88_core_create(struct pci_dev *pci, int nr)
         if (!core)
                 return NULL;
 
-        atomic_inc(&core->refcount);
+        refcount_set(&core->refcount, 1);
         core->pci_bus  = pci->bus->number;
         core->pci_slot = PCI_SLOT(pci->devfn);
         core->pci_irqmask = PCI_INT_RISC_RD_BERRINT | PCI_INT_RISC_WR_BERRINT |
diff --git a/drivers/media/pci/cx88/cx88-core.c b/drivers/media/pci/cx88/cx88-core.c
index 973a9cd4c635..8bfa5b7ed91b 100644
--- a/drivers/media/pci/cx88/cx88-core.c
+++ b/drivers/media/pci/cx88/cx88-core.c
@@ -1052,7 +1052,7 @@ struct cx88_core *cx88_core_get(struct pci_dev *pci)
                         mutex_unlock(&devlist);
                         return NULL;
                 }
-                atomic_inc(&core->refcount);
+                refcount_inc(&core->refcount);
                 mutex_unlock(&devlist);
                 return core;
         }
@@ -1073,7 +1073,7 @@ void cx88_core_put(struct cx88_core *core, struct pci_dev *pci)
         release_mem_region(pci_resource_start(pci, 0),
                            pci_resource_len(pci, 0));
 
-        if (!atomic_dec_and_test(&core->refcount))
+        if (!refcount_dec_and_test(&core->refcount))
                 return;
 
         mutex_lock(&devlist);
diff --git a/drivers/media/pci/cx88/cx88-dvb.c b/drivers/media/pci/cx88/cx88-dvb.c
index ddf90678df34..49a335f4603e 100644
--- a/drivers/media/pci/cx88/cx88-dvb.c
+++ b/drivers/media/pci/cx88/cx88-dvb.c
@@ -306,7 +306,7 @@ static const struct zl10353_config cx88_terratec_cinergy_ht_pci_mkii_config = {
         .if2           = 45600,
 };
 
-static struct mb86a16_config twinhan_vp1027 = {
+static const struct mb86a16_config twinhan_vp1027 = {
         .demod_address  = 0x08,
 };
 
diff --git a/drivers/media/pci/cx88/cx88.h b/drivers/media/pci/cx88/cx88.h
index 115414cf520f..6777926f20f2 100644
--- a/drivers/media/pci/cx88/cx88.h
+++ b/drivers/media/pci/cx88/cx88.h
@@ -24,6 +24,7 @@
 #include <linux/i2c-algo-bit.h>
 #include <linux/videodev2.h>
 #include <linux/kdev_t.h>
+#include <linux/refcount.h>
 
 #include <media/v4l2-device.h>
 #include <media/v4l2-fh.h>
@@ -339,7 +340,7 @@ struct cx8802_dev;
 
 struct cx88_core {
         struct list_head           devlist;
-        atomic_t                   refcount;
+        refcount_t                 refcount;
 
         /* board name */
         int                        nr;
diff --git a/drivers/media/pci/dm1105/dm1105.c b/drivers/media/pci/dm1105/dm1105.c
index a7724b78fbb4..1d41934cfaf5 100644
--- a/drivers/media/pci/dm1105/dm1105.c
+++ b/drivers/media/pci/dm1105/dm1105.c
@@ -815,7 +815,7 @@ static void dm1105_hw_exit(struct dm1105_dev *dev)
         dm1105_dma_unmap(dev);
 }
 
-static struct stv0299_config sharp_z0194a_config = {
+static const struct stv0299_config sharp_z0194a_config = {
         .demod_address = 0x68,
         .inittab = sharp_z0194a_inittab,
         .mclk = 88000000UL,
diff --git a/drivers/media/pci/ivtv/ivtv-driver.c b/drivers/media/pci/ivtv/ivtv-driver.c
index e73c153285f0..e8fa99b6c7b4 100644
--- a/drivers/media/pci/ivtv/ivtv-driver.c
+++ b/drivers/media/pci/ivtv/ivtv-driver.c
@@ -409,7 +409,7 @@ void ivtv_read_eeprom(struct ivtv *itv, struct tveeprom *tv)
 
         itv->i2c_client.addr = 0xA0 >> 1;
         tveeprom_read(&itv->i2c_client, eedata, sizeof(eedata));
-        tveeprom_hauppauge_analog(&itv->i2c_client, tv, eedata);
+        tveeprom_hauppauge_analog(tv, eedata);
 }
 
 static void ivtv_process_eeprom(struct ivtv *itv)
@@ -770,9 +770,8 @@ static int ivtv_init_struct1(struct ivtv *itv)
         init_waitqueue_head(&itv->event_waitq);
         init_waitqueue_head(&itv->vsync_waitq);
         init_waitqueue_head(&itv->dma_waitq);
-        init_timer(&itv->dma_timer);
-        itv->dma_timer.function = ivtv_unfinished_dma;
-        itv->dma_timer.data = (unsigned long)itv;
+        setup_timer(&itv->dma_timer, ivtv_unfinished_dma,
+                    (unsigned long)itv);
 
         itv->cur_dma_stream = -1;
         itv->cur_pio_stream = -1;
diff --git a/drivers/media/pci/ivtv/ivtv-ioctl.c b/drivers/media/pci/ivtv/ivtv-ioctl.c
index f956188f7f19..670462d195b5 100644
--- a/drivers/media/pci/ivtv/ivtv-ioctl.c
+++ b/drivers/media/pci/ivtv/ivtv-ioctl.c
@@ -1506,10 +1506,8 @@ static int ivtv_subscribe_event(struct v4l2_fh *fh, const struct v4l2_event_subs
         case V4L2_EVENT_VSYNC:
         case V4L2_EVENT_EOS:
                 return v4l2_event_subscribe(fh, sub, 0, NULL);
-        case V4L2_EVENT_CTRL:
-                return v4l2_event_subscribe(fh, sub, 0, &v4l2_ctrl_sub_ev_ops);
         default:
-                return -EINVAL;
+                return v4l2_ctrl_subscribe_event(fh, sub);
         }
 }
 
diff --git a/drivers/media/pci/ivtv/ivtv-udma.c b/drivers/media/pci/ivtv/ivtv-udma.c
index 2c9232ef7baa..3b33e87ed73b 100644
--- a/drivers/media/pci/ivtv/ivtv-udma.c
+++ b/drivers/media/pci/ivtv/ivtv-udma.c
@@ -76,7 +76,7 @@ void ivtv_udma_fill_sg_array (struct ivtv_user_dma *dma, u32 buffer_offset, u32
         int i;
         struct scatterlist *sg;
 
-        for (i = 0, sg = dma->SGlist; i < dma->SG_length; i++, sg = sg_next(sg)) {
+        for_each_sg(dma->SGlist, sg, dma->SG_length, i) {
                 dma->SGarray[i].size = cpu_to_le32(sg_dma_len(sg));
                 dma->SGarray[i].src = cpu_to_le32(sg_dma_address(sg));
                 dma->SGarray[i].dst = cpu_to_le32(buffer_offset);
diff --git a/drivers/media/pci/mantis/mantis_vp1034.c b/drivers/media/pci/mantis/mantis_vp1034.c
index 3b1928594b12..e4972ff823c2 100644
--- a/drivers/media/pci/mantis/mantis_vp1034.c
+++ b/drivers/media/pci/mantis/mantis_vp1034.c
@@ -36,7 +36,7 @@
 #include "mantis_vp1034.h"
 #include "mantis_reg.h"
 
-static struct mb86a16_config vp1034_mb86a16_config = {
+static const struct mb86a16_config vp1034_mb86a16_config = {
         .demod_address  = 0x08,
         .set_voltage    = vp1034_set_voltage,
 };
diff --git a/drivers/media/pci/netup_unidvb/netup_unidvb_core.c b/drivers/media/pci/netup_unidvb/netup_unidvb_core.c
index 191bd8299dc3..9444483fb942 100644
--- a/drivers/media/pci/netup_unidvb/netup_unidvb_core.c
+++ b/drivers/media/pci/netup_unidvb/netup_unidvb_core.c
@@ -663,9 +663,8 @@ static int netup_unidvb_dma_init(struct netup_unidvb_dev *ndev, int num)
         spin_lock_init(&dma->lock);
         INIT_WORK(&dma->work, netup_unidvb_dma_worker);
         INIT_LIST_HEAD(&dma->free_buffers);
-        dma->timeout.function = netup_unidvb_dma_timeout;
-        dma->timeout.data = (unsigned long)dma;
-        init_timer(&dma->timeout);
+        setup_timer(&dma->timeout, netup_unidvb_dma_timeout,
+                    (unsigned long)dma);
         dma->ring_buffer_size = ndev->dma_size / 2;
         dma->addr_virt = ndev->dma_virt + dma->ring_buffer_size * num;
         dma->addr_phys = (dma_addr_t)((u64)ndev->dma_phys +
diff --git a/drivers/media/pci/saa7134/saa7134-cards.c b/drivers/media/pci/saa7134/saa7134-cards.c
index 321253827997..f79380faf499 100644
--- a/drivers/media/pci/saa7134/saa7134-cards.c
+++ b/drivers/media/pci/saa7134/saa7134-cards.c
@@ -7319,7 +7319,7 @@ static void hauppauge_eeprom(struct saa7134_dev *dev, u8 *eeprom_data)
 {
         struct tveeprom tv;
 
-        tveeprom_hauppauge_analog(&dev->i2c_client, &tv, eeprom_data);
+        tveeprom_hauppauge_analog(&tv, eeprom_data);
 
         /* Make sure we support the board model */
         switch (tv.model) {
diff --git a/drivers/media/pci/saa7134/saa7134-dvb.c b/drivers/media/pci/saa7134/saa7134-dvb.c
index efdece5ab11c..731dee0a66e7 100644
--- a/drivers/media/pci/saa7134/saa7134-dvb.c
+++ b/drivers/media/pci/saa7134/saa7134-dvb.c
@@ -1042,11 +1042,11 @@ static int md8800_set_high_voltage2(struct dvb_frontend *fe, long arg)
  * nxt200x based ATSC cards, helper functions
  */
 
-static struct nxt200x_config avertvhda180 = {
+static const struct nxt200x_config avertvhda180 = {
         .demod_address    = 0x0a,
 };
 
-static struct nxt200x_config kworldatsc110 = {
+static const struct nxt200x_config kworldatsc110 = {
         .demod_address    = 0x0a,
 };
 
diff --git a/drivers/media/pci/saa7134/saa7134-ts.c b/drivers/media/pci/saa7134/saa7134-ts.c
index 578e03f8c041..7414878af9e0 100644
--- a/drivers/media/pci/saa7134/saa7134-ts.c
+++ b/drivers/media/pci/saa7134/saa7134-ts.c
@@ -223,9 +223,8 @@ int saa7134_ts_init1(struct saa7134_dev *dev)
         dev->ts.nr_packets = ts_nr_packets;
 
         INIT_LIST_HEAD(&dev->ts_q.queue);
-        init_timer(&dev->ts_q.timeout);
-        dev->ts_q.timeout.function = saa7134_buffer_timeout;
-        dev->ts_q.timeout.data     = (unsigned long)(&dev->ts_q);
+        setup_timer(&dev->ts_q.timeout, saa7134_buffer_timeout,
+                    (unsigned long)(&dev->ts_q));
         dev->ts_q.dev              = dev;
         dev->ts_q.need_two         = 1;
         dev->ts_started            = 0;
diff --git a/drivers/media/pci/saa7134/saa7134-vbi.c b/drivers/media/pci/saa7134/saa7134-vbi.c
index 46193370e41a..bcad9b2d9bb3 100644
--- a/drivers/media/pci/saa7134/saa7134-vbi.c
+++ b/drivers/media/pci/saa7134/saa7134-vbi.c
@@ -181,9 +181,8 @@ struct vb2_ops saa7134_vbi_qops = {
 int saa7134_vbi_init1(struct saa7134_dev *dev)
 {
         INIT_LIST_HEAD(&dev->vbi_q.queue);
-        init_timer(&dev->vbi_q.timeout);
-        dev->vbi_q.timeout.function = saa7134_buffer_timeout;
-        dev->vbi_q.timeout.data     = (unsigned long)(&dev->vbi_q);
+        setup_timer(&dev->vbi_q.timeout, saa7134_buffer_timeout,
+                    (unsigned long)(&dev->vbi_q));
         dev->vbi_q.dev              = dev;
 
         if (vbibufs < 2)
diff --git a/drivers/media/pci/saa7134/saa7134-video.c b/drivers/media/pci/saa7134/saa7134-video.c
index 4b1c4327f112..51d42bbf969e 100644
--- a/drivers/media/pci/saa7134/saa7134-video.c
+++ b/drivers/media/pci/saa7134/saa7134-video.c
@@ -2145,9 +2145,8 @@ int saa7134_video_init1(struct saa7134_dev *dev)
         dev->automute       = 0;
 
         INIT_LIST_HEAD(&dev->video_q.queue);
-        init_timer(&dev->video_q.timeout);
-        dev->video_q.timeout.function = saa7134_buffer_timeout;
-        dev->video_q.timeout.data     = (unsigned long)(&dev->video_q);
+        setup_timer(&dev->video_q.timeout, saa7134_buffer_timeout,
+                    (unsigned long)(&dev->video_q));
         dev->video_q.dev              = dev;
         dev->fmt = format_by_fourcc(V4L2_PIX_FMT_BGR24);
         dev->width    = 720;
diff --git a/drivers/media/pci/saa7164/saa7164-cards.c b/drivers/media/pci/saa7164/saa7164-cards.c
index 0e1cd7e153ca..3af16062e79d 100644
--- a/drivers/media/pci/saa7164/saa7164-cards.c
+++ b/drivers/media/pci/saa7164/saa7164-cards.c
@@ -780,9 +780,7 @@ static void hauppauge_eeprom(struct saa7164_dev *dev, u8 *eeprom_data)
 {
         struct tveeprom tv;
 
-        /* TODO: Assumption: eeprom on bus 0 */
-        tveeprom_hauppauge_analog(&dev->i2c_bus[0].i2c_client, &tv,
-                eeprom_data);
+        tveeprom_hauppauge_analog(&tv, eeprom_data);
 
         /* Make sure we support the board model */
         switch (tv.model) {
diff --git a/drivers/media/pci/saa7164/saa7164-cmd.c b/drivers/media/pci/saa7164/saa7164-cmd.c
index f55c177fd1e4..175015ca79f2 100644
--- a/drivers/media/pci/saa7164/saa7164-cmd.c
+++ b/drivers/media/pci/saa7164/saa7164-cmd.c
@@ -130,14 +130,13 @@ int saa7164_irq_dequeue(struct saa7164_dev *dev)
  * -bus/c running buffer. */
 static int saa7164_cmd_dequeue(struct saa7164_dev *dev)
 {
-        int loop = 1;
         int ret;
         u32 timeout;
         wait_queue_head_t *q = NULL;
         u8 tmp[512];
         dprintk(DBGLVL_CMD, "%s()\n", __func__);
 
-        while (loop) {
+        while (true) {
 
                 struct tmComResInfo tRsp = { 0, 0, 0, 0, 0, 0 };
                 ret = saa7164_bus_get(dev, &tRsp, NULL, 1);
@@ -178,8 +177,6 @@ static int saa7164_cmd_dequeue(struct saa7164_dev *dev)
                 wake_up(q);
                 return SAA_OK;
         }
-
-        return SAA_OK;
 }
 
 static int saa7164_cmd_set(struct saa7164_dev *dev, struct tmComResInfo *msg,
diff --git a/drivers/media/pci/solo6x10/solo6x10-v4l2-enc.c b/drivers/media/pci/solo6x10/solo6x10-v4l2-enc.c
index 25a2137ab799..25f9f2ebff1d 100644
--- a/drivers/media/pci/solo6x10/solo6x10-v4l2-enc.c
+++ b/drivers/media/pci/solo6x10/solo6x10-v4l2-enc.c
@@ -1140,14 +1140,13 @@ static int solo_subscribe_event(struct v4l2_fh *fh,
 {
 
         switch (sub->type) {
-        case V4L2_EVENT_CTRL:
-                return v4l2_ctrl_subscribe_event(fh, sub);
         case V4L2_EVENT_MOTION_DET:
                 /* Allow for up to 30 events (1 second for NTSC) to be
                  * stored. */
                 return v4l2_event_subscribe(fh, sub, 30, NULL);
+        default:
+                return v4l2_ctrl_subscribe_event(fh, sub);
         }
-        return -EINVAL;
 }
 
 static const struct v4l2_file_operations solo_enc_fops = {
diff --git a/drivers/media/pci/solo6x10/solo6x10-v4l2.c b/drivers/media/pci/solo6x10/solo6x10-v4l2.c
index 896bec6627aa..3266fc21825f 100644
--- a/drivers/media/pci/solo6x10/solo6x10-v4l2.c
+++ b/drivers/media/pci/solo6x10/solo6x10-v4l2.c
@@ -341,6 +341,17 @@ static void solo_stop_streaming(struct vb2_queue *q)
         struct solo_dev *solo_dev = vb2_get_drv_priv(q);
 
         solo_stop_thread(solo_dev);
+
+        spin_lock(&solo_dev->slock);
+        while (!list_empty(&solo_dev->vidq_active)) {
+                struct solo_vb2_buf *buf = list_entry(
+                                solo_dev->vidq_active.next,
+                                struct solo_vb2_buf, list);
+
+                list_del(&buf->list);
+                vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+        }
+        spin_unlock(&solo_dev->slock);
         INIT_LIST_HEAD(&solo_dev->vidq_active);
 }
 
diff --git a/drivers/media/pci/ttpci/av7110_ir.c b/drivers/media/pci/ttpci/av7110_ir.c
index 10e28f067b45..ca05198de2c2 100644
--- a/drivers/media/pci/ttpci/av7110_ir.c
+++ b/drivers/media/pci/ttpci/av7110_ir.c
@@ -333,9 +333,8 @@ int av7110_ir_init(struct av7110 *av7110)
         av_list[av_cnt++] = av7110;
         av7110_check_ir_config(av7110, true);
 
-        init_timer(&av7110->ir.keyup_timer);
-        av7110->ir.keyup_timer.function = av7110_emit_keyup;
-        av7110->ir.keyup_timer.data = (unsigned long) &av7110->ir;
+        setup_timer(&av7110->ir.keyup_timer, av7110_emit_keyup,
+                    (unsigned long)&av7110->ir);
 
         input_dev = input_allocate_device();
         if (!input_dev)
diff --git a/drivers/media/pci/ttpci/budget-av.c b/drivers/media/pci/ttpci/budget-av.c
index 19f07d4aba6a..dc7be8fac9a3 100644
--- a/drivers/media/pci/ttpci/budget-av.c
+++ b/drivers/media/pci/ttpci/budget-av.c
@@ -577,7 +577,7 @@ static u8 typhoon_cinergy1200s_inittab[] = {
         0xff, 0xff
 };
 
-static struct stv0299_config typhoon_config = {
+static const struct stv0299_config typhoon_config = {
         .demod_address = 0x68,
         .inittab = typhoon_cinergy1200s_inittab,
         .mclk = 88000000UL,
@@ -590,7 +590,7 @@ static struct stv0299_config typhoon_config = {
 };
 
 
-static struct stv0299_config cinergy_1200s_config = {
+static const struct stv0299_config cinergy_1200s_config = {
         .demod_address = 0x68,
         .inittab = typhoon_cinergy1200s_inittab,
         .mclk = 88000000UL,
@@ -602,7 +602,7 @@ static struct stv0299_config cinergy_1200s_config = {
         .set_symbol_rate = philips_su1278_ty_ci_set_symbol_rate,
 };
 
-static struct stv0299_config cinergy_1200s_1894_0010_config = {
+static const struct stv0299_config cinergy_1200s_1894_0010_config = {
         .demod_address = 0x68,
         .inittab = typhoon_cinergy1200s_inittab,
         .mclk = 88000000UL,
@@ -876,7 +876,7 @@ static int philips_sd1878_ci_set_symbol_rate(struct dvb_frontend *fe,
         return 0;
 }
 
-static struct stv0299_config philips_sd1878_config = {
+static const struct stv0299_config philips_sd1878_config = {
         .demod_address = 0x68,
      .inittab = philips_sd1878_inittab,
         .mclk = 88000000UL,
diff --git a/drivers/media/pci/ttpci/budget-ci.c b/drivers/media/pci/ttpci/budget-ci.c
index 68355484ba7d..11b9227307bf 100644
--- a/drivers/media/pci/ttpci/budget-ci.c
+++ b/drivers/media/pci/ttpci/budget-ci.c
@@ -693,7 +693,7 @@ static int philips_su1278_tt_tuner_set_params(struct dvb_frontend *fe)
         return 0;
 }
 
-static struct stv0299_config philips_su1278_tt_config = {
+static const struct stv0299_config philips_su1278_tt_config = {
 
         .demod_address = 0x68,
         .inittab = philips_su1278_tt_inittab,
diff --git a/drivers/media/pci/ttpci/budget.c b/drivers/media/pci/ttpci/budget.c
index 5f17e1c9a207..81fe35cedd10 100644
--- a/drivers/media/pci/ttpci/budget.c
+++ b/drivers/media/pci/ttpci/budget.c
@@ -397,7 +397,7 @@ static struct tda10086_config tda10086_config = {
         .xtal_freq = TDA10086_XTAL_16M,
 };
 
-static struct stv0299_config alps_bsru6_config_activy = {
+static const struct stv0299_config alps_bsru6_config_activy = {
         .demod_address = 0x68,
         .inittab = alps_bsru6_inittab,
         .mclk = 88000000UL,
@@ -407,7 +407,7 @@ static struct stv0299_config alps_bsru6_config_activy = {
         .set_symbol_rate = alps_bsru6_set_symbol_rate,
 };
 
-static struct stv0299_config alps_bsbe1_config_activy = {
+static const struct stv0299_config alps_bsbe1_config_activy = {
         .demod_address = 0x68,
         .inittab = alps_bsbe1_inittab,
         .mclk = 88000000UL,
diff --git a/drivers/media/pci/tw5864/tw5864-video.c b/drivers/media/pci/tw5864/tw5864-video.c
index 9421216bb942..2a044be729da 100644
--- a/drivers/media/pci/tw5864/tw5864-video.c
+++ b/drivers/media/pci/tw5864/tw5864-video.c
@@ -664,15 +664,14 @@ static int tw5864_subscribe_event(struct v4l2_fh *fh,
                                   const struct v4l2_event_subscription *sub)
 {
         switch (sub->type) {
-        case V4L2_EVENT_CTRL:
-                return v4l2_ctrl_subscribe_event(fh, sub);
         case V4L2_EVENT_MOTION_DET:
                 /*
                  * Allow for up to 30 events (1 second for NTSC) to be stored.
                  */
                 return v4l2_event_subscribe(fh, sub, 30, NULL);
+        default:
+                return v4l2_ctrl_subscribe_event(fh, sub);
         }
-        return -EINVAL;
 }
 
 static void tw5864_frame_interval_set(struct tw5864_input *input)
@@ -717,6 +716,8 @@ static void tw5864_frame_interval_set(struct tw5864_input *input)
 static int tw5864_frameinterval_get(struct tw5864_input *input,
                                     struct v4l2_fract *frameinterval)
 {
+        struct tw5864_dev *dev = input->root;
+
         switch (input->std) {
         case STD_NTSC:
                 frameinterval->numerator = 1001;
@@ -728,8 +729,8 @@ static int tw5864_frameinterval_get(struct tw5864_input *input,
                 frameinterval->denominator = 25;
                 break;
         default:
-                WARN(1, "tw5864_frameinterval_get requested for unknown std %d\n",
-                     input->std);
+                dev_warn(&dev->pci->dev, "tw5864_frameinterval_get requested for unknown std %d\n",
+                         input->std);
                 return -EINVAL;
         }
 
diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index c9106e105bab..ac026ee1ca07 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -151,7 +151,7 @@ if V4L_MEM2MEM_DRIVERS
 
 config VIDEO_CODA
         tristate "Chips&Media Coda multi-standard codec IP"
-        depends on VIDEO_DEV && VIDEO_V4L2 && ARCH_MXC
+        depends on VIDEO_DEV && VIDEO_V4L2 && (ARCH_MXC || COMPILE_TEST)
         depends on HAS_DMA
         select SRAM
         select VIDEOBUF2_DMA_CONTIG
@@ -165,6 +165,21 @@ config VIDEO_CODA
 config VIDEO_IMX_VDOA
         def_tristate VIDEO_CODA if SOC_IMX6Q || COMPILE_TEST
 
+config VIDEO_MEDIATEK_JPEG
+        tristate "Mediatek JPEG Codec driver"
+        depends on MTK_IOMMU_V1 || COMPILE_TEST
+        depends on VIDEO_DEV && VIDEO_V4L2
+        depends on ARCH_MEDIATEK || COMPILE_TEST
+        depends on HAS_DMA
+        select VIDEOBUF2_DMA_CONTIG
+        select V4L2_MEM2MEM_DEV
+        ---help---
+          Mediatek jpeg codec driver provides HW capability to decode
+          JPEG format
+
+          To compile this driver as a module, choose M here: the
+          module will be called mtk-jpeg
+
 config VIDEO_MEDIATEK_VPU
         tristate "Mediatek Video Processor Unit"
         depends on VIDEO_DEV && VIDEO_V4L2 && HAS_DMA
@@ -405,6 +420,7 @@ config VIDEO_RENESAS_VSP1
         depends on (ARCH_RENESAS && OF) || COMPILE_TEST
         depends on (!ARM64 && !VIDEO_RENESAS_FCP) || VIDEO_RENESAS_FCP
         select VIDEOBUF2_DMA_CONTIG
+        select VIDEOBUF2_VMALLOC
         ---help---
           This is a V4L2 driver for the Renesas VSP1 video processing engine.
 
@@ -451,6 +467,8 @@ menuconfig V4L_TEST_DRIVERS
 
 if V4L_TEST_DRIVERS
 
+source "drivers/media/platform/vimc/Kconfig"
+
 source "drivers/media/platform/vivid/Kconfig"
 
 config VIDEO_VIM2M
@@ -474,3 +492,31 @@ menuconfig DVB_PLATFORM_DRIVERS
 if DVB_PLATFORM_DRIVERS
 source "drivers/media/platform/sti/c8sectpfe/Kconfig"
 endif #DVB_PLATFORM_DRIVERS
+
+menuconfig CEC_PLATFORM_DRIVERS
+        bool "CEC platform devices"
+        depends on MEDIA_CEC_SUPPORT
+
+if CEC_PLATFORM_DRIVERS
+
+config VIDEO_SAMSUNG_S5P_CEC
+       tristate "Samsung S5P CEC driver"
+       depends on CEC_CORE && (PLAT_S5P || ARCH_EXYNOS || COMPILE_TEST)
+       select MEDIA_CEC_NOTIFIER
+       ---help---
+         This is a driver for Samsung S5P HDMI CEC interface. It uses the
+         generic CEC framework interface.
+         CEC bus is present in the HDMI connector and enables communication
+         between compatible devices.
+
+config VIDEO_STI_HDMI_CEC
+       tristate "STMicroelectronics STiH4xx HDMI CEC driver"
+       depends on CEC_CORE && (ARCH_STI || COMPILE_TEST)
+       select MEDIA_CEC_NOTIFIER
+       ---help---
+         This is a driver for STIH4xx HDMI CEC interface. It uses the
+         generic CEC framework interface.
+         CEC bus is present in the HDMI connector and enables communication
+         between compatible devices.
+
+endif #CEC_PLATFORM_DRIVERS
diff --git a/drivers/media/platform/Makefile b/drivers/media/platform/Makefile
index 349ddf6a69da..63303d63c64c 100644
--- a/drivers/media/platform/Makefile
+++ b/drivers/media/platform/Makefile
@@ -13,6 +13,7 @@ obj-$(CONFIG_VIDEO_PXA27x)	+= pxa_camera.o
 
 obj-$(CONFIG_VIDEO_VIU) += fsl-viu.o
 
+obj-$(CONFIG_VIDEO_VIMC)                += vimc/
 obj-$(CONFIG_VIDEO_VIVID)               += vivid/
 obj-$(CONFIG_VIDEO_VIM2M)               += vim2m.o
 
@@ -33,11 +34,13 @@ obj-$(CONFIG_VIDEO_SAMSUNG_S5P_JPEG)	+= s5p-jpeg/
 obj-$(CONFIG_VIDEO_SAMSUNG_S5P_MFC)     += s5p-mfc/
 
 obj-$(CONFIG_VIDEO_SAMSUNG_S5P_G2D)     += s5p-g2d/
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_CEC)     += s5p-cec/
 obj-$(CONFIG_VIDEO_SAMSUNG_EXYNOS_GSC)  += exynos-gsc/
 
 obj-$(CONFIG_VIDEO_STI_BDISP)           += sti/bdisp/
 obj-$(CONFIG_VIDEO_STI_HVA)             += sti/hva/
 obj-$(CONFIG_DVB_C8SECTPFE)             += sti/c8sectpfe/
+obj-$(CONFIG_VIDEO_STI_HDMI_CEC)        += sti/cec/
 
 obj-$(CONFIG_VIDEO_STI_DELTA)           += sti/delta/
 
@@ -63,6 +66,7 @@ obj-$(CONFIG_VIDEO_XILINX)		+= xilinx/
 obj-$(CONFIG_VIDEO_RCAR_VIN)            += rcar-vin/
 
 obj-$(CONFIG_VIDEO_ATMEL_ISC)           += atmel/
+obj-$(CONFIG_VIDEO_ATMEL_ISI)           += atmel/
 
 ccflags-y += -I$(srctree)/drivers/media/i2c
 
@@ -71,3 +75,5 @@ obj-$(CONFIG_VIDEO_MEDIATEK_VPU)	+= mtk-vpu/
 obj-$(CONFIG_VIDEO_MEDIATEK_VCODEC)     += mtk-vcodec/
 
 obj-$(CONFIG_VIDEO_MEDIATEK_MDP)        += mtk-mdp/
+
+obj-$(CONFIG_VIDEO_MEDIATEK_JPEG)       += mtk-jpeg/
diff --git a/drivers/media/platform/atmel/Kconfig b/drivers/media/platform/atmel/Kconfig
index 867dca22a473..9bd0f19b127f 100644
--- a/drivers/media/platform/atmel/Kconfig
+++ b/drivers/media/platform/atmel/Kconfig
@@ -6,4 +6,13 @@ config VIDEO_ATMEL_ISC
         select REGMAP_MMIO
         help
            This module makes the ATMEL Image Sensor Controller available
-           as a v4l2 device.
\ No newline at end of file
+           as a v4l2 device.
+
+config VIDEO_ATMEL_ISI
+        tristate "ATMEL Image Sensor Interface (ISI) support"
+        depends on VIDEO_V4L2 && OF && HAS_DMA
+        depends on ARCH_AT91 || COMPILE_TEST
+        select VIDEOBUF2_DMA_CONTIG
+        ---help---
+          This module makes the ATMEL Image Sensor Interface available
+          as a v4l2 device.
diff --git a/drivers/media/platform/atmel/Makefile b/drivers/media/platform/atmel/Makefile
index 9d7c999d434d..27000d099a5e 100644
--- a/drivers/media/platform/atmel/Makefile
+++ b/drivers/media/platform/atmel/Makefile
@@ -1 +1,2 @@
 obj-$(CONFIG_VIDEO_ATMEL_ISC) += atmel-isc.o
+obj-$(CONFIG_VIDEO_ATMEL_ISI) += atmel-isi.o
diff --git a/drivers/media/platform/atmel/atmel-isc-regs.h b/drivers/media/platform/atmel/atmel-isc-regs.h
index 00c449717cde..6936ac467609 100644
--- a/drivers/media/platform/atmel/atmel-isc-regs.h
+++ b/drivers/media/platform/atmel/atmel-isc-regs.h
@@ -65,6 +65,7 @@
 #define ISC_INTSR       0x00000034
 
 #define ISC_INT_DDONE           BIT(8)
+#define ISC_INT_HISDONE         BIT(12)
 
 /* ISC White Balance Control Register */
 #define ISC_WB_CTRL     0x00000058
@@ -72,30 +73,98 @@
 /* ISC White Balance Configuration Register */
 #define ISC_WB_CFG      0x0000005c
 
+/* ISC White Balance Offset for R, GR Register */
+#define ISC_WB_O_RGR    0x00000060
+
+/* ISC White Balance Offset for B, GB Register */
+#define ISC_WB_O_BGR    0x00000064
+
+/* ISC White Balance Gain for R, GR Register */
+#define ISC_WB_G_RGR    0x00000068
+
+/* ISC White Balance Gain for B, GB Register */
+#define ISC_WB_G_BGR    0x0000006c
+
 /* ISC Color Filter Array Control Register */
 #define ISC_CFA_CTRL    0x00000070
 
 /* ISC Color Filter Array Configuration Register */
 #define ISC_CFA_CFG     0x00000074
+#define ISC_CFA_CFG_EITPOL      BIT(4)
 
 #define ISC_BAY_CFG_GRGR        0x0
 #define ISC_BAY_CFG_RGRG        0x1
 #define ISC_BAY_CFG_GBGB        0x2
 #define ISC_BAY_CFG_BGBG        0x3
-#define ISC_BAY_CFG_MASK        GENMASK(1, 0)
 
 /* ISC Color Correction Control Register */
 #define ISC_CC_CTRL     0x00000078
 
+/* ISC Color Correction RR RG Register */
+#define ISC_CC_RR_RG    0x0000007c
+
+/* ISC Color Correction RB OR Register */
+#define ISC_CC_RB_OR    0x00000080
+
+/* ISC Color Correction GR GG Register */
+#define ISC_CC_GR_GG    0x00000084
+
+/* ISC Color Correction GB OG Register */
+#define ISC_CC_GB_OG    0x00000088
+
+/* ISC Color Correction BR BG Register */
+#define ISC_CC_BR_BG    0x0000008c
+
+/* ISC Color Correction BB OB Register */
+#define ISC_CC_BB_OB    0x00000090
+
 /* ISC Gamma Correction Control Register */
 #define ISC_GAM_CTRL    0x00000094
 
+/* ISC_Gamma Correction Blue Entry Register */
+#define ISC_GAM_BENTRY  0x00000098
+
+/* ISC_Gamma Correction Green Entry Register */
+#define ISC_GAM_GENTRY  0x00000198
+
+/* ISC_Gamma Correction Green Entry Register */
+#define ISC_GAM_RENTRY  0x00000298
+
 /* Color Space Conversion Control Register */
 #define ISC_CSC_CTRL    0x00000398
 
+/* Color Space Conversion YR YG Register */
+#define ISC_CSC_YR_YG   0x0000039c
+
+/* Color Space Conversion YB OY Register */
+#define ISC_CSC_YB_OY   0x000003a0
+
+/* Color Space Conversion CBR CBG Register */
+#define ISC_CSC_CBR_CBG 0x000003a4
+
+/* Color Space Conversion CBB OCB Register */
+#define ISC_CSC_CBB_OCB 0x000003a8
+
+/* Color Space Conversion CRR CRG Register */
+#define ISC_CSC_CRR_CRG 0x000003ac
+
+/* Color Space Conversion CRB OCR Register */
+#define ISC_CSC_CRB_OCR 0x000003b0
+
 /* Contrast And Brightness Control Register */
 #define ISC_CBC_CTRL    0x000003b4
 
+/* Contrast And Brightness Configuration Register */
+#define ISC_CBC_CFG     0x000003b8
+
+/* Brightness Register */
+#define ISC_CBC_BRIGHT  0x000003bc
+#define ISC_CBC_BRIGHT_MASK     GENMASK(10, 0)
+
+/* Contrast Register */
+#define ISC_CBC_CONTRAST        0x000003c0
+#define ISC_CBC_CONTRAST_MASK   GENMASK(11, 0)
+
 /* Subsampling 4:4:4 to 4:2:2 Control Register */
 #define ISC_SUB422_CTRL 0x000003c4
 
@@ -120,6 +189,27 @@
 #define ISC_RLP_CFG_MODE_YYCC_LIMITED   0xc
 #define ISC_RLP_CFG_MODE_MASK           GENMASK(3, 0)
 
+/* Histogram Control Register */
+#define ISC_HIS_CTRL    0x000003d4
+
+#define ISC_HIS_CTRL_EN                 BIT(0)
+#define ISC_HIS_CTRL_DIS                0x0
+
+/* Histogram Configuration Register */
+#define ISC_HIS_CFG     0x000003d8
+
+#define ISC_HIS_CFG_MODE_GR             0x0
+#define ISC_HIS_CFG_MODE_R              0x1
+#define ISC_HIS_CFG_MODE_GB             0x2
+#define ISC_HIS_CFG_MODE_B              0x3
+#define ISC_HIS_CFG_MODE_Y              0x4
+#define ISC_HIS_CFG_MODE_RAW            0x5
+#define ISC_HIS_CFG_MODE_YCCIR656       0x6
+
+#define ISC_HIS_CFG_BAYSEL_SHIFT        4
+
+#define ISC_HIS_CFG_RAR                 BIT(8)
+
 /* DMA Configuration Register */
 #define ISC_DCFG        0x000003e0
 #define ISC_DCFG_IMODE_PACKED8          0x0
@@ -159,7 +249,13 @@
 /* DMA Address 0 Register */
 #define ISC_DAD0        0x000003ec
 
-/* DMA Stride 0 Register */
-#define ISC_DST0        0x000003f0
+/* DMA Address 1 Register */
+#define ISC_DAD1        0x000003f4
+
+/* DMA Address 2 Register */
+#define ISC_DAD2        0x000003fc
+
+/* Histogram Entry */
+#define ISC_HIS_ENTRY   0x00000410
 
 #endif
diff --git a/drivers/media/platform/atmel/atmel-isc.c b/drivers/media/platform/atmel/atmel-isc.c
index fa68fe912c95..c4b2115559a5 100644
--- a/drivers/media/platform/atmel/atmel-isc.c
+++ b/drivers/media/platform/atmel/atmel-isc.c
@@ -29,6 +29,7 @@
 #include <linux/clk-provider.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
+#include <linux/math64.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
@@ -36,7 +37,9 @@
 #include <linux/regmap.h>
 #include <linux/videodev2.h>
 
+#include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
 #include <media/v4l2-image-sizes.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-of.h>
@@ -89,10 +92,12 @@ struct isc_subdev_entity {
  * struct isc_format - ISC media bus format information
  * @fourcc:             Fourcc code for this format
  * @mbus_code:          V4L2 media bus format code.
- * @bpp:                Bytes per pixel (when stored in memory)
+ * @bpp:                Bits per pixel (when stored in memory)
  * @reg_bps:            reg value for bits per sample
  *                      (when transferred over a bus)
- * @support:            Indicates format supported by subdev
+ * @pipeline:           pipeline switch
+ * @sd_support:         Subdev supports this format
+ * @isc_support:        ISC can convert raw format to this format
  */
 struct isc_format {
         u32     fourcc;
@@ -100,11 +105,42 @@ struct isc_format {
         u8      bpp;
 
         u32     reg_bps;
+        u32     reg_bay_cfg;
         u32     reg_rlp_mode;
         u32     reg_dcfg_imode;
         u32     reg_dctrl_dview;
 
-        bool    support;
+        u32     pipeline;
+
+        bool    sd_support;
+        bool    isc_support;
+};
+
+
+#define HIST_ENTRIES            512
+#define HIST_BAYER              (ISC_HIS_CFG_MODE_B + 1)
+
+enum{
+        HIST_INIT = 0,
+        HIST_ENABLED,
+        HIST_DISABLED,
+};
+
+struct isc_ctrls {
+        struct v4l2_ctrl_handler handler;
+
+        u32 brightness;
+        u32 contrast;
+        u8 gamma_index;
+        u8 awb;
+
+        u32 r_gain;
+        u32 b_gain;
+
+        u32 hist_entry[HIST_ENTRIES];
+        u32 hist_count[HIST_BAYER];
+        u8 hist_id;
+        u8 hist_stat;
 };
 
 #define ISC_PIPE_LINE_NODE_NUM  11
@@ -131,6 +167,10 @@ struct isc_device {
         struct isc_format       **user_formats;
         unsigned int            num_user_formats;
         const struct isc_format *current_fmt;
+        const struct isc_format *raw_fmt;
+
+        struct isc_ctrls        ctrls;
+        struct work_struct      awb_work;
 
         struct mutex            lock;
 
@@ -140,51 +180,134 @@ struct isc_device {
         struct list_head                subdev_entities;
 };
 
+#define RAW_FMT_IND_START    0
+#define RAW_FMT_IND_END      11
+#define ISC_FMT_IND_START    12
+#define ISC_FMT_IND_END      14
+
 static struct isc_format isc_formats[] = {
-        { V4L2_PIX_FMT_SBGGR8, MEDIA_BUS_FMT_SBGGR8_1X8,
-          1, ISC_PFE_CFG0_BPS_EIGHT, ISC_RLP_CFG_MODE_DAT8,
-          ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, false },
-        { V4L2_PIX_FMT_SGBRG8, MEDIA_BUS_FMT_SGBRG8_1X8,
-          1, ISC_PFE_CFG0_BPS_EIGHT, ISC_RLP_CFG_MODE_DAT8,
-          ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, false },
-        { V4L2_PIX_FMT_SGRBG8, MEDIA_BUS_FMT_SGRBG8_1X8,
-          1, ISC_PFE_CFG0_BPS_EIGHT, ISC_RLP_CFG_MODE_DAT8,
-          ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, false },
-        { V4L2_PIX_FMT_SRGGB8, MEDIA_BUS_FMT_SRGGB8_1X8,
-          1, ISC_PFE_CFG0_BPS_EIGHT, ISC_RLP_CFG_MODE_DAT8,
-          ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, false },
-
-        { V4L2_PIX_FMT_SBGGR10, MEDIA_BUS_FMT_SBGGR10_1X10,
-          2, ISC_PFG_CFG0_BPS_TEN, ISC_RLP_CFG_MODE_DAT10,
-          ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
-        { V4L2_PIX_FMT_SGBRG10, MEDIA_BUS_FMT_SGBRG10_1X10,
-          2, ISC_PFG_CFG0_BPS_TEN, ISC_RLP_CFG_MODE_DAT10,
-          ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
-        { V4L2_PIX_FMT_SGRBG10, MEDIA_BUS_FMT_SGRBG10_1X10,
-          2, ISC_PFG_CFG0_BPS_TEN, ISC_RLP_CFG_MODE_DAT10,
-          ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
-        { V4L2_PIX_FMT_SRGGB10, MEDIA_BUS_FMT_SRGGB10_1X10,
-          2, ISC_PFG_CFG0_BPS_TEN, ISC_RLP_CFG_MODE_DAT10,
-          ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
-
-        { V4L2_PIX_FMT_SBGGR12, MEDIA_BUS_FMT_SBGGR12_1X12,
-          2, ISC_PFG_CFG0_BPS_TWELVE, ISC_RLP_CFG_MODE_DAT12,
-          ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
-        { V4L2_PIX_FMT_SGBRG12, MEDIA_BUS_FMT_SGBRG12_1X12,
-          2, ISC_PFG_CFG0_BPS_TWELVE, ISC_RLP_CFG_MODE_DAT12,
-          ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
-        { V4L2_PIX_FMT_SGRBG12, MEDIA_BUS_FMT_SGRBG12_1X12,
-          2, ISC_PFG_CFG0_BPS_TWELVE, ISC_RLP_CFG_MODE_DAT12,
-          ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
-        { V4L2_PIX_FMT_SRGGB12, MEDIA_BUS_FMT_SRGGB12_1X12,
-          2, ISC_PFG_CFG0_BPS_TWELVE, ISC_RLP_CFG_MODE_DAT12,
-          ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
-
-        { V4L2_PIX_FMT_YUYV, MEDIA_BUS_FMT_YUYV8_2X8,
-          2, ISC_PFE_CFG0_BPS_EIGHT, ISC_RLP_CFG_MODE_DAT8,
-          ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, false },
+        { V4L2_PIX_FMT_SBGGR8, MEDIA_BUS_FMT_SBGGR8_1X8, 8,
+          ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_BGBG, ISC_RLP_CFG_MODE_DAT8,
+          ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, 0x0,
+          false, false },
+        { V4L2_PIX_FMT_SGBRG8, MEDIA_BUS_FMT_SGBRG8_1X8, 8,
+          ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_GBGB, ISC_RLP_CFG_MODE_DAT8,
+          ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, 0x0,
+          false, false },
+        { V4L2_PIX_FMT_SGRBG8, MEDIA_BUS_FMT_SGRBG8_1X8, 8,
+          ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_GRGR, ISC_RLP_CFG_MODE_DAT8,
+          ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, 0x0,
+          false, false },
+        { V4L2_PIX_FMT_SRGGB8, MEDIA_BUS_FMT_SRGGB8_1X8, 8,
+          ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_RGRG, ISC_RLP_CFG_MODE_DAT8,
+          ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, 0x0,
+          false, false },
+
+        { V4L2_PIX_FMT_SBGGR10, MEDIA_BUS_FMT_SBGGR10_1X10, 16,
+          ISC_PFG_CFG0_BPS_TEN, ISC_BAY_CFG_BGBG, ISC_RLP_CFG_MODE_DAT10,
+          ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, 0x0,
+          false, false },
+        { V4L2_PIX_FMT_SGBRG10, MEDIA_BUS_FMT_SGBRG10_1X10, 16,
+          ISC_PFG_CFG0_BPS_TEN, ISC_BAY_CFG_GBGB, ISC_RLP_CFG_MODE_DAT10,
+          ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, 0x0,
+          false, false },
+        { V4L2_PIX_FMT_SGRBG10, MEDIA_BUS_FMT_SGRBG10_1X10, 16,
+          ISC_PFG_CFG0_BPS_TEN, ISC_BAY_CFG_GRGR, ISC_RLP_CFG_MODE_DAT10,
+          ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, 0x0,
+          false, false },
+        { V4L2_PIX_FMT_SRGGB10, MEDIA_BUS_FMT_SRGGB10_1X10, 16,
+          ISC_PFG_CFG0_BPS_TEN, ISC_BAY_CFG_RGRG, ISC_RLP_CFG_MODE_DAT10,
+          ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, 0x0,
+          false, false },
+
+        { V4L2_PIX_FMT_SBGGR12, MEDIA_BUS_FMT_SBGGR12_1X12, 16,
+          ISC_PFG_CFG0_BPS_TWELVE, ISC_BAY_CFG_BGBG, ISC_RLP_CFG_MODE_DAT12,
+          ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, 0x0,
+          false, false },
+        { V4L2_PIX_FMT_SGBRG12, MEDIA_BUS_FMT_SGBRG12_1X12, 16,
+          ISC_PFG_CFG0_BPS_TWELVE, ISC_BAY_CFG_GBGB, ISC_RLP_CFG_MODE_DAT12,
+          ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, 0x0,
+          false, false },
+        { V4L2_PIX_FMT_SGRBG12, MEDIA_BUS_FMT_SGRBG12_1X12, 16,
+          ISC_PFG_CFG0_BPS_TWELVE, ISC_BAY_CFG_GRGR, ISC_RLP_CFG_MODE_DAT12,
+          ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, 0x0,
+          false, false },
+        { V4L2_PIX_FMT_SRGGB12, MEDIA_BUS_FMT_SRGGB12_1X12, 16,
+          ISC_PFG_CFG0_BPS_TWELVE, ISC_BAY_CFG_RGRG, ISC_RLP_CFG_MODE_DAT12,
+          ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, 0x0,
+          false, false },
+
+        { V4L2_PIX_FMT_YUV420, 0x0, 12,
+          ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_BGBG, ISC_RLP_CFG_MODE_YYCC,
+          ISC_DCFG_IMODE_YC420P | ISC_DCFG_YMBSIZE_BEATS8 |
+          ISC_DCFG_CMBSIZE_BEATS8, ISC_DCTRL_DVIEW_PLANAR, 0x7fb,
+          false, false },
+        { V4L2_PIX_FMT_YUV422P, 0x0, 16,
+          ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_BGBG, ISC_RLP_CFG_MODE_YYCC,
+          ISC_DCFG_IMODE_YC422P | ISC_DCFG_YMBSIZE_BEATS8 |
+          ISC_DCFG_CMBSIZE_BEATS8, ISC_DCTRL_DVIEW_PLANAR, 0x3fb,
+          false, false },
+        { V4L2_PIX_FMT_RGB565, MEDIA_BUS_FMT_RGB565_2X8_LE, 16,
+          ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_BGBG, ISC_RLP_CFG_MODE_RGB565,
+          ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, 0x7b,
+          false, false },
+
+        { V4L2_PIX_FMT_YUYV, MEDIA_BUS_FMT_YUYV8_2X8, 16,
+          ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_BGBG, ISC_RLP_CFG_MODE_DAT8,
+          ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, 0x0,
+          false, false },
+};
+
+#define GAMMA_MAX       2
+#define GAMMA_ENTRIES   64
+
+/* Gamma table with gamma 1/2.2 */
+static const u32 isc_gamma_table[GAMMA_MAX + 1][GAMMA_ENTRIES] = {
+        /* 0 --> gamma 1/1.8 */
+        {      0x65,  0x66002F,  0x950025,  0xBB0020,  0xDB001D,  0xF8001A,
+          0x1130018, 0x12B0017, 0x1420016, 0x1580014, 0x16D0013, 0x1810012,
+          0x1940012, 0x1A60012, 0x1B80011, 0x1C90010, 0x1DA0010, 0x1EA000F,
+          0x1FA000F, 0x209000F, 0x218000F, 0x227000E, 0x235000E, 0x243000E,
+          0x251000E, 0x25F000D, 0x26C000D, 0x279000D, 0x286000D, 0x293000C,
+          0x2A0000C, 0x2AC000C, 0x2B8000C, 0x2C4000C, 0x2D0000B, 0x2DC000B,
+          0x2E7000B, 0x2F3000B, 0x2FE000B, 0x309000B, 0x314000B, 0x31F000A,
+          0x32A000A, 0x334000B, 0x33F000A, 0x349000A, 0x354000A, 0x35E000A,
+          0x368000A, 0x372000A, 0x37C000A, 0x386000A, 0x3900009, 0x399000A,
+          0x3A30009, 0x3AD0009, 0x3B60009, 0x3BF000A, 0x3C90009, 0x3D20009,
+          0x3DB0009, 0x3E40009, 0x3ED0009, 0x3F60009 },
+
+        /* 1 --> gamma 1/2 */
+        {      0x7F,  0x800034,  0xB50028,  0xDE0021, 0x100001E, 0x11E001B,
+          0x1390019, 0x1520017, 0x16A0015, 0x1800014, 0x1940014, 0x1A80013,
+          0x1BB0012, 0x1CD0011, 0x1DF0010, 0x1EF0010, 0x200000F, 0x20F000F,
+          0x21F000E, 0x22D000F, 0x23C000E, 0x24A000E, 0x258000D, 0x265000D,
+          0x273000C, 0x27F000D, 0x28C000C, 0x299000C, 0x2A5000C, 0x2B1000B,
+          0x2BC000C, 0x2C8000B, 0x2D3000C, 0x2DF000B, 0x2EA000A, 0x2F5000A,
+          0x2FF000B, 0x30A000A, 0x314000B, 0x31F000A, 0x329000A, 0x333000A,
+          0x33D0009, 0x3470009, 0x350000A, 0x35A0009, 0x363000A, 0x36D0009,
+          0x3760009, 0x37F0009, 0x3880009, 0x3910009, 0x39A0009, 0x3A30009,
+          0x3AC0008, 0x3B40009, 0x3BD0008, 0x3C60008, 0x3CE0008, 0x3D60009,
+          0x3DF0008, 0x3E70008, 0x3EF0008, 0x3F70008 },
+
+        /* 2 --> gamma 1/2.2 */
+        {      0x99,  0x9B0038,  0xD4002A,  0xFF0023, 0x122001F, 0x141001B,
+          0x15D0019, 0x1760017, 0x18E0015, 0x1A30015, 0x1B80013, 0x1CC0012,
+          0x1DE0011, 0x1F00010, 0x2010010, 0x2110010, 0x221000F, 0x230000F,
+          0x23F000E, 0x24D000E, 0x25B000D, 0x269000C, 0x276000C, 0x283000C,
+          0x28F000C, 0x29B000C, 0x2A7000C, 0x2B3000B, 0x2BF000B, 0x2CA000B,
+          0x2D5000B, 0x2E0000A, 0x2EB000A, 0x2F5000A, 0x2FF000A, 0x30A000A,
+          0x3140009, 0x31E0009, 0x327000A, 0x3310009, 0x33A0009, 0x3440009,
+          0x34D0009, 0x3560009, 0x35F0009, 0x3680008, 0x3710008, 0x3790009,
+          0x3820008, 0x38A0008, 0x3930008, 0x39B0008, 0x3A30008, 0x3AB0008,
+          0x3B30008, 0x3BB0008, 0x3C30008, 0x3CB0007, 0x3D20008, 0x3DA0007,
+          0x3E20007, 0x3E90007, 0x3F00008, 0x3F80007 },
 };
 
+static unsigned int sensor_preferred = 1;
+module_param(sensor_preferred, uint, 0644);
+MODULE_PARM_DESC(sensor_preferred,
+                 "Sensor is preferred to output the specified format (1-on 0-off), default 1");
+
 static int isc_clk_enable(struct clk_hw *hw)
 {
         struct isc_clk *isc_clk = to_isc_clk(hw);
@@ -447,27 +570,155 @@ static int isc_buffer_prepare(struct vb2_buffer *vb)
         return 0;
 }
 
-static inline void isc_start_dma(struct regmap *regmap,
-                                  struct isc_buffer *frm, u32 dview)
+static inline bool sensor_is_preferred(const struct isc_format *isc_fmt)
+{
+        return (sensor_preferred && isc_fmt->sd_support) ||
+                !isc_fmt->isc_support;
+}
+
+static void isc_start_dma(struct isc_device *isc)
 {
-        dma_addr_t addr;
+        struct regmap *regmap = isc->regmap;
+        struct v4l2_pix_format *pixfmt = &isc->fmt.fmt.pix;
+        u32 sizeimage = pixfmt->sizeimage;
+        u32 dctrl_dview;
+        dma_addr_t addr0;
+
+        addr0 = vb2_dma_contig_plane_dma_addr(&isc->cur_frm->vb.vb2_buf, 0);
+        regmap_write(regmap, ISC_DAD0, addr0);
+
+        switch (pixfmt->pixelformat) {
+        case V4L2_PIX_FMT_YUV420:
+                regmap_write(regmap, ISC_DAD1, addr0 + (sizeimage * 2) / 3);
+                regmap_write(regmap, ISC_DAD2, addr0 + (sizeimage * 5) / 6);
+                break;
+        case V4L2_PIX_FMT_YUV422P:
+                regmap_write(regmap, ISC_DAD1, addr0 + sizeimage / 2);
+                regmap_write(regmap, ISC_DAD2, addr0 + (sizeimage * 3) / 4);
+                break;
+        default:
+                break;
+        }
 
-        addr = vb2_dma_contig_plane_dma_addr(&frm->vb.vb2_buf, 0);
+        if (sensor_is_preferred(isc->current_fmt))
+                dctrl_dview = ISC_DCTRL_DVIEW_PACKED;
+        else
+                dctrl_dview = isc->current_fmt->reg_dctrl_dview;
 
-        regmap_write(regmap, ISC_DCTRL, dview | ISC_DCTRL_IE_IS);
-        regmap_write(regmap, ISC_DAD0, addr);
+        regmap_write(regmap, ISC_DCTRL, dctrl_dview | ISC_DCTRL_IE_IS);
         regmap_write(regmap, ISC_CTRLEN, ISC_CTRL_CAPTURE);
 }
 
 static void isc_set_pipeline(struct isc_device *isc, u32 pipeline)
 {
-        u32 val;
+        struct regmap *regmap = isc->regmap;
+        struct isc_ctrls *ctrls = &isc->ctrls;
+        u32 val, bay_cfg;
+        const u32 *gamma;
         unsigned int i;
 
+        /* WB-->CFA-->CC-->GAM-->CSC-->CBC-->SUB422-->SUB420 */
         for (i = 0; i < ISC_PIPE_LINE_NODE_NUM; i++) {
                 val = pipeline & BIT(i) ? 1 : 0;
                 regmap_field_write(isc->pipeline[i], val);
         }
+
+        if (!pipeline)
+                return;
+
+        bay_cfg = isc->raw_fmt->reg_bay_cfg;
+
+        regmap_write(regmap, ISC_WB_CFG, bay_cfg);
+        regmap_write(regmap, ISC_WB_O_RGR, 0x0);
+        regmap_write(regmap, ISC_WB_O_BGR, 0x0);
+        regmap_write(regmap, ISC_WB_G_RGR, ctrls->r_gain | (0x1 << 25));
+        regmap_write(regmap, ISC_WB_G_BGR, ctrls->b_gain | (0x1 << 25));
+
+        regmap_write(regmap, ISC_CFA_CFG, bay_cfg | ISC_CFA_CFG_EITPOL);
+
+        gamma = &isc_gamma_table[ctrls->gamma_index][0];
+        regmap_bulk_write(regmap, ISC_GAM_BENTRY, gamma, GAMMA_ENTRIES);
+        regmap_bulk_write(regmap, ISC_GAM_GENTRY, gamma, GAMMA_ENTRIES);
+        regmap_bulk_write(regmap, ISC_GAM_RENTRY, gamma, GAMMA_ENTRIES);
+
+        /* Convert RGB to YUV */
+        regmap_write(regmap, ISC_CSC_YR_YG, 0x42 | (0x81 << 16));
+        regmap_write(regmap, ISC_CSC_YB_OY, 0x19 | (0x10 << 16));
+        regmap_write(regmap, ISC_CSC_CBR_CBG, 0xFDA | (0xFB6 << 16));
+        regmap_write(regmap, ISC_CSC_CBB_OCB, 0x70 | (0x80 << 16));
+        regmap_write(regmap, ISC_CSC_CRR_CRG, 0x70 | (0xFA2 << 16));
+        regmap_write(regmap, ISC_CSC_CRB_OCR, 0xFEE | (0x80 << 16));
+
+        regmap_write(regmap, ISC_CBC_BRIGHT, ctrls->brightness);
+        regmap_write(regmap, ISC_CBC_CONTRAST, ctrls->contrast);
+}
+
+static int isc_update_profile(struct isc_device *isc)
+{
+        struct regmap *regmap = isc->regmap;
+        u32 sr;
+        int counter = 100;
+
+        regmap_write(regmap, ISC_CTRLEN, ISC_CTRL_UPPRO);
+
+        regmap_read(regmap, ISC_CTRLSR, &sr);
+        while ((sr & ISC_CTRL_UPPRO) && counter--) {
+                usleep_range(1000, 2000);
+                regmap_read(regmap, ISC_CTRLSR, &sr);
+        }
+
+        if (counter < 0) {
+                v4l2_warn(&isc->v4l2_dev, "Time out to update profie\n");
+                return -ETIMEDOUT;
+        }
+
+        return 0;
+}
+
+static void isc_set_histogram(struct isc_device *isc)
+{
+        struct regmap *regmap = isc->regmap;
+        struct isc_ctrls *ctrls = &isc->ctrls;
+
+        if (ctrls->awb && (ctrls->hist_stat != HIST_ENABLED)) {
+                regmap_write(regmap, ISC_HIS_CFG, ISC_HIS_CFG_MODE_R |
+                      (isc->raw_fmt->reg_bay_cfg << ISC_HIS_CFG_BAYSEL_SHIFT) |
+                      ISC_HIS_CFG_RAR);
+                regmap_write(regmap, ISC_HIS_CTRL, ISC_HIS_CTRL_EN);
+                regmap_write(regmap, ISC_INTEN, ISC_INT_HISDONE);
+                ctrls->hist_id = ISC_HIS_CFG_MODE_R;
+                isc_update_profile(isc);
+                regmap_write(regmap, ISC_CTRLEN, ISC_CTRL_HISREQ);
+
+                ctrls->hist_stat = HIST_ENABLED;
+        } else if (!ctrls->awb && (ctrls->hist_stat != HIST_DISABLED)) {
+                regmap_write(regmap, ISC_INTDIS, ISC_INT_HISDONE);
+                regmap_write(regmap, ISC_HIS_CTRL, ISC_HIS_CTRL_DIS);
+
+                ctrls->hist_stat = HIST_DISABLED;
+        }
+}
+
+static inline void isc_get_param(const struct isc_format *fmt,
+                                     u32 *rlp_mode, u32 *dcfg_imode)
+{
+        switch (fmt->fourcc) {
+        case V4L2_PIX_FMT_SBGGR10:
+        case V4L2_PIX_FMT_SGBRG10:
+        case V4L2_PIX_FMT_SGRBG10:
+        case V4L2_PIX_FMT_SRGGB10:
+        case V4L2_PIX_FMT_SBGGR12:
+        case V4L2_PIX_FMT_SGBRG12:
+        case V4L2_PIX_FMT_SGRBG12:
+        case V4L2_PIX_FMT_SRGGB12:
+                *rlp_mode = fmt->reg_rlp_mode;
+                *dcfg_imode = fmt->reg_dcfg_imode;
+                break;
+        default:
+                *rlp_mode = ISC_RLP_CFG_MODE_DAT8;
+                *dcfg_imode = ISC_DCFG_IMODE_PACKED8;
+                break;
+        }
 }
 
 static int isc_configure(struct isc_device *isc)
@@ -475,39 +726,40 @@ static int isc_configure(struct isc_device *isc)
         struct regmap *regmap = isc->regmap;
         const struct isc_format *current_fmt = isc->current_fmt;
         struct isc_subdev_entity *subdev = isc->current_subdev;
-        u32 val, mask;
-        int counter = 10;
+        u32 pfe_cfg0, rlp_mode, dcfg_imode, mask, pipeline;
+
+        if (sensor_is_preferred(current_fmt)) {
+                pfe_cfg0 = current_fmt->reg_bps;
+                pipeline = 0x0;
+                isc_get_param(current_fmt, &rlp_mode, &dcfg_imode);
+                isc->ctrls.hist_stat = HIST_INIT;
+        } else {
+                pfe_cfg0  = isc->raw_fmt->reg_bps;
+                pipeline = current_fmt->pipeline;
+                rlp_mode = current_fmt->reg_rlp_mode;
+                dcfg_imode = current_fmt->reg_dcfg_imode;
+        }
 
-        val = current_fmt->reg_bps | subdev->pfe_cfg0 |
-              ISC_PFE_CFG0_MODE_PROGRESSIVE;
+        pfe_cfg0  |= subdev->pfe_cfg0 | ISC_PFE_CFG0_MODE_PROGRESSIVE;
         mask = ISC_PFE_CFG0_BPS_MASK | ISC_PFE_CFG0_HPOL_LOW |
                ISC_PFE_CFG0_VPOL_LOW | ISC_PFE_CFG0_PPOL_LOW |
                ISC_PFE_CFG0_MODE_MASK;
 
-        regmap_update_bits(regmap, ISC_PFE_CFG0, mask, val);
+        regmap_update_bits(regmap, ISC_PFE_CFG0, mask, pfe_cfg0);
 
         regmap_update_bits(regmap, ISC_RLP_CFG, ISC_RLP_CFG_MODE_MASK,
-                           current_fmt->reg_rlp_mode);
+                           rlp_mode);
 
-        regmap_update_bits(regmap, ISC_DCFG, ISC_DCFG_IMODE_MASK,
-                           current_fmt->reg_dcfg_imode);
+        regmap_update_bits(regmap, ISC_DCFG, ISC_DCFG_IMODE_MASK, dcfg_imode);
 
-        /* Disable the pipeline */
-        isc_set_pipeline(isc, 0x0);
+        /* Set the pipeline */
+        isc_set_pipeline(isc, pipeline);
 
-        /* Update profile */
-        regmap_write(regmap, ISC_CTRLEN, ISC_CTRL_UPPRO);
-
-        regmap_read(regmap, ISC_CTRLSR, &val);
-        while ((val & ISC_CTRL_UPPRO) && counter--) {
-                usleep_range(1000, 2000);
-                regmap_read(regmap, ISC_CTRLSR, &val);
-        }
+        if (pipeline)
+                isc_set_histogram(isc);
 
-        if (counter < 0)
-                return -ETIMEDOUT;
-
-        return 0;
+        /* Update profile */
+        return isc_update_profile(isc);
 }
 
 static int isc_start_streaming(struct vb2_queue *vq, unsigned int count)
@@ -517,7 +769,6 @@ static int isc_start_streaming(struct vb2_queue *vq, unsigned int count)
         struct isc_buffer *buf;
         unsigned long flags;
         int ret;
-        u32 val;
 
         /* Enable stream on the sub device */
         ret = v4l2_subdev_call(isc->current_subdev->sd, video, s_stream, 1);
@@ -528,12 +779,6 @@ static int isc_start_streaming(struct vb2_queue *vq, unsigned int count)
 
         pm_runtime_get_sync(isc->dev);
 
-        /* Disable all the interrupts */
-        regmap_write(isc->regmap, ISC_INTDIS, (u32)~0UL);
-
-        /* Clean the interrupt status register */
-        regmap_read(regmap, ISC_INTSR, &val);
-
         ret = isc_configure(isc);
         if (unlikely(ret))
                 goto err_configure;
@@ -551,7 +796,7 @@ static int isc_start_streaming(struct vb2_queue *vq, unsigned int count)
                                         struct isc_buffer, list);
         list_del(&isc->cur_frm->list);
 
-        isc_start_dma(regmap, isc->cur_frm, isc->current_fmt->reg_dctrl_dview);
+        isc_start_dma(isc);
 
         spin_unlock_irqrestore(&isc->dma_queue_lock, flags);
 
@@ -620,8 +865,7 @@ static void isc_buffer_queue(struct vb2_buffer *vb)
         if (!isc->cur_frm && list_empty(&isc->dma_queue) &&
                 vb2_is_streaming(vb->vb2_queue)) {
                 isc->cur_frm = buf;
-                isc_start_dma(isc->regmap, isc->cur_frm,
-                        isc->current_fmt->reg_dctrl_dview);
+                isc_start_dma(isc);
         } else
                 list_add_tail(&buf->list, &isc->dma_queue);
         spin_unlock_irqrestore(&isc->dma_queue_lock, flags);
@@ -691,13 +935,14 @@ static struct isc_format *find_format_by_fourcc(struct isc_device *isc,
 }
 
 static int isc_try_fmt(struct isc_device *isc, struct v4l2_format *f,
-                        struct isc_format **current_fmt)
+                        struct isc_format **current_fmt, u32 *code)
 {
         struct isc_format *isc_fmt;
         struct v4l2_pix_format *pixfmt = &f->fmt.pix;
         struct v4l2_subdev_format format = {
                 .which = V4L2_SUBDEV_FORMAT_TRY,
         };
+        u32 mbus_code;
         int ret;
 
         if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
@@ -717,7 +962,12 @@ static int isc_try_fmt(struct isc_device *isc, struct v4l2_format *f,
         if (pixfmt->height > ISC_MAX_SUPPORT_HEIGHT)
                 pixfmt->height = ISC_MAX_SUPPORT_HEIGHT;
 
-        v4l2_fill_mbus_format(&format.format, pixfmt, isc_fmt->mbus_code);
+        if (sensor_is_preferred(isc_fmt))
+                mbus_code = isc_fmt->mbus_code;
+        else
+                mbus_code = isc->raw_fmt->mbus_code;
+
+        v4l2_fill_mbus_format(&format.format, pixfmt, mbus_code);
         ret = v4l2_subdev_call(isc->current_subdev->sd, pad, set_fmt,
                                isc->current_subdev->config, &format);
         if (ret < 0)
@@ -726,12 +976,15 @@ static int isc_try_fmt(struct isc_device *isc, struct v4l2_format *f,
         v4l2_fill_pix_format(pixfmt, &format.format);
 
         pixfmt->field = V4L2_FIELD_NONE;
-        pixfmt->bytesperline = pixfmt->width * isc_fmt->bpp;
+        pixfmt->bytesperline = (pixfmt->width * isc_fmt->bpp) >> 3;
         pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
 
         if (current_fmt)
                 *current_fmt = isc_fmt;
 
+        if (code)
+                *code = mbus_code;
+
         return 0;
 }
 
@@ -741,14 +994,14 @@ static int isc_set_fmt(struct isc_device *isc, struct v4l2_format *f)
                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
         };
         struct isc_format *current_fmt;
+        u32 mbus_code;
         int ret;
 
-        ret = isc_try_fmt(isc, f, &current_fmt);
+        ret = isc_try_fmt(isc, f, &current_fmt, &mbus_code);
         if (ret)
                 return ret;
 
-        v4l2_fill_mbus_format(&format.format, &f->fmt.pix,
-                              current_fmt->mbus_code);
+        v4l2_fill_mbus_format(&format.format, &f->fmt.pix, mbus_code);
         ret = v4l2_subdev_call(isc->current_subdev->sd, pad,
                                set_fmt, NULL, &format);
         if (ret < 0)
@@ -776,7 +1029,7 @@ static int isc_try_fmt_vid_cap(struct file *file, void *priv,
 {
         struct isc_device *isc = video_drvdata(file);
 
-        return isc_try_fmt(isc, f, NULL);
+        return isc_try_fmt(isc, f, NULL, NULL);
 }
 
 static int isc_enum_input(struct file *file, void *priv,
@@ -842,7 +1095,10 @@ static int isc_enum_framesizes(struct file *file, void *fh,
         if (!isc_fmt)
                 return -EINVAL;
 
-        fse.code = isc_fmt->mbus_code;
+        if (sensor_is_preferred(isc_fmt))
+                fse.code = isc_fmt->mbus_code;
+        else
+                fse.code = isc->raw_fmt->mbus_code;
 
         ret = v4l2_subdev_call(isc->current_subdev->sd, pad, enum_frame_size,
                                NULL, &fse);
@@ -873,7 +1129,10 @@ static int isc_enum_frameintervals(struct file *file, void *fh,
         if (!isc_fmt)
                 return -EINVAL;
 
-        fie.code = isc_fmt->mbus_code;
+        if (sensor_is_preferred(isc_fmt))
+                fie.code = isc_fmt->mbus_code;
+        else
+                fie.code = isc->raw_fmt->mbus_code;
 
         ret = v4l2_subdev_call(isc->current_subdev->sd, pad,
                                enum_frame_interval, NULL, &fie);
@@ -911,6 +1170,10 @@ static const struct v4l2_ioctl_ops isc_ioctl_ops = {
         .vidioc_s_parm                  = isc_s_parm,
         .vidioc_enum_framesizes         = isc_enum_framesizes,
         .vidioc_enum_frameintervals     = isc_enum_frameintervals,
+
+        .vidioc_log_status              = v4l2_ctrl_log_status,
+        .vidioc_subscribe_event         = v4l2_ctrl_subscribe_event,
+        .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
 };
 
 static int isc_open(struct file *file)
@@ -984,14 +1247,13 @@ static irqreturn_t isc_interrupt(int irq, void *dev_id)
         u32 isc_intsr, isc_intmask, pending;
         irqreturn_t ret = IRQ_NONE;
 
-        spin_lock(&isc->dma_queue_lock);
-
         regmap_read(regmap, ISC_INTSR, &isc_intsr);
         regmap_read(regmap, ISC_INTMASK, &isc_intmask);
 
         pending = isc_intsr & isc_intmask;
 
         if (likely(pending & ISC_INT_DDONE)) {
+                spin_lock(&isc->dma_queue_lock);
                 if (isc->cur_frm) {
                         struct vb2_v4l2_buffer *vbuf = &isc->cur_frm->vb;
                         struct vb2_buffer *vb = &vbuf->vb2_buf;
@@ -1007,21 +1269,144 @@ static irqreturn_t isc_interrupt(int irq, void *dev_id)
                                                      struct isc_buffer, list);
                         list_del(&isc->cur_frm->list);
 
-                        isc_start_dma(regmap, isc->cur_frm,
-                                      isc->current_fmt->reg_dctrl_dview);
+                        isc_start_dma(isc);
                 }
 
                 if (isc->stop)
                         complete(&isc->comp);
 
                 ret = IRQ_HANDLED;
+                spin_unlock(&isc->dma_queue_lock);
         }
 
-        spin_unlock(&isc->dma_queue_lock);
+        if (pending & ISC_INT_HISDONE) {
+                schedule_work(&isc->awb_work);
+                ret = IRQ_HANDLED;
+        }
 
         return ret;
 }
 
+static void isc_hist_count(struct isc_device *isc)
+{
+        struct regmap *regmap = isc->regmap;
+        struct isc_ctrls *ctrls = &isc->ctrls;
+        u32 *hist_count = &ctrls->hist_count[ctrls->hist_id];
+        u32 *hist_entry = &ctrls->hist_entry[0];
+        u32 i;
+
+        regmap_bulk_read(regmap, ISC_HIS_ENTRY, hist_entry, HIST_ENTRIES);
+
+        *hist_count = 0;
+        for (i = 0; i < HIST_ENTRIES; i++)
+                *hist_count += i * (*hist_entry++);
+}
+
+static void isc_wb_update(struct isc_ctrls *ctrls)
+{
+        u32 *hist_count = &ctrls->hist_count[0];
+        u64 g_count = (u64)hist_count[ISC_HIS_CFG_MODE_GB] << 9;
+        u32 hist_r = hist_count[ISC_HIS_CFG_MODE_R];
+        u32 hist_b = hist_count[ISC_HIS_CFG_MODE_B];
+
+        if (hist_r)
+                ctrls->r_gain = div_u64(g_count, hist_r);
+
+        if (hist_b)
+                ctrls->b_gain = div_u64(g_count, hist_b);
+}
+
+static void isc_awb_work(struct work_struct *w)
+{
+        struct isc_device *isc =
+                container_of(w, struct isc_device, awb_work);
+        struct regmap *regmap = isc->regmap;
+        struct isc_ctrls *ctrls = &isc->ctrls;
+        u32 hist_id = ctrls->hist_id;
+        u32 baysel;
+
+        if (ctrls->hist_stat != HIST_ENABLED)
+                return;
+
+        isc_hist_count(isc);
+
+        if (hist_id != ISC_HIS_CFG_MODE_B) {
+                hist_id++;
+        } else {
+                isc_wb_update(ctrls);
+                hist_id = ISC_HIS_CFG_MODE_R;
+        }
+
+        ctrls->hist_id = hist_id;
+        baysel = isc->raw_fmt->reg_bay_cfg << ISC_HIS_CFG_BAYSEL_SHIFT;
+
+        pm_runtime_get_sync(isc->dev);
+
+        regmap_write(regmap, ISC_HIS_CFG, hist_id | baysel | ISC_HIS_CFG_RAR);
+        isc_update_profile(isc);
+        regmap_write(regmap, ISC_CTRLEN, ISC_CTRL_HISREQ);
+
+        pm_runtime_put_sync(isc->dev);
+}
+
+static int isc_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct isc_device *isc = container_of(ctrl->handler,
+                                             struct isc_device, ctrls.handler);
+        struct isc_ctrls *ctrls = &isc->ctrls;
+
+        switch (ctrl->id) {
+        case V4L2_CID_BRIGHTNESS:
+                ctrls->brightness = ctrl->val & ISC_CBC_BRIGHT_MASK;
+                break;
+        case V4L2_CID_CONTRAST:
+                ctrls->contrast = ctrl->val & ISC_CBC_CONTRAST_MASK;
+                break;
+        case V4L2_CID_GAMMA:
+                ctrls->gamma_index = ctrl->val;
+                break;
+        case V4L2_CID_AUTO_WHITE_BALANCE:
+                ctrls->awb = ctrl->val;
+                if (ctrls->hist_stat != HIST_ENABLED) {
+                        ctrls->r_gain = 0x1 << 9;
+                        ctrls->b_gain = 0x1 << 9;
+                }
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+static const struct v4l2_ctrl_ops isc_ctrl_ops = {
+        .s_ctrl = isc_s_ctrl,
+};
+
+static int isc_ctrl_init(struct isc_device *isc)
+{
+        const struct v4l2_ctrl_ops *ops = &isc_ctrl_ops;
+        struct isc_ctrls *ctrls = &isc->ctrls;
+        struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+        int ret;
+
+        ctrls->hist_stat = HIST_INIT;
+
+        ret = v4l2_ctrl_handler_init(hdl, 4);
+        if (ret < 0)
+                return ret;
+
+        v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BRIGHTNESS, -1024, 1023, 1, 0);
+        v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST, -2048, 2047, 1, 256);
+        v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAMMA, 0, GAMMA_MAX, 1, 2);
+        v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
+
+        v4l2_ctrl_handler_setup(hdl);
+
+        return 0;
+}
+
+
 static int isc_async_bound(struct v4l2_async_notifier *notifier,
                             struct v4l2_subdev *subdev,
                             struct v4l2_async_subdev *asd)
@@ -1047,10 +1432,11 @@ static void isc_async_unbind(struct v4l2_async_notifier *notifier,
 {
         struct isc_device *isc = container_of(notifier->v4l2_dev,
                                               struct isc_device, v4l2_dev);
-
+        cancel_work_sync(&isc->awb_work);
         video_unregister_device(&isc->video_dev);
         if (isc->current_subdev->config)
                 v4l2_subdev_free_pad_config(isc->current_subdev->config);
+        v4l2_ctrl_handler_free(&isc->ctrls.handler);
 }
 
 static struct isc_format *find_format_by_code(unsigned int code, int *index)
@@ -1074,14 +1460,16 @@ static int isc_formats_init(struct isc_device *isc)
 {
         struct isc_format *fmt;
         struct v4l2_subdev *subdev = isc->current_subdev->sd;
-        int num_fmts = 0, i, j;
+        unsigned int num_fmts, i, j;
         struct v4l2_subdev_mbus_code_enum mbus_code = {
                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
         };
 
         fmt = &isc_formats[0];
         for (i = 0; i < ARRAY_SIZE(isc_formats); i++) {
-                fmt->support = false;
+                fmt->isc_support = false;
+                fmt->sd_support = false;
+
                 fmt++;
         }
 
@@ -1092,8 +1480,22 @@ static int isc_formats_init(struct isc_device *isc)
                 if (!fmt)
                         continue;
 
-                fmt->support = true;
-                num_fmts++;
+                fmt->sd_support = true;
+
+                if (i <= RAW_FMT_IND_END) {
+                        for (j = ISC_FMT_IND_START; j <= ISC_FMT_IND_END; j++)
+                                isc_formats[j].isc_support = true;
+
+                        isc->raw_fmt = fmt;
+                }
+        }
+
+        fmt = &isc_formats[0];
+        for (i = 0, num_fmts = 0; i < ARRAY_SIZE(isc_formats); i++) {
+                if (fmt->isc_support || fmt->sd_support)
+                        num_fmts++;
+
+                fmt++;
         }
 
         if (!num_fmts)
@@ -1110,7 +1512,7 @@ static int isc_formats_init(struct isc_device *isc)
 
         fmt = &isc_formats[0];
         for (i = 0, j = 0; i < ARRAY_SIZE(isc_formats); i++) {
-                if (fmt->support)
+                if (fmt->isc_support || fmt->sd_support)
                         isc->user_formats[j++] = fmt;
 
                 fmt++;
@@ -1132,7 +1534,7 @@ static int isc_set_default_fmt(struct isc_device *isc)
         };
         int ret;
 
-        ret = isc_try_fmt(isc, &f, NULL);
+        ret = isc_try_fmt(isc, &f, NULL, NULL);
         if (ret)
                 return ret;
 
@@ -1151,6 +1553,12 @@ static int isc_async_complete(struct v4l2_async_notifier *notifier)
         struct vb2_queue *q = &isc->vb2_vidq;
         int ret;
 
+        ret = v4l2_device_register_subdev_nodes(&isc->v4l2_dev);
+        if (ret < 0) {
+                v4l2_err(&isc->v4l2_dev, "Failed to register subdev nodes\n");
+                return ret;
+        }
+
         isc->current_subdev = container_of(notifier,
                                            struct isc_subdev_entity, notifier);
         sd_entity = isc->current_subdev;
@@ -1198,6 +1606,14 @@ static int isc_async_complete(struct v4l2_async_notifier *notifier)
                 return ret;
         }
 
+        ret = isc_ctrl_init(isc);
+        if (ret) {
+                v4l2_err(&isc->v4l2_dev, "Init isc ctrols failed: %d\n", ret);
+                return ret;
+        }
+
+        INIT_WORK(&isc->awb_work, isc_awb_work);
+
         /* Register video device */
         strlcpy(vdev->name, ATMEL_ISC_NAME, sizeof(vdev->name));
         vdev->release           = video_device_release_empty;
@@ -1207,7 +1623,7 @@ static int isc_async_complete(struct v4l2_async_notifier *notifier)
         vdev->vfl_dir           = VFL_DIR_RX;
         vdev->queue             = q;
         vdev->lock              = &isc->lock;
-        vdev->ctrl_handler      = isc->current_subdev->sd->ctrl_handler;
+        vdev->ctrl_handler      = &isc->ctrls.handler;
         vdev->device_caps       = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE;
         video_set_drvdata(vdev, isc);
 
diff --git a/drivers/media/platform/atmel/atmel-isi.c b/drivers/media/platform/atmel/atmel-isi.c
new file mode 100644
index 000000000000..e4867f84514c
--- /dev/null
+++ b/drivers/media/platform/atmel/atmel-isi.c
@@ -0,0 +1,1368 @@
+/*
+ * Copyright (c) 2011 Atmel Corporation
+ * Josh Wu, <josh.wu@atmel.com>
+ *
+ * Based on previous work by Lars Haring, <lars.haring@atmel.com>
+ * and Sedji Gaouaou
+ * Based on the bttv driver for Bt848 with respective copyright holders
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+
+#include <linux/videodev2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-of.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/v4l2-image-sizes.h>
+
+#include "atmel-isi.h"
+
+#define MAX_SUPPORT_WIDTH               2048
+#define MAX_SUPPORT_HEIGHT              2048
+#define MIN_FRAME_RATE                  15
+#define FRAME_INTERVAL_MILLI_SEC        (1000 / MIN_FRAME_RATE)
+
+/* Frame buffer descriptor */
+struct fbd {
+        /* Physical address of the frame buffer */
+        u32 fb_address;
+        /* DMA Control Register(only in HISI2) */
+        u32 dma_ctrl;
+        /* Physical address of the next fbd */
+        u32 next_fbd_address;
+};
+
+static void set_dma_ctrl(struct fbd *fb_desc, u32 ctrl)
+{
+        fb_desc->dma_ctrl = ctrl;
+}
+
+struct isi_dma_desc {
+        struct list_head list;
+        struct fbd *p_fbd;
+        dma_addr_t fbd_phys;
+};
+
+/* Frame buffer data */
+struct frame_buffer {
+        struct vb2_v4l2_buffer vb;
+        struct isi_dma_desc *p_dma_desc;
+        struct list_head list;
+};
+
+struct isi_graph_entity {
+        struct device_node *node;
+
+        struct v4l2_async_subdev asd;
+        struct v4l2_subdev *subdev;
+};
+
+/*
+ * struct isi_format - ISI media bus format information
+ * @fourcc:             Fourcc code for this format
+ * @mbus_code:          V4L2 media bus format code.
+ * @bpp:                Bytes per pixel (when stored in memory)
+ * @swap:               Byte swap configuration value
+ * @support:            Indicates format supported by subdev
+ * @skip:               Skip duplicate format supported by subdev
+ */
+struct isi_format {
+        u32     fourcc;
+        u32     mbus_code;
+        u8      bpp;
+        u32     swap;
+};
+
+
+struct atmel_isi {
+        /* Protects the access of variables shared with the ISR */
+        spinlock_t                      irqlock;
+        struct device                   *dev;
+        void __iomem                    *regs;
+
+        int                             sequence;
+
+        /* Allocate descriptors for dma buffer use */
+        struct fbd                      *p_fb_descriptors;
+        dma_addr_t                      fb_descriptors_phys;
+        struct                          list_head dma_desc_head;
+        struct isi_dma_desc             dma_desc[VIDEO_MAX_FRAME];
+        bool                            enable_preview_path;
+
+        struct completion               complete;
+        /* ISI peripherial clock */
+        struct clk                      *pclk;
+        unsigned int                    irq;
+
+        struct isi_platform_data        pdata;
+        u16                             width_flags;    /* max 12 bits */
+
+        struct list_head                video_buffer_list;
+        struct frame_buffer             *active;
+
+        struct v4l2_device              v4l2_dev;
+        struct video_device             *vdev;
+        struct v4l2_async_notifier      notifier;
+        struct isi_graph_entity         entity;
+        struct v4l2_format              fmt;
+
+        const struct isi_format         **user_formats;
+        unsigned int                    num_user_formats;
+        const struct isi_format         *current_fmt;
+
+        struct mutex                    lock;
+        struct vb2_queue                queue;
+};
+
+#define notifier_to_isi(n) container_of(n, struct atmel_isi, notifier)
+
+static void isi_writel(struct atmel_isi *isi, u32 reg, u32 val)
+{
+        writel(val, isi->regs + reg);
+}
+static u32 isi_readl(struct atmel_isi *isi, u32 reg)
+{
+        return readl(isi->regs + reg);
+}
+
+static void configure_geometry(struct atmel_isi *isi)
+{
+        u32 cfg2, psize;
+        u32 fourcc = isi->current_fmt->fourcc;
+
+        isi->enable_preview_path = fourcc == V4L2_PIX_FMT_RGB565 ||
+                                   fourcc == V4L2_PIX_FMT_RGB32;
+
+        /* According to sensor's output format to set cfg2 */
+        cfg2 = isi->current_fmt->swap;
+
+        isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
+        /* Set width */
+        cfg2 |= ((isi->fmt.fmt.pix.width - 1) << ISI_CFG2_IM_HSIZE_OFFSET) &
+                        ISI_CFG2_IM_HSIZE_MASK;
+        /* Set height */
+        cfg2 |= ((isi->fmt.fmt.pix.height - 1) << ISI_CFG2_IM_VSIZE_OFFSET)
+                        & ISI_CFG2_IM_VSIZE_MASK;
+        isi_writel(isi, ISI_CFG2, cfg2);
+
+        /* No down sampling, preview size equal to sensor output size */
+        psize = ((isi->fmt.fmt.pix.width - 1) << ISI_PSIZE_PREV_HSIZE_OFFSET) &
+                ISI_PSIZE_PREV_HSIZE_MASK;
+        psize |= ((isi->fmt.fmt.pix.height - 1) << ISI_PSIZE_PREV_VSIZE_OFFSET) &
+                ISI_PSIZE_PREV_VSIZE_MASK;
+        isi_writel(isi, ISI_PSIZE, psize);
+        isi_writel(isi, ISI_PDECF, ISI_PDECF_NO_SAMPLING);
+}
+
+static irqreturn_t atmel_isi_handle_streaming(struct atmel_isi *isi)
+{
+        if (isi->active) {
+                struct vb2_v4l2_buffer *vbuf = &isi->active->vb;
+                struct frame_buffer *buf = isi->active;
+
+                list_del_init(&buf->list);
+                vbuf->vb2_buf.timestamp = ktime_get_ns();
+                vbuf->sequence = isi->sequence++;
+                vbuf->field = V4L2_FIELD_NONE;
+                vb2_buffer_done(&vbuf->vb2_buf, VB2_BUF_STATE_DONE);
+        }
+
+        if (list_empty(&isi->video_buffer_list)) {
+                isi->active = NULL;
+        } else {
+                /* start next dma frame. */
+                isi->active = list_entry(isi->video_buffer_list.next,
+                                        struct frame_buffer, list);
+                if (!isi->enable_preview_path) {
+                        isi_writel(isi, ISI_DMA_C_DSCR,
+                                (u32)isi->active->p_dma_desc->fbd_phys);
+                        isi_writel(isi, ISI_DMA_C_CTRL,
+                                ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
+                        isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_C_CH);
+                } else {
+                        isi_writel(isi, ISI_DMA_P_DSCR,
+                                (u32)isi->active->p_dma_desc->fbd_phys);
+                        isi_writel(isi, ISI_DMA_P_CTRL,
+                                ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
+                        isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_P_CH);
+                }
+        }
+        return IRQ_HANDLED;
+}
+
+/* ISI interrupt service routine */
+static irqreturn_t isi_interrupt(int irq, void *dev_id)
+{
+        struct atmel_isi *isi = dev_id;
+        u32 status, mask, pending;
+        irqreturn_t ret = IRQ_NONE;
+
+        spin_lock(&isi->irqlock);
+
+        status = isi_readl(isi, ISI_STATUS);
+        mask = isi_readl(isi, ISI_INTMASK);
+        pending = status & mask;
+
+        if (pending & ISI_CTRL_SRST) {
+                complete(&isi->complete);
+                isi_writel(isi, ISI_INTDIS, ISI_CTRL_SRST);
+                ret = IRQ_HANDLED;
+        } else if (pending & ISI_CTRL_DIS) {
+                complete(&isi->complete);
+                isi_writel(isi, ISI_INTDIS, ISI_CTRL_DIS);
+                ret = IRQ_HANDLED;
+        } else {
+                if (likely(pending & ISI_SR_CXFR_DONE) ||
+                                likely(pending & ISI_SR_PXFR_DONE))
+                        ret = atmel_isi_handle_streaming(isi);
+        }
+
+        spin_unlock(&isi->irqlock);
+        return ret;
+}
+
+#define WAIT_ISI_RESET          1
+#define WAIT_ISI_DISABLE        0
+static int atmel_isi_wait_status(struct atmel_isi *isi, int wait_reset)
+{
+        unsigned long timeout;
+        /*
+         * The reset or disable will only succeed if we have a
+         * pixel clock from the camera.
+         */
+        init_completion(&isi->complete);
+
+        if (wait_reset) {
+                isi_writel(isi, ISI_INTEN, ISI_CTRL_SRST);
+                isi_writel(isi, ISI_CTRL, ISI_CTRL_SRST);
+        } else {
+                isi_writel(isi, ISI_INTEN, ISI_CTRL_DIS);
+                isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
+        }
+
+        timeout = wait_for_completion_timeout(&isi->complete,
+                        msecs_to_jiffies(500));
+        if (timeout == 0)
+                return -ETIMEDOUT;
+
+        return 0;
+}
+
+/* ------------------------------------------------------------------
+        Videobuf operations
+   ------------------------------------------------------------------*/
+static int queue_setup(struct vb2_queue *vq,
+                                unsigned int *nbuffers, unsigned int *nplanes,
+                                unsigned int sizes[], struct device *alloc_devs[])
+{
+        struct atmel_isi *isi = vb2_get_drv_priv(vq);
+        unsigned long size;
+
+        size = isi->fmt.fmt.pix.sizeimage;
+
+        /* Make sure the image size is large enough. */
+        if (*nplanes)
+                return sizes[0] < size ? -EINVAL : 0;
+
+        *nplanes = 1;
+        sizes[0] = size;
+
+        isi->active = NULL;
+
+        dev_dbg(isi->dev, "%s, count=%d, size=%ld\n", __func__,
+                *nbuffers, size);
+
+        return 0;
+}
+
+static int buffer_init(struct vb2_buffer *vb)
+{
+        struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+        struct frame_buffer *buf = container_of(vbuf, struct frame_buffer, vb);
+
+        buf->p_dma_desc = NULL;
+        INIT_LIST_HEAD(&buf->list);
+
+        return 0;
+}
+
+static int buffer_prepare(struct vb2_buffer *vb)
+{
+        struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+        struct frame_buffer *buf = container_of(vbuf, struct frame_buffer, vb);
+        struct atmel_isi *isi = vb2_get_drv_priv(vb->vb2_queue);
+        unsigned long size;
+        struct isi_dma_desc *desc;
+
+        size = isi->fmt.fmt.pix.sizeimage;
+
+        if (vb2_plane_size(vb, 0) < size) {
+                dev_err(isi->dev, "%s data will not fit into plane (%lu < %lu)\n",
+                                __func__, vb2_plane_size(vb, 0), size);
+                return -EINVAL;
+        }
+
+        vb2_set_plane_payload(vb, 0, size);
+
+        if (!buf->p_dma_desc) {
+                if (list_empty(&isi->dma_desc_head)) {
+                        dev_err(isi->dev, "Not enough dma descriptors.\n");
+                        return -EINVAL;
+                } else {
+                        /* Get an available descriptor */
+                        desc = list_entry(isi->dma_desc_head.next,
+                                                struct isi_dma_desc, list);
+                        /* Delete the descriptor since now it is used */
+                        list_del_init(&desc->list);
+
+                        /* Initialize the dma descriptor */
+                        desc->p_fbd->fb_address =
+                                        vb2_dma_contig_plane_dma_addr(vb, 0);
+                        desc->p_fbd->next_fbd_address = 0;
+                        set_dma_ctrl(desc->p_fbd, ISI_DMA_CTRL_WB);
+
+                        buf->p_dma_desc = desc;
+                }
+        }
+        return 0;
+}
+
+static void buffer_cleanup(struct vb2_buffer *vb)
+{
+        struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+        struct atmel_isi *isi = vb2_get_drv_priv(vb->vb2_queue);
+        struct frame_buffer *buf = container_of(vbuf, struct frame_buffer, vb);
+
+        /* This descriptor is available now and we add to head list */
+        if (buf->p_dma_desc)
+                list_add(&buf->p_dma_desc->list, &isi->dma_desc_head);
+}
+
+static void start_dma(struct atmel_isi *isi, struct frame_buffer *buffer)
+{
+        u32 ctrl, cfg1;
+
+        cfg1 = isi_readl(isi, ISI_CFG1);
+        /* Enable irq: cxfr for the codec path, pxfr for the preview path */
+        isi_writel(isi, ISI_INTEN,
+                        ISI_SR_CXFR_DONE | ISI_SR_PXFR_DONE);
+
+        /* Check if already in a frame */
+        if (!isi->enable_preview_path) {
+                if (isi_readl(isi, ISI_STATUS) & ISI_CTRL_CDC) {
+                        dev_err(isi->dev, "Already in frame handling.\n");
+                        return;
+                }
+
+                isi_writel(isi, ISI_DMA_C_DSCR,
+                                (u32)buffer->p_dma_desc->fbd_phys);
+                isi_writel(isi, ISI_DMA_C_CTRL,
+                                ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
+                isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_C_CH);
+        } else {
+                isi_writel(isi, ISI_DMA_P_DSCR,
+                                (u32)buffer->p_dma_desc->fbd_phys);
+                isi_writel(isi, ISI_DMA_P_CTRL,
+                                ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
+                isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_P_CH);
+        }
+
+        cfg1 &= ~ISI_CFG1_FRATE_DIV_MASK;
+        /* Enable linked list */
+        cfg1 |= isi->pdata.frate | ISI_CFG1_DISCR;
+
+        /* Enable ISI */
+        ctrl = ISI_CTRL_EN;
+
+        if (!isi->enable_preview_path)
+                ctrl |= ISI_CTRL_CDC;
+
+        isi_writel(isi, ISI_CTRL, ctrl);
+        isi_writel(isi, ISI_CFG1, cfg1);
+}
+
+static void buffer_queue(struct vb2_buffer *vb)
+{
+        struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+        struct atmel_isi *isi = vb2_get_drv_priv(vb->vb2_queue);
+        struct frame_buffer *buf = container_of(vbuf, struct frame_buffer, vb);
+        unsigned long flags = 0;
+
+        spin_lock_irqsave(&isi->irqlock, flags);
+        list_add_tail(&buf->list, &isi->video_buffer_list);
+
+        if (isi->active == NULL) {
+                isi->active = buf;
+                if (vb2_is_streaming(vb->vb2_queue))
+                        start_dma(isi, buf);
+        }
+        spin_unlock_irqrestore(&isi->irqlock, flags);
+}
+
+static int start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+        struct atmel_isi *isi = vb2_get_drv_priv(vq);
+        struct frame_buffer *buf, *node;
+        int ret;
+
+        /* Enable stream on the sub device */
+        ret = v4l2_subdev_call(isi->entity.subdev, video, s_stream, 1);
+        if (ret && ret != -ENOIOCTLCMD) {
+                dev_err(isi->dev, "stream on failed in subdev\n");
+                goto err_start_stream;
+        }
+
+        pm_runtime_get_sync(isi->dev);
+
+        /* Reset ISI */
+        ret = atmel_isi_wait_status(isi, WAIT_ISI_RESET);
+        if (ret < 0) {
+                dev_err(isi->dev, "Reset ISI timed out\n");
+                goto err_reset;
+        }
+        /* Disable all interrupts */
+        isi_writel(isi, ISI_INTDIS, (u32)~0UL);
+
+        isi->sequence = 0;
+        configure_geometry(isi);
+
+        spin_lock_irq(&isi->irqlock);
+        /* Clear any pending interrupt */
+        isi_readl(isi, ISI_STATUS);
+
+        start_dma(isi, isi->active);
+        spin_unlock_irq(&isi->irqlock);
+
+        return 0;
+
+err_reset:
+        pm_runtime_put(isi->dev);
+        v4l2_subdev_call(isi->entity.subdev, video, s_stream, 0);
+
+err_start_stream:
+        spin_lock_irq(&isi->irqlock);
+        isi->active = NULL;
+        /* Release all active buffers */
+        list_for_each_entry_safe(buf, node, &isi->video_buffer_list, list) {
+                list_del_init(&buf->list);
+                vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
+        }
+        spin_unlock_irq(&isi->irqlock);
+
+        return ret;
+}
+
+/* abort streaming and wait for last buffer */
+static void stop_streaming(struct vb2_queue *vq)
+{
+        struct atmel_isi *isi = vb2_get_drv_priv(vq);
+        struct frame_buffer *buf, *node;
+        int ret = 0;
+        unsigned long timeout;
+
+        /* Disable stream on the sub device */
+        ret = v4l2_subdev_call(isi->entity.subdev, video, s_stream, 0);
+        if (ret && ret != -ENOIOCTLCMD)
+                dev_err(isi->dev, "stream off failed in subdev\n");
+
+        spin_lock_irq(&isi->irqlock);
+        isi->active = NULL;
+        /* Release all active buffers */
+        list_for_each_entry_safe(buf, node, &isi->video_buffer_list, list) {
+                list_del_init(&buf->list);
+                vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+        }
+        spin_unlock_irq(&isi->irqlock);
+
+        if (!isi->enable_preview_path) {
+                timeout = jiffies + FRAME_INTERVAL_MILLI_SEC * HZ;
+                /* Wait until the end of the current frame. */
+                while ((isi_readl(isi, ISI_STATUS) & ISI_CTRL_CDC) &&
+                                time_before(jiffies, timeout))
+                        msleep(1);
+
+                if (time_after(jiffies, timeout))
+                        dev_err(isi->dev,
+                                "Timeout waiting for finishing codec request\n");
+        }
+
+        /* Disable interrupts */
+        isi_writel(isi, ISI_INTDIS,
+                        ISI_SR_CXFR_DONE | ISI_SR_PXFR_DONE);
+
+        /* Disable ISI and wait for it is done */
+        ret = atmel_isi_wait_status(isi, WAIT_ISI_DISABLE);
+        if (ret < 0)
+                dev_err(isi->dev, "Disable ISI timed out\n");
+
+        pm_runtime_put(isi->dev);
+}
+
+static const struct vb2_ops isi_video_qops = {
+        .queue_setup            = queue_setup,
+        .buf_init               = buffer_init,
+        .buf_prepare            = buffer_prepare,
+        .buf_cleanup            = buffer_cleanup,
+        .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,
+                              struct v4l2_format *fmt)
+{
+        struct atmel_isi *isi = video_drvdata(file);
+
+        *fmt = isi->fmt;
+
+        return 0;
+}
+
+static const struct isi_format *find_format_by_fourcc(struct atmel_isi *isi,
+                                                      unsigned int fourcc)
+{
+        unsigned int num_formats = isi->num_user_formats;
+        const struct isi_format *fmt;
+        unsigned int i;
+
+        for (i = 0; i < num_formats; i++) {
+                fmt = isi->user_formats[i];
+                if (fmt->fourcc == fourcc)
+                        return fmt;
+        }
+
+        return NULL;
+}
+
+static int isi_try_fmt(struct atmel_isi *isi, struct v4l2_format *f,
+                       const struct isi_format **current_fmt)
+{
+        const struct isi_format *isi_fmt;
+        struct v4l2_pix_format *pixfmt = &f->fmt.pix;
+        struct v4l2_subdev_pad_config pad_cfg;
+        struct v4l2_subdev_format format = {
+                .which = V4L2_SUBDEV_FORMAT_TRY,
+        };
+        int ret;
+
+        if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+                return -EINVAL;
+
+        isi_fmt = find_format_by_fourcc(isi, pixfmt->pixelformat);
+        if (!isi_fmt) {
+                isi_fmt = isi->user_formats[isi->num_user_formats - 1];
+                pixfmt->pixelformat = isi_fmt->fourcc;
+        }
+
+        /* Limit to Atmel ISC hardware capabilities */
+        if (pixfmt->width > MAX_SUPPORT_WIDTH)
+                pixfmt->width = MAX_SUPPORT_WIDTH;
+        if (pixfmt->height > MAX_SUPPORT_HEIGHT)
+                pixfmt->height = MAX_SUPPORT_HEIGHT;
+
+        v4l2_fill_mbus_format(&format.format, pixfmt, isi_fmt->mbus_code);
+        ret = v4l2_subdev_call(isi->entity.subdev, pad, set_fmt,
+                               &pad_cfg, &format);
+        if (ret < 0)
+                return ret;
+
+        v4l2_fill_pix_format(pixfmt, &format.format);
+
+        pixfmt->field = V4L2_FIELD_NONE;
+        pixfmt->bytesperline = pixfmt->width * isi_fmt->bpp;
+        pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
+
+        if (current_fmt)
+                *current_fmt = isi_fmt;
+
+        return 0;
+}
+
+static int isi_set_fmt(struct atmel_isi *isi, struct v4l2_format *f)
+{
+        struct v4l2_subdev_format format = {
+                .which = V4L2_SUBDEV_FORMAT_ACTIVE,
+        };
+        const struct isi_format *current_fmt;
+        int ret;
+
+        ret = isi_try_fmt(isi, f, &current_fmt);
+        if (ret)
+                return ret;
+
+        v4l2_fill_mbus_format(&format.format, &f->fmt.pix,
+                              current_fmt->mbus_code);
+        ret = v4l2_subdev_call(isi->entity.subdev, pad,
+                               set_fmt, NULL, &format);
+        if (ret < 0)
+                return ret;
+
+        isi->fmt = *f;
+        isi->current_fmt = current_fmt;
+
+        return 0;
+}
+
+static int isi_s_fmt_vid_cap(struct file *file, void *priv,
+                              struct v4l2_format *f)
+{
+        struct atmel_isi *isi = video_drvdata(file);
+
+        if (vb2_is_streaming(&isi->queue))
+                return -EBUSY;
+
+        return isi_set_fmt(isi, f);
+}
+
+static int isi_try_fmt_vid_cap(struct file *file, void *priv,
+                                struct v4l2_format *f)
+{
+        struct atmel_isi *isi = video_drvdata(file);
+
+        return isi_try_fmt(isi, f, NULL);
+}
+
+static int isi_enum_fmt_vid_cap(struct file *file, void  *priv,
+                                struct v4l2_fmtdesc *f)
+{
+        struct atmel_isi *isi = video_drvdata(file);
+
+        if (f->index >= isi->num_user_formats)
+                return -EINVAL;
+
+        f->pixelformat = isi->user_formats[f->index]->fourcc;
+        return 0;
+}
+
+static int isi_querycap(struct file *file, void *priv,
+                        struct v4l2_capability *cap)
+{
+        strlcpy(cap->driver, "atmel-isi", sizeof(cap->driver));
+        strlcpy(cap->card, "Atmel Image Sensor Interface", sizeof(cap->card));
+        strlcpy(cap->bus_info, "platform:isi", sizeof(cap->bus_info));
+        return 0;
+}
+
+static int isi_enum_input(struct file *file, void *priv,
+                           struct v4l2_input *i)
+{
+        if (i->index != 0)
+                return -EINVAL;
+
+        i->type = V4L2_INPUT_TYPE_CAMERA;
+        strlcpy(i->name, "Camera", sizeof(i->name));
+        return 0;
+}
+
+static int isi_g_input(struct file *file, void *priv, unsigned int *i)
+{
+        *i = 0;
+        return 0;
+}
+
+static int isi_s_input(struct file *file, void *priv, unsigned int i)
+{
+        if (i > 0)
+                return -EINVAL;
+        return 0;
+}
+
+static int isi_g_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
+{
+        struct atmel_isi *isi = video_drvdata(file);
+
+        if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+                return -EINVAL;
+
+        a->parm.capture.readbuffers = 2;
+        return v4l2_subdev_call(isi->entity.subdev, video, g_parm, a);
+}
+
+static int isi_s_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
+{
+        struct atmel_isi *isi = video_drvdata(file);
+
+        if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+                return -EINVAL;
+
+        a->parm.capture.readbuffers = 2;
+        return v4l2_subdev_call(isi->entity.subdev, video, s_parm, a);
+}
+
+static int isi_enum_framesizes(struct file *file, void *fh,
+                               struct v4l2_frmsizeenum *fsize)
+{
+        struct atmel_isi *isi = video_drvdata(file);
+        const struct isi_format *isi_fmt;
+        struct v4l2_subdev_frame_size_enum fse = {
+                .index = fsize->index,
+                .which = V4L2_SUBDEV_FORMAT_ACTIVE,
+        };
+        int ret;
+
+        isi_fmt = find_format_by_fourcc(isi, fsize->pixel_format);
+        if (!isi_fmt)
+                return -EINVAL;
+
+        fse.code = isi_fmt->mbus_code;
+
+        ret = v4l2_subdev_call(isi->entity.subdev, pad, enum_frame_size,
+                               NULL, &fse);
+        if (ret)
+                return ret;
+
+        fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+        fsize->discrete.width = fse.max_width;
+        fsize->discrete.height = fse.max_height;
+
+        return 0;
+}
+
+static int isi_enum_frameintervals(struct file *file, void *fh,
+                                    struct v4l2_frmivalenum *fival)
+{
+        struct atmel_isi *isi = video_drvdata(file);
+        const struct isi_format *isi_fmt;
+        struct v4l2_subdev_frame_interval_enum fie = {
+                .index = fival->index,
+                .width = fival->width,
+                .height = fival->height,
+                .which = V4L2_SUBDEV_FORMAT_ACTIVE,
+        };
+        int ret;
+
+        isi_fmt = find_format_by_fourcc(isi, fival->pixel_format);
+        if (!isi_fmt)
+                return -EINVAL;
+
+        fie.code = isi_fmt->mbus_code;
+
+        ret = v4l2_subdev_call(isi->entity.subdev, pad,
+                               enum_frame_interval, NULL, &fie);
+        if (ret)
+                return ret;
+
+        fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+        fival->discrete = fie.interval;
+
+        return 0;
+}
+
+static void isi_camera_set_bus_param(struct atmel_isi *isi)
+{
+        u32 cfg1 = 0;
+
+        /* set bus param for ISI */
+        if (isi->pdata.hsync_act_low)
+                cfg1 |= ISI_CFG1_HSYNC_POL_ACTIVE_LOW;
+        if (isi->pdata.vsync_act_low)
+                cfg1 |= ISI_CFG1_VSYNC_POL_ACTIVE_LOW;
+        if (isi->pdata.pclk_act_falling)
+                cfg1 |= ISI_CFG1_PIXCLK_POL_ACTIVE_FALLING;
+        if (isi->pdata.has_emb_sync)
+                cfg1 |= ISI_CFG1_EMB_SYNC;
+        if (isi->pdata.full_mode)
+                cfg1 |= ISI_CFG1_FULL_MODE;
+
+        cfg1 |= ISI_CFG1_THMASK_BEATS_16;
+
+        /* Enable PM and peripheral clock before operate isi registers */
+        pm_runtime_get_sync(isi->dev);
+
+        isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
+        isi_writel(isi, ISI_CFG1, cfg1);
+
+        pm_runtime_put(isi->dev);
+}
+
+/* -----------------------------------------------------------------------*/
+static int atmel_isi_parse_dt(struct atmel_isi *isi,
+                        struct platform_device *pdev)
+{
+        struct device_node *np = pdev->dev.of_node;
+        struct v4l2_of_endpoint ep;
+        int err;
+
+        /* Default settings for ISI */
+        isi->pdata.full_mode = 1;
+        isi->pdata.frate = ISI_CFG1_FRATE_CAPTURE_ALL;
+
+        np = of_graph_get_next_endpoint(np, NULL);
+        if (!np) {
+                dev_err(&pdev->dev, "Could not find the endpoint\n");
+                return -EINVAL;
+        }
+
+        err = v4l2_of_parse_endpoint(np, &ep);
+        of_node_put(np);
+        if (err) {
+                dev_err(&pdev->dev, "Could not parse the endpoint\n");
+                return err;
+        }
+
+        switch (ep.bus.parallel.bus_width) {
+        case 8:
+                isi->pdata.data_width_flags = ISI_DATAWIDTH_8;
+                break;
+        case 10:
+                isi->pdata.data_width_flags =
+                                ISI_DATAWIDTH_8 | ISI_DATAWIDTH_10;
+                break;
+        default:
+                dev_err(&pdev->dev, "Unsupported bus width: %d\n",
+                                ep.bus.parallel.bus_width);
+                return -EINVAL;
+        }
+
+        if (ep.bus.parallel.flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+                isi->pdata.hsync_act_low = true;
+        if (ep.bus.parallel.flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+                isi->pdata.vsync_act_low = true;
+        if (ep.bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
+                isi->pdata.pclk_act_falling = true;
+
+        if (ep.bus_type == V4L2_MBUS_BT656)
+                isi->pdata.has_emb_sync = true;
+
+        return 0;
+}
+
+static int isi_open(struct file *file)
+{
+        struct atmel_isi *isi = video_drvdata(file);
+        struct v4l2_subdev *sd = isi->entity.subdev;
+        int ret;
+
+        if (mutex_lock_interruptible(&isi->lock))
+                return -ERESTARTSYS;
+
+        ret = v4l2_fh_open(file);
+        if (ret < 0)
+                goto unlock;
+
+        if (!v4l2_fh_is_singular_file(file))
+                goto fh_rel;
+
+        ret = v4l2_subdev_call(sd, core, s_power, 1);
+        if (ret < 0 && ret != -ENOIOCTLCMD)
+                goto fh_rel;
+
+        ret = isi_set_fmt(isi, &isi->fmt);
+        if (ret)
+                v4l2_subdev_call(sd, core, s_power, 0);
+fh_rel:
+        if (ret)
+                v4l2_fh_release(file);
+unlock:
+        mutex_unlock(&isi->lock);
+        return ret;
+}
+
+static int isi_release(struct file *file)
+{
+        struct atmel_isi *isi = video_drvdata(file);
+        struct v4l2_subdev *sd = isi->entity.subdev;
+        bool fh_singular;
+        int ret;
+
+        mutex_lock(&isi->lock);
+
+        fh_singular = v4l2_fh_is_singular_file(file);
+
+        ret = _vb2_fop_release(file, NULL);
+
+        if (fh_singular)
+                v4l2_subdev_call(sd, core, s_power, 0);
+
+        mutex_unlock(&isi->lock);
+
+        return ret;
+}
+
+static const struct v4l2_ioctl_ops isi_ioctl_ops = {
+        .vidioc_querycap                = isi_querycap,
+
+        .vidioc_try_fmt_vid_cap         = isi_try_fmt_vid_cap,
+        .vidioc_g_fmt_vid_cap           = isi_g_fmt_vid_cap,
+        .vidioc_s_fmt_vid_cap           = isi_s_fmt_vid_cap,
+        .vidioc_enum_fmt_vid_cap        = isi_enum_fmt_vid_cap,
+
+        .vidioc_enum_input              = isi_enum_input,
+        .vidioc_g_input                 = isi_g_input,
+        .vidioc_s_input                 = isi_s_input,
+
+        .vidioc_g_parm                  = isi_g_parm,
+        .vidioc_s_parm                  = isi_s_parm,
+        .vidioc_enum_framesizes         = isi_enum_framesizes,
+        .vidioc_enum_frameintervals     = isi_enum_frameintervals,
+
+        .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
+        .vidioc_create_bufs             = vb2_ioctl_create_bufs,
+        .vidioc_querybuf                = vb2_ioctl_querybuf,
+        .vidioc_qbuf                    = vb2_ioctl_qbuf,
+        .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
+        .vidioc_expbuf                  = vb2_ioctl_expbuf,
+        .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         = v4l2_ctrl_subscribe_event,
+        .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
+};
+
+static const struct v4l2_file_operations isi_fops = {
+        .owner          = THIS_MODULE,
+        .unlocked_ioctl = video_ioctl2,
+        .open           = isi_open,
+        .release        = isi_release,
+        .poll           = vb2_fop_poll,
+        .mmap           = vb2_fop_mmap,
+        .read           = vb2_fop_read,
+};
+
+static int isi_set_default_fmt(struct atmel_isi *isi)
+{
+        struct v4l2_format f = {
+                .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+                .fmt.pix = {
+                        .width          = VGA_WIDTH,
+                        .height         = VGA_HEIGHT,
+                        .field          = V4L2_FIELD_NONE,
+                        .pixelformat    = isi->user_formats[0]->fourcc,
+                },
+        };
+        int ret;
+
+        ret = isi_try_fmt(isi, &f, NULL);
+        if (ret)
+                return ret;
+        isi->current_fmt = isi->user_formats[0];
+        isi->fmt = f;
+        return 0;
+}
+
+static const struct isi_format isi_formats[] = {
+        {
+                .fourcc = V4L2_PIX_FMT_YUYV,
+                .mbus_code = MEDIA_BUS_FMT_YUYV8_2X8,
+                .bpp = 2,
+                .swap = ISI_CFG2_YCC_SWAP_DEFAULT,
+        }, {
+                .fourcc = V4L2_PIX_FMT_YUYV,
+                .mbus_code = MEDIA_BUS_FMT_YVYU8_2X8,
+                .bpp = 2,
+                .swap = ISI_CFG2_YCC_SWAP_MODE_1,
+        }, {
+                .fourcc = V4L2_PIX_FMT_YUYV,
+                .mbus_code = MEDIA_BUS_FMT_UYVY8_2X8,
+                .bpp = 2,
+                .swap = ISI_CFG2_YCC_SWAP_MODE_2,
+        }, {
+                .fourcc = V4L2_PIX_FMT_YUYV,
+                .mbus_code = MEDIA_BUS_FMT_VYUY8_2X8,
+                .bpp = 2,
+                .swap = ISI_CFG2_YCC_SWAP_MODE_3,
+        }, {
+                .fourcc = V4L2_PIX_FMT_RGB565,
+                .mbus_code = MEDIA_BUS_FMT_YUYV8_2X8,
+                .bpp = 2,
+                .swap = ISI_CFG2_YCC_SWAP_MODE_2,
+        }, {
+                .fourcc = V4L2_PIX_FMT_RGB565,
+                .mbus_code = MEDIA_BUS_FMT_YVYU8_2X8,
+                .bpp = 2,
+                .swap = ISI_CFG2_YCC_SWAP_MODE_3,
+        }, {
+                .fourcc = V4L2_PIX_FMT_RGB565,
+                .mbus_code = MEDIA_BUS_FMT_UYVY8_2X8,
+                .bpp = 2,
+                .swap = ISI_CFG2_YCC_SWAP_DEFAULT,
+        }, {
+                .fourcc = V4L2_PIX_FMT_RGB565,
+                .mbus_code = MEDIA_BUS_FMT_VYUY8_2X8,
+                .bpp = 2,
+                .swap = ISI_CFG2_YCC_SWAP_MODE_1,
+        },
+};
+
+static int isi_formats_init(struct atmel_isi *isi)
+{
+        const struct isi_format *isi_fmts[ARRAY_SIZE(isi_formats)];
+        unsigned int num_fmts = 0, i, j;
+        struct v4l2_subdev *subdev = isi->entity.subdev;
+        struct v4l2_subdev_mbus_code_enum mbus_code = {
+                .which = V4L2_SUBDEV_FORMAT_ACTIVE,
+        };
+
+        while (!v4l2_subdev_call(subdev, pad, enum_mbus_code,
+                                 NULL, &mbus_code)) {
+                for (i = 0; i < ARRAY_SIZE(isi_formats); i++) {
+                        if (isi_formats[i].mbus_code != mbus_code.code)
+                                continue;
+
+                        /* Code supported, have we got this fourcc yet? */
+                        for (j = 0; j < num_fmts; j++)
+                                if (isi_fmts[j]->fourcc == isi_formats[i].fourcc)
+                                        /* Already available */
+                                        break;
+                        if (j == num_fmts)
+                                /* new */
+                                isi_fmts[num_fmts++] = isi_formats + i;
+                }
+                mbus_code.index++;
+        }
+
+        if (!num_fmts)
+                return -ENXIO;
+
+        isi->num_user_formats = num_fmts;
+        isi->user_formats = devm_kcalloc(isi->dev,
+                                         num_fmts, sizeof(struct isi_format *),
+                                         GFP_KERNEL);
+        if (!isi->user_formats) {
+                dev_err(isi->dev, "could not allocate memory\n");
+                return -ENOMEM;
+        }
+
+        memcpy(isi->user_formats, isi_fmts,
+               num_fmts * sizeof(struct isi_format *));
+        isi->current_fmt = isi->user_formats[0];
+
+        return 0;
+}
+
+static int isi_graph_notify_complete(struct v4l2_async_notifier *notifier)
+{
+        struct atmel_isi *isi = notifier_to_isi(notifier);
+        int ret;
+
+        isi->vdev->ctrl_handler = isi->entity.subdev->ctrl_handler;
+        ret = isi_formats_init(isi);
+        if (ret) {
+                dev_err(isi->dev, "No supported mediabus format found\n");
+                return ret;
+        }
+        isi_camera_set_bus_param(isi);
+
+        ret = isi_set_default_fmt(isi);
+        if (ret) {
+                dev_err(isi->dev, "Could not set default format\n");
+                return ret;
+        }
+
+        ret = video_register_device(isi->vdev, VFL_TYPE_GRABBER, -1);
+        if (ret) {
+                dev_err(isi->dev, "Failed to register video device\n");
+                return ret;
+        }
+
+        dev_dbg(isi->dev, "Device registered as %s\n",
+                video_device_node_name(isi->vdev));
+        return 0;
+}
+
+static void isi_graph_notify_unbind(struct v4l2_async_notifier *notifier,
+                                     struct v4l2_subdev *sd,
+                                     struct v4l2_async_subdev *asd)
+{
+        struct atmel_isi *isi = notifier_to_isi(notifier);
+
+        dev_dbg(isi->dev, "Removing %s\n", video_device_node_name(isi->vdev));
+
+        /* Checks internaly if vdev have been init or not */
+        video_unregister_device(isi->vdev);
+}
+
+static int isi_graph_notify_bound(struct v4l2_async_notifier *notifier,
+                                   struct v4l2_subdev *subdev,
+                                   struct v4l2_async_subdev *asd)
+{
+        struct atmel_isi *isi = notifier_to_isi(notifier);
+
+        dev_dbg(isi->dev, "subdev %s bound\n", subdev->name);
+
+        isi->entity.subdev = subdev;
+
+        return 0;
+}
+
+static int isi_graph_parse(struct atmel_isi *isi, struct device_node *node)
+{
+        struct device_node *ep = NULL;
+        struct device_node *remote;
+
+        while (1) {
+                ep = of_graph_get_next_endpoint(node, ep);
+                if (!ep)
+                        return -EINVAL;
+
+                remote = of_graph_get_remote_port_parent(ep);
+                if (!remote) {
+                        of_node_put(ep);
+                        return -EINVAL;
+                }
+
+                /* Remote node to connect */
+                isi->entity.node = remote;
+                isi->entity.asd.match_type = V4L2_ASYNC_MATCH_OF;
+                isi->entity.asd.match.of.node = remote;
+                return 0;
+        }
+}
+
+static int isi_graph_init(struct atmel_isi *isi)
+{
+        struct v4l2_async_subdev **subdevs = NULL;
+        int ret;
+
+        /* Parse the graph to extract a list of subdevice DT nodes. */
+        ret = isi_graph_parse(isi, isi->dev->of_node);
+        if (ret < 0) {
+                dev_err(isi->dev, "Graph parsing failed\n");
+                return ret;
+        }
+
+        /* Register the subdevices notifier. */
+        subdevs = devm_kzalloc(isi->dev, sizeof(*subdevs), GFP_KERNEL);
+        if (subdevs == NULL) {
+                of_node_put(isi->entity.node);
+                return -ENOMEM;
+        }
+
+        subdevs[0] = &isi->entity.asd;
+
+        isi->notifier.subdevs = subdevs;
+        isi->notifier.num_subdevs = 1;
+        isi->notifier.bound = isi_graph_notify_bound;
+        isi->notifier.unbind = isi_graph_notify_unbind;
+        isi->notifier.complete = isi_graph_notify_complete;
+
+        ret = v4l2_async_notifier_register(&isi->v4l2_dev, &isi->notifier);
+        if (ret < 0) {
+                dev_err(isi->dev, "Notifier registration failed\n");
+                of_node_put(isi->entity.node);
+                return ret;
+        }
+
+        return 0;
+}
+
+
+static int atmel_isi_probe(struct platform_device *pdev)
+{
+        int irq;
+        struct atmel_isi *isi;
+        struct vb2_queue *q;
+        struct resource *regs;
+        int ret, i;
+
+        isi = devm_kzalloc(&pdev->dev, sizeof(struct atmel_isi), GFP_KERNEL);
+        if (!isi) {
+                dev_err(&pdev->dev, "Can't allocate interface!\n");
+                return -ENOMEM;
+        }
+
+        isi->pclk = devm_clk_get(&pdev->dev, "isi_clk");
+        if (IS_ERR(isi->pclk))
+                return PTR_ERR(isi->pclk);
+
+        ret = atmel_isi_parse_dt(isi, pdev);
+        if (ret)
+                return ret;
+
+        isi->active = NULL;
+        isi->dev = &pdev->dev;
+        mutex_init(&isi->lock);
+        spin_lock_init(&isi->irqlock);
+        INIT_LIST_HEAD(&isi->video_buffer_list);
+        INIT_LIST_HEAD(&isi->dma_desc_head);
+
+        q = &isi->queue;
+
+        /* Initialize the top-level structure */
+        ret = v4l2_device_register(&pdev->dev, &isi->v4l2_dev);
+        if (ret)
+                return ret;
+
+        isi->vdev = video_device_alloc();
+        if (isi->vdev == NULL) {
+                ret = -ENOMEM;
+                goto err_vdev_alloc;
+        }
+
+        /* video node */
+        isi->vdev->fops = &isi_fops;
+        isi->vdev->v4l2_dev = &isi->v4l2_dev;
+        isi->vdev->queue = &isi->queue;
+        strlcpy(isi->vdev->name, KBUILD_MODNAME, sizeof(isi->vdev->name));
+        isi->vdev->release = video_device_release;
+        isi->vdev->ioctl_ops = &isi_ioctl_ops;
+        isi->vdev->lock = &isi->lock;
+        isi->vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
+                V4L2_CAP_READWRITE;
+        video_set_drvdata(isi->vdev, isi);
+
+        /* buffer queue */
+        q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        q->io_modes = VB2_MMAP | VB2_READ | VB2_DMABUF;
+        q->lock = &isi->lock;
+        q->drv_priv = isi;
+        q->buf_struct_size = sizeof(struct frame_buffer);
+        q->ops = &isi_video_qops;
+        q->mem_ops = &vb2_dma_contig_memops;
+        q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+        q->min_buffers_needed = 2;
+        q->dev = &pdev->dev;
+
+        ret = vb2_queue_init(q);
+        if (ret < 0) {
+                dev_err(&pdev->dev, "failed to initialize VB2 queue\n");
+                goto err_vb2_queue;
+        }
+        isi->p_fb_descriptors = dma_alloc_coherent(&pdev->dev,
+                                sizeof(struct fbd) * VIDEO_MAX_FRAME,
+                                &isi->fb_descriptors_phys,
+                                GFP_KERNEL);
+        if (!isi->p_fb_descriptors) {
+                dev_err(&pdev->dev, "Can't allocate descriptors!\n");
+                ret = -ENOMEM;
+                goto err_dma_alloc;
+        }
+
+        for (i = 0; i < VIDEO_MAX_FRAME; i++) {
+                isi->dma_desc[i].p_fbd = isi->p_fb_descriptors + i;
+                isi->dma_desc[i].fbd_phys = isi->fb_descriptors_phys +
+                                        i * sizeof(struct fbd);
+                list_add(&isi->dma_desc[i].list, &isi->dma_desc_head);
+        }
+
+        regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        isi->regs = devm_ioremap_resource(&pdev->dev, regs);
+        if (IS_ERR(isi->regs)) {
+                ret = PTR_ERR(isi->regs);
+                goto err_ioremap;
+        }
+
+        if (isi->pdata.data_width_flags & ISI_DATAWIDTH_8)
+                isi->width_flags = 1 << 7;
+        if (isi->pdata.data_width_flags & ISI_DATAWIDTH_10)
+                isi->width_flags |= 1 << 9;
+
+        irq = platform_get_irq(pdev, 0);
+        if (irq < 0) {
+                ret = irq;
+                goto err_req_irq;
+        }
+
+        ret = devm_request_irq(&pdev->dev, irq, isi_interrupt, 0, "isi", isi);
+        if (ret) {
+                dev_err(&pdev->dev, "Unable to request irq %d\n", irq);
+                goto err_req_irq;
+        }
+        isi->irq = irq;
+
+        ret = isi_graph_init(isi);
+        if (ret < 0)
+                goto err_req_irq;
+
+        pm_suspend_ignore_children(&pdev->dev, true);
+        pm_runtime_enable(&pdev->dev);
+        platform_set_drvdata(pdev, isi);
+        return 0;
+
+err_req_irq:
+err_ioremap:
+        dma_free_coherent(&pdev->dev,
+                        sizeof(struct fbd) * VIDEO_MAX_FRAME,
+                        isi->p_fb_descriptors,
+                        isi->fb_descriptors_phys);
+err_dma_alloc:
+err_vb2_queue:
+        video_device_release(isi->vdev);
+err_vdev_alloc:
+        v4l2_device_unregister(&isi->v4l2_dev);
+
+        return ret;
+}
+
+static int atmel_isi_remove(struct platform_device *pdev)
+{
+        struct atmel_isi *isi = platform_get_drvdata(pdev);
+
+        dma_free_coherent(&pdev->dev,
+                        sizeof(struct fbd) * VIDEO_MAX_FRAME,
+                        isi->p_fb_descriptors,
+                        isi->fb_descriptors_phys);
+        pm_runtime_disable(&pdev->dev);
+        v4l2_async_notifier_unregister(&isi->notifier);
+        v4l2_device_unregister(&isi->v4l2_dev);
+
+        return 0;
+}
+
+#ifdef CONFIG_PM
+static int atmel_isi_runtime_suspend(struct device *dev)
+{
+        struct atmel_isi *isi = dev_get_drvdata(dev);
+
+        clk_disable_unprepare(isi->pclk);
+
+        return 0;
+}
+static int atmel_isi_runtime_resume(struct device *dev)
+{
+        struct atmel_isi *isi = dev_get_drvdata(dev);
+
+        return clk_prepare_enable(isi->pclk);
+}
+#endif /* CONFIG_PM */
+
+static const struct dev_pm_ops atmel_isi_dev_pm_ops = {
+        SET_RUNTIME_PM_OPS(atmel_isi_runtime_suspend,
+                                atmel_isi_runtime_resume, NULL)
+};
+
+static const struct of_device_id atmel_isi_of_match[] = {
+        { .compatible = "atmel,at91sam9g45-isi" },
+        { }
+};
+MODULE_DEVICE_TABLE(of, atmel_isi_of_match);
+
+static struct platform_driver atmel_isi_driver = {
+        .driver         = {
+                .name = "atmel_isi",
+                .of_match_table = of_match_ptr(atmel_isi_of_match),
+                .pm     = &atmel_isi_dev_pm_ops,
+        },
+        .probe          = atmel_isi_probe,
+        .remove         = atmel_isi_remove,
+};
+
+module_platform_driver(atmel_isi_driver);
+
+MODULE_AUTHOR("Josh Wu <josh.wu@atmel.com>");
+MODULE_DESCRIPTION("The V4L2 driver for Atmel Linux");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("video");
diff --git a/drivers/media/platform/soc_camera/atmel-isi.h b/drivers/media/platform/atmel/atmel-isi.h
index 0acb32a2b65c..0acb32a2b65c 100644
--- a/drivers/media/platform/soc_camera/atmel-isi.h
+++ b/drivers/media/platform/atmel/atmel-isi.h
diff --git a/drivers/media/platform/coda/coda-bit.c b/drivers/media/platform/coda/coda-bit.c
index 466a44e4549e..403214e00e95 100644
--- a/drivers/media/platform/coda/coda-bit.c
+++ b/drivers/media/platform/coda/coda-bit.c
@@ -179,6 +179,25 @@ static void coda_kfifo_sync_to_device_write(struct coda_ctx *ctx)
         coda_write(dev, wr_ptr, CODA_REG_BIT_WR_PTR(ctx->reg_idx));
 }
 
+static int coda_bitstream_pad(struct coda_ctx *ctx, u32 size)
+{
+        unsigned char *buf;
+        u32 n;
+
+        if (size < 6)
+                size = 6;
+
+        buf = kmalloc(size, GFP_KERNEL);
+        if (!buf)
+                return -ENOMEM;
+
+        coda_h264_filler_nal(size, buf);
+        n = kfifo_in(&ctx->bitstream_fifo, buf, size);
+        kfree(buf);
+
+        return (n < size) ? -ENOSPC : 0;
+}
+
 static int coda_bitstream_queue(struct coda_ctx *ctx,
                                 struct vb2_v4l2_buffer *src_buf)
 {
@@ -198,10 +217,10 @@ static int coda_bitstream_queue(struct coda_ctx *ctx,
 static bool coda_bitstream_try_queue(struct coda_ctx *ctx,
                                      struct vb2_v4l2_buffer *src_buf)
 {
+        unsigned long payload = vb2_get_plane_payload(&src_buf->vb2_buf, 0);
         int ret;
 
-        if (coda_get_bitstream_payload(ctx) +
-            vb2_get_plane_payload(&src_buf->vb2_buf, 0) + 512 >=
+        if (coda_get_bitstream_payload(ctx) + payload + 512 >=
             ctx->bitstream.size)
                 return false;
 
@@ -210,6 +229,11 @@ static bool coda_bitstream_try_queue(struct coda_ctx *ctx,
                 return true;
         }
 
+        /* Add zero padding before the first H.264 buffer, if it is too small */
+        if (ctx->qsequence == 0 && payload < 512 &&
+            ctx->codec->src_fourcc == V4L2_PIX_FMT_H264)
+                coda_bitstream_pad(ctx, 512 - payload);
+
         ret = coda_bitstream_queue(ctx, src_buf);
         if (ret < 0) {
                 v4l2_err(&ctx->dev->v4l2_dev, "bitstream buffer overflow\n");
@@ -224,7 +248,7 @@ static bool coda_bitstream_try_queue(struct coda_ctx *ctx,
         return true;
 }
 
-void coda_fill_bitstream(struct coda_ctx *ctx, bool streaming)
+void coda_fill_bitstream(struct coda_ctx *ctx, struct list_head *buffer_list)
 {
         struct vb2_v4l2_buffer *src_buf;
         struct coda_buffer_meta *meta;
@@ -252,9 +276,16 @@ void coda_fill_bitstream(struct coda_ctx *ctx, bool streaming)
                                  "dropping invalid JPEG frame %d\n",
                                  ctx->qsequence);
                         src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
-                        v4l2_m2m_buf_done(src_buf, streaming ?
-                                          VB2_BUF_STATE_ERROR :
-                                          VB2_BUF_STATE_QUEUED);
+                        if (buffer_list) {
+                                struct v4l2_m2m_buffer *m2m_buf;
+
+                                m2m_buf = container_of(src_buf,
+                                                       struct v4l2_m2m_buffer,
+                                                       vb);
+                                list_add_tail(&m2m_buf->list, buffer_list);
+                        } else {
+                                v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
+                        }
                         continue;
                 }
 
@@ -295,7 +326,16 @@ void coda_fill_bitstream(struct coda_ctx *ctx, bool streaming)
                                 trace_coda_bit_queue(ctx, src_buf, meta);
                         }
 
-                        v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
+                        if (buffer_list) {
+                                struct v4l2_m2m_buffer *m2m_buf;
+
+                                m2m_buf = container_of(src_buf,
+                                                       struct v4l2_m2m_buffer,
+                                                       vb);
+                                list_add_tail(&m2m_buf->list, buffer_list);
+                        } else {
+                                v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
+                        }
                 } else {
                         break;
                 }
@@ -1508,6 +1548,47 @@ static int coda_decoder_reqbufs(struct coda_ctx *ctx,
         return 0;
 }
 
+static bool coda_reorder_enable(struct coda_ctx *ctx)
+{
+        const char * const *profile_names;
+        const char * const *level_names;
+        struct coda_dev *dev = ctx->dev;
+        int profile, level;
+
+        if (dev->devtype->product != CODA_7541 &&
+            dev->devtype->product != CODA_960)
+                return false;
+
+        if (ctx->codec->src_fourcc == V4L2_PIX_FMT_JPEG)
+                return false;
+
+        if (ctx->codec->src_fourcc != V4L2_PIX_FMT_H264)
+                return true;
+
+        profile = coda_h264_profile(ctx->params.h264_profile_idc);
+        if (profile < 0) {
+                v4l2_warn(&dev->v4l2_dev, "Invalid H264 Profile: %d\n",
+                         ctx->params.h264_profile_idc);
+                return false;
+        }
+
+        level = coda_h264_level(ctx->params.h264_level_idc);
+        if (level < 0) {
+                v4l2_warn(&dev->v4l2_dev, "Invalid H264 Level: %d\n",
+                         ctx->params.h264_level_idc);
+                return false;
+        }
+
+        profile_names = v4l2_ctrl_get_menu(V4L2_CID_MPEG_VIDEO_H264_PROFILE);
+        level_names = v4l2_ctrl_get_menu(V4L2_CID_MPEG_VIDEO_H264_LEVEL);
+
+        v4l2_dbg(1, coda_debug, &dev->v4l2_dev, "H264 Profile/Level: %s L%s\n",
+                 profile_names[profile], level_names[level]);
+
+        /* Baseline profile does not support reordering */
+        return profile > V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE;
+}
+
 static int __coda_start_decoding(struct coda_ctx *ctx)
 {
         struct coda_q_data *q_data_src, *q_data_dst;
@@ -1554,8 +1635,7 @@ static int __coda_start_decoding(struct coda_ctx *ctx)
         coda_write(dev, bitstream_buf, CODA_CMD_DEC_SEQ_BB_START);
         coda_write(dev, bitstream_size / 1024, CODA_CMD_DEC_SEQ_BB_SIZE);
         val = 0;
-        if ((dev->devtype->product == CODA_7541) ||
-            (dev->devtype->product == CODA_960))
+        if (coda_reorder_enable(ctx))
                 val |= CODA_REORDER_ENABLE;
         if (ctx->codec->src_fourcc == V4L2_PIX_FMT_JPEG)
                 val |= CODA_NO_INT_ENABLE;
@@ -1747,7 +1827,7 @@ static int coda_prepare_decode(struct coda_ctx *ctx)
 
         /* Try to copy source buffer contents into the bitstream ringbuffer */
         mutex_lock(&ctx->bitstream_mutex);
-        coda_fill_bitstream(ctx, true);
+        coda_fill_bitstream(ctx, NULL);
         mutex_unlock(&ctx->bitstream_mutex);
 
         if (coda_get_bitstream_payload(ctx) < 512 &&
diff --git a/drivers/media/platform/coda/coda-common.c b/drivers/media/platform/coda/coda-common.c
index eb6548f46cba..d523e990d509 100644
--- a/drivers/media/platform/coda/coda-common.c
+++ b/drivers/media/platform/coda/coda-common.c
@@ -71,6 +71,10 @@ static int disable_vdoa;
 module_param(disable_vdoa, int, 0644);
 MODULE_PARM_DESC(disable_vdoa, "Disable Video Data Order Adapter tiled to raster-scan conversion");
 
+static int enable_bwb = 0;
+module_param(enable_bwb, int, 0644);
+MODULE_PARM_DESC(enable_bwb, "Enable BWB unit, may crash on certain streams");
+
 void coda_write(struct coda_dev *dev, u32 data, u32 reg)
 {
         v4l2_dbg(2, coda_debug, &dev->v4l2_dev,
@@ -386,6 +390,7 @@ static int coda_enum_fmt(struct file *file, void *priv,
 {
         struct video_device *vdev = video_devdata(file);
         const struct coda_video_device *cvd = to_coda_video_device(vdev);
+        struct coda_ctx *ctx = fh_to_ctx(priv);
         const u32 *formats;
 
         if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
@@ -398,6 +403,11 @@ static int coda_enum_fmt(struct file *file, void *priv,
         if (f->index >= CODA_MAX_FORMATS || formats[f->index] == 0)
                 return -EINVAL;
 
+        /* Skip YUYV if the vdoa is not available */
+        if (!ctx->vdoa && f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+            formats[f->index] == V4L2_PIX_FMT_YUYV)
+                return -EINVAL;
+
         f->pixelformat = formats[f->index];
 
         return 0;
@@ -813,10 +823,6 @@ static int coda_qbuf(struct file *file, void *priv,
 static bool coda_buf_is_end_of_stream(struct coda_ctx *ctx,
                                       struct vb2_v4l2_buffer *buf)
 {
-        struct vb2_queue *src_vq;
-
-        src_vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
-
         return ((ctx->bit_stream_param & CODA_BIT_STREAM_END_FLAG) &&
                 (buf->sequence == (ctx->qsequence - 1)));
 }
@@ -881,6 +887,47 @@ static int coda_g_selection(struct file *file, void *fh,
         return 0;
 }
 
+static int coda_try_encoder_cmd(struct file *file, void *fh,
+                                struct v4l2_encoder_cmd *ec)
+{
+        if (ec->cmd != V4L2_ENC_CMD_STOP)
+                return -EINVAL;
+
+        if (ec->flags & V4L2_ENC_CMD_STOP_AT_GOP_END)
+                return -EINVAL;
+
+        return 0;
+}
+
+static int coda_encoder_cmd(struct file *file, void *fh,
+                            struct v4l2_encoder_cmd *ec)
+{
+        struct coda_ctx *ctx = fh_to_ctx(fh);
+        struct vb2_queue *dst_vq;
+        int ret;
+
+        ret = coda_try_encoder_cmd(file, fh, ec);
+        if (ret < 0)
+                return ret;
+
+        /* Ignore encoder stop command silently in decoder context */
+        if (ctx->inst_type != CODA_INST_ENCODER)
+                return 0;
+
+        /* Set the stream-end flag on this context */
+        ctx->bit_stream_param |= CODA_BIT_STREAM_END_FLAG;
+
+        /* If there is no buffer in flight, wake up */
+        if (ctx->qsequence == ctx->osequence) {
+                dst_vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx,
+                                         V4L2_BUF_TYPE_VIDEO_CAPTURE);
+                dst_vq->last_buffer_dequeued = true;
+                wake_up(&dst_vq->done_wq);
+        }
+
+        return 0;
+}
+
 static int coda_try_decoder_cmd(struct file *file, void *fh,
                                 struct v4l2_decoder_cmd *dc)
 {
@@ -1054,6 +1101,8 @@ static const struct v4l2_ioctl_ops coda_ioctl_ops = {
 
         .vidioc_g_selection     = coda_g_selection,
 
+        .vidioc_try_encoder_cmd = coda_try_encoder_cmd,
+        .vidioc_encoder_cmd     = coda_encoder_cmd,
         .vidioc_try_decoder_cmd = coda_try_decoder_cmd,
         .vidioc_decoder_cmd     = coda_decoder_cmd,
 
@@ -1327,12 +1376,28 @@ static void coda_buf_queue(struct vb2_buffer *vb)
                  */
                 if (vb2_get_plane_payload(vb, 0) == 0)
                         coda_bit_stream_end_flag(ctx);
+
+                if (q_data->fourcc == V4L2_PIX_FMT_H264) {
+                        /*
+                         * Unless already done, try to obtain profile_idc and
+                         * level_idc from the SPS header. This allows to decide
+                         * whether to enable reordering during sequence
+                         * initialization.
+                         */
+                        if (!ctx->params.h264_profile_idc)
+                                coda_sps_parse_profile(ctx, vb);
+                }
+
                 mutex_lock(&ctx->bitstream_mutex);
                 v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
                 if (vb2_is_streaming(vb->vb2_queue))
-                        coda_fill_bitstream(ctx, true);
+                        /* This set buf->sequence = ctx->qsequence++ */
+                        coda_fill_bitstream(ctx, NULL);
                 mutex_unlock(&ctx->bitstream_mutex);
         } else {
+                if (ctx->inst_type == CODA_INST_ENCODER &&
+                    vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+                        vbuf->sequence = ctx->qsequence++;
                 v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
         }
 }
@@ -1344,7 +1409,7 @@ int coda_alloc_aux_buf(struct coda_dev *dev, struct coda_aux_buf *buf,
                                         GFP_KERNEL);
         if (!buf->vaddr) {
                 v4l2_err(&dev->v4l2_dev,
-                         "Failed to allocate %s buffer of size %u\n",
+                         "Failed to allocate %s buffer of size %zu\n",
                          name, size);
                 return -ENOMEM;
         }
@@ -1382,18 +1447,22 @@ static int coda_start_streaming(struct vb2_queue *q, unsigned int count)
         struct coda_ctx *ctx = vb2_get_drv_priv(q);
         struct v4l2_device *v4l2_dev = &ctx->dev->v4l2_dev;
         struct coda_q_data *q_data_src, *q_data_dst;
+        struct v4l2_m2m_buffer *m2m_buf, *tmp;
         struct vb2_v4l2_buffer *buf;
+        struct list_head list;
         int ret = 0;
 
         if (count < 1)
                 return -EINVAL;
 
+        INIT_LIST_HEAD(&list);
+
         q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
         if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
                 if (ctx->inst_type == CODA_INST_DECODER && ctx->use_bit) {
                         /* copy the buffers that were queued before streamon */
                         mutex_lock(&ctx->bitstream_mutex);
-                        coda_fill_bitstream(ctx, false);
+                        coda_fill_bitstream(ctx, &list);
                         mutex_unlock(&ctx->bitstream_mutex);
 
                         if (coda_get_bitstream_payload(ctx) < 512) {
@@ -1408,8 +1477,8 @@ static int coda_start_streaming(struct vb2_queue *q, unsigned int count)
         }
 
         /* Don't start the coda unless both queues are on */
-        if (!(ctx->streamon_out & ctx->streamon_cap))
-                return 0;
+        if (!(ctx->streamon_out && ctx->streamon_cap))
+                goto out;
 
         q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
         if ((q_data_src->width != q_data_dst->width &&
@@ -1444,15 +1513,26 @@ static int coda_start_streaming(struct vb2_queue *q, unsigned int count)
         ret = ctx->ops->start_streaming(ctx);
         if (ctx->inst_type == CODA_INST_DECODER) {
                 if (ret == -EAGAIN)
-                        return 0;
-                else if (ret < 0)
-                        goto err;
+                        goto out;
         }
+        if (ret < 0)
+                goto err;
 
-        return ret;
+out:
+        if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+                list_for_each_entry_safe(m2m_buf, tmp, &list, list) {
+                        list_del(&m2m_buf->list);
+                        v4l2_m2m_buf_done(&m2m_buf->vb, VB2_BUF_STATE_DONE);
+                }
+        }
+        return 0;
 
 err:
         if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+                list_for_each_entry_safe(m2m_buf, tmp, &list, list) {
+                        list_del(&m2m_buf->list);
+                        v4l2_m2m_buf_done(&m2m_buf->vb, VB2_BUF_STATE_QUEUED);
+                }
                 while ((buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx)))
                         v4l2_m2m_buf_done(buf, VB2_BUF_STATE_QUEUED);
         } else {
@@ -1832,7 +1912,8 @@ static int coda_open(struct file *file)
         ctx->idx = idx;
         switch (dev->devtype->product) {
         case CODA_960:
-                ctx->frame_mem_ctrl = 1 << 12;
+                if (enable_bwb)
+                        ctx->frame_mem_ctrl = CODA9_FRAME_ENABLE_BWB;
                 /* fallthrough */
         case CODA_7541:
                 ctx->reg_idx = 0;
@@ -2126,7 +2207,12 @@ static void coda_fw_callback(const struct firmware *fw, void *context);
 
 static int coda_firmware_request(struct coda_dev *dev)
 {
-        char *fw = dev->devtype->firmware[dev->firmware];
+        char *fw;
+
+        if (dev->firmware >= ARRAY_SIZE(dev->devtype->firmware))
+                return -EINVAL;
+
+        fw = dev->devtype->firmware[dev->firmware];
 
         dev_dbg(&dev->plat_dev->dev, "requesting firmware '%s' for %s\n", fw,
                 coda_product_name(dev->devtype->product));
@@ -2142,16 +2228,16 @@ static void coda_fw_callback(const struct firmware *fw, void *context)
         struct platform_device *pdev = dev->plat_dev;
         int i, ret;
 
-        if (!fw && dev->firmware == 1) {
-                v4l2_err(&dev->v4l2_dev, "firmware request failed\n");
-                goto put_pm;
-        }
         if (!fw) {
-                dev->firmware = 1;
-                coda_firmware_request(dev);
+                dev->firmware++;
+                ret = coda_firmware_request(dev);
+                if (ret < 0) {
+                        v4l2_err(&dev->v4l2_dev, "firmware request failed\n");
+                        goto put_pm;
+                }
                 return;
         }
-        if (dev->firmware == 1) {
+        if (dev->firmware > 0) {
                 /*
                  * Since we can't suppress warnings for failed asynchronous
                  * firmware requests, report that the fallback firmware was
diff --git a/drivers/media/platform/coda/coda-h264.c b/drivers/media/platform/coda/coda-h264.c
index 09dfcca7cc50..0e27412e01f5 100644
--- a/drivers/media/platform/coda/coda-h264.c
+++ b/drivers/media/platform/coda/coda-h264.c
@@ -13,12 +13,59 @@
 
 #include <linux/kernel.h>
 #include <linux/string.h>
+#include <linux/videodev2.h>
 #include <coda.h>
 
-static const u8 coda_filler_nal[14] = { 0x00, 0x00, 0x00, 0x01, 0x0c, 0xff,
-                        0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80 };
 static const u8 coda_filler_size[8] = { 0, 7, 14, 13, 12, 11, 10, 9 };
 
+static const u8 *coda_find_nal_header(const u8 *buf, const u8 *end)
+{
+        u32 val = 0xffffffff;
+
+        do {
+                val = val << 8 | *buf++;
+                if (buf >= end)
+                        return NULL;
+        } while (val != 0x00000001);
+
+        return buf;
+}
+
+int coda_sps_parse_profile(struct coda_ctx *ctx, struct vb2_buffer *vb)
+{
+        const u8 *buf = vb2_plane_vaddr(vb, 0);
+        const u8 *end = buf + vb2_get_plane_payload(vb, 0);
+
+        /* Find SPS header */
+        do {
+                buf = coda_find_nal_header(buf, end);
+                if (!buf)
+                        return -EINVAL;
+        } while ((*buf++ & 0x1f) != 0x7);
+
+        ctx->params.h264_profile_idc = buf[0];
+        ctx->params.h264_level_idc = buf[2];
+
+        return 0;
+}
+
+int coda_h264_filler_nal(int size, char *p)
+{
+        if (size < 6)
+                return -EINVAL;
+
+        p[0] = 0x00;
+        p[1] = 0x00;
+        p[2] = 0x00;
+        p[3] = 0x01;
+        p[4] = 0x0c;
+        memset(p + 5, 0xff, size - 6);
+        /* Add rbsp stop bit and trailing at the end */
+        p[size - 1] = 0x80;
+
+        return 0;
+}
+
 int coda_h264_padding(int size, char *p)
 {
         int nal_size;
@@ -29,10 +76,38 @@ int coda_h264_padding(int size, char *p)
                 return 0;
 
         nal_size = coda_filler_size[diff];
-        memcpy(p, coda_filler_nal, nal_size);
-
-        /* Add rbsp stop bit and trailing at the end */
-        *(p + nal_size - 1) = 0x80;
+        coda_h264_filler_nal(nal_size, p);
 
         return nal_size;
 }
+
+int coda_h264_profile(int profile_idc)
+{
+        switch (profile_idc) {
+        case 66: return V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE;
+        case 77: return V4L2_MPEG_VIDEO_H264_PROFILE_MAIN;
+        case 88: return V4L2_MPEG_VIDEO_H264_PROFILE_EXTENDED;
+        case 100: return V4L2_MPEG_VIDEO_H264_PROFILE_HIGH;
+        default: return -EINVAL;
+        }
+}
+
+int coda_h264_level(int level_idc)
+{
+        switch (level_idc) {
+        case 10: return V4L2_MPEG_VIDEO_H264_LEVEL_1_0;
+        case 9:  return V4L2_MPEG_VIDEO_H264_LEVEL_1B;
+        case 11: return V4L2_MPEG_VIDEO_H264_LEVEL_1_1;
+        case 12: return V4L2_MPEG_VIDEO_H264_LEVEL_1_2;
+        case 13: return V4L2_MPEG_VIDEO_H264_LEVEL_1_3;
+        case 20: return V4L2_MPEG_VIDEO_H264_LEVEL_2_0;
+        case 21: return V4L2_MPEG_VIDEO_H264_LEVEL_2_1;
+        case 22: return V4L2_MPEG_VIDEO_H264_LEVEL_2_2;
+        case 30: return V4L2_MPEG_VIDEO_H264_LEVEL_3_0;
+        case 31: return V4L2_MPEG_VIDEO_H264_LEVEL_3_1;
+        case 32: return V4L2_MPEG_VIDEO_H264_LEVEL_3_2;
+        case 40: return V4L2_MPEG_VIDEO_H264_LEVEL_4_0;
+        case 41: return V4L2_MPEG_VIDEO_H264_LEVEL_4_1;
+        default: return -EINVAL;
+        }
+}
diff --git a/drivers/media/platform/coda/coda.h b/drivers/media/platform/coda/coda.h
index 4b831c91ae4a..20222befb9b2 100644
--- a/drivers/media/platform/coda/coda.h
+++ b/drivers/media/platform/coda/coda.h
@@ -28,7 +28,7 @@
 
 #include "coda_regs.h"
 
-#define CODA_MAX_FRAMEBUFFERS   8
+#define CODA_MAX_FRAMEBUFFERS   17
 #define FMO_SLICE_SAVE_BUF_SIZE (32)
 
 enum {
@@ -117,6 +117,8 @@ struct coda_params {
         u8                      h264_deblk_enabled;
         u8                      h264_deblk_alpha;
         u8                      h264_deblk_beta;
+        u8                      h264_profile_idc;
+        u8                      h264_level_idc;
         u8                      mpeg4_intra_qp;
         u8                      mpeg4_inter_qp;
         u8                      gop_size;
@@ -259,7 +261,7 @@ int coda_decoder_queue_init(void *priv, struct vb2_queue *src_vq,
 
 int coda_hw_reset(struct coda_ctx *ctx);
 
-void coda_fill_bitstream(struct coda_ctx *ctx, bool streaming);
+void coda_fill_bitstream(struct coda_ctx *ctx, struct list_head *buffer_list);
 
 void coda_set_gdi_regs(struct coda_ctx *ctx);
 
@@ -290,7 +292,11 @@ void coda_bit_stream_end_flag(struct coda_ctx *ctx);
 void coda_m2m_buf_done(struct coda_ctx *ctx, struct vb2_v4l2_buffer *buf,
                        enum vb2_buffer_state state);
 
+int coda_h264_filler_nal(int size, char *p);
 int coda_h264_padding(int size, char *p);
+int coda_h264_profile(int profile_idc);
+int coda_h264_level(int level_idc);
+int coda_sps_parse_profile(struct coda_ctx *ctx, struct vb2_buffer *vb);
 
 bool coda_jpeg_check_buffer(struct coda_ctx *ctx, struct vb2_buffer *vb);
 int coda_jpeg_write_tables(struct coda_ctx *ctx);
diff --git a/drivers/media/platform/coda/coda_regs.h b/drivers/media/platform/coda/coda_regs.h
index 3490602fa6e1..77ee46a93427 100644
--- a/drivers/media/platform/coda/coda_regs.h
+++ b/drivers/media/platform/coda/coda_regs.h
@@ -51,6 +51,7 @@
 #define         CODA7_STREAM_SEL_64BITS_ENDIAN  (1 << 1)
 #define         CODA_STREAM_ENDIAN_SELECT       (1 << 0)
 #define CODA_REG_BIT_FRAME_MEM_CTRL             0x110
+#define         CODA9_FRAME_ENABLE_BWB          (1 << 12)
 #define         CODA9_FRAME_TILED2LINEAR        (1 << 11)
 #define         CODA_FRAME_CHROMA_INTERLEAVE    (1 << 2)
 #define         CODA_IMAGE_ENDIAN_SELECT        (1 << 0)
diff --git a/drivers/media/platform/davinci/vpif_display.c b/drivers/media/platform/davinci/vpif_display.c
index 50c30731bb78..7e5cf9923c8d 100644
--- a/drivers/media/platform/davinci/vpif_display.c
+++ b/drivers/media/platform/davinci/vpif_display.c
@@ -1287,7 +1287,7 @@ static __init int vpif_probe(struct platform_device *pdev)
         }
 
         if (!vpif_obj.config->asd_sizes) {
-                i2c_adap = i2c_get_adapter(1);
+                i2c_adap = i2c_get_adapter(vpif_obj.config->i2c_adapter_id);
                 for (i = 0; i < subdev_count; i++) {
                         vpif_obj.sd[i] =
                                 v4l2_i2c_new_subdev_board(&vpif_obj.v4l2_dev,
diff --git a/drivers/media/platform/exynos-gsc/gsc-core.c b/drivers/media/platform/exynos-gsc/gsc-core.c
index 0f0c389f8897..59a634201830 100644
--- a/drivers/media/platform/exynos-gsc/gsc-core.c
+++ b/drivers/media/platform/exynos-gsc/gsc-core.c
@@ -112,6 +112,15 @@ static const struct gsc_fmt gsc_formats[] = {
                 .num_planes     = 1,
                 .num_comp       = 2,
         }, {
+                .name           = "YUV 4:2:2 non-contig, Y/CbCr",
+                .pixelformat    = V4L2_PIX_FMT_NV16M,
+                .depth          = { 8, 8 },
+                .color          = GSC_YUV422,
+                .yorder         = GSC_LSB_Y,
+                .corder         = GSC_CBCR,
+                .num_planes     = 2,
+                .num_comp       = 2,
+        }, {
                 .name           = "YUV 4:2:2 planar, Y/CrCb",
                 .pixelformat    = V4L2_PIX_FMT_NV61,
                 .depth          = { 16 },
@@ -121,6 +130,15 @@ static const struct gsc_fmt gsc_formats[] = {
                 .num_planes     = 1,
                 .num_comp       = 2,
         }, {
+                .name           = "YUV 4:2:2 non-contig, Y/CrCb",
+                .pixelformat    = V4L2_PIX_FMT_NV61M,
+                .depth          = { 8, 8 },
+                .color          = GSC_YUV422,
+                .yorder         = GSC_LSB_Y,
+                .corder         = GSC_CRCB,
+                .num_planes     = 2,
+                .num_comp       = 2,
+        }, {
                 .name           = "YUV 4:2:0 planar, YCbCr",
                 .pixelformat    = V4L2_PIX_FMT_YUV420,
                 .depth          = { 12 },
@@ -158,6 +176,15 @@ static const struct gsc_fmt gsc_formats[] = {
                 .num_planes     = 1,
                 .num_comp       = 2,
         }, {
+                .name           = "YUV 4:2:0 non-contig. 2p, Y/CrCb",
+                .pixelformat    = V4L2_PIX_FMT_NV21M,
+                .depth          = { 8, 4 },
+                .color          = GSC_YUV420,
+                .yorder         = GSC_LSB_Y,
+                .corder         = GSC_CRCB,
+                .num_planes     = 2,
+                .num_comp       = 2,
+        }, {
                 .name           = "YUV 4:2:0 non-contig. 2p, Y/CbCr",
                 .pixelformat    = V4L2_PIX_FMT_NV12M,
                 .depth          = { 8, 4 },
diff --git a/drivers/media/platform/fsl-viu.c b/drivers/media/platform/fsl-viu.c
index ae8c6b35a357..97e164b2075a 100644
--- a/drivers/media/platform/fsl-viu.c
+++ b/drivers/media/platform/fsl-viu.c
@@ -1466,9 +1466,8 @@ static int viu_of_probe(struct platform_device *op)
         viu_dev->decoder = v4l2_i2c_new_subdev(&viu_dev->v4l2_dev, ad,
                         "saa7113", VIU_VIDEO_DECODER_ADDR, NULL);
 
-        viu_dev->vidq.timeout.function = viu_vid_timeout;
-        viu_dev->vidq.timeout.data     = (unsigned long)viu_dev;
-        init_timer(&viu_dev->vidq.timeout);
+        setup_timer(&viu_dev->vidq.timeout, viu_vid_timeout,
+                    (unsigned long)viu_dev);
         viu_dev->std = V4L2_STD_NTSC_M;
         viu_dev->first = 1;
 
diff --git a/drivers/media/platform/m2m-deinterlace.c b/drivers/media/platform/m2m-deinterlace.c
index bedc7cc4c7d6..980066b8d32a 100644
--- a/drivers/media/platform/m2m-deinterlace.c
+++ b/drivers/media/platform/m2m-deinterlace.c
@@ -1017,6 +1017,7 @@ static int deinterlace_probe(struct platform_device *pdev)
 
         if (!dma_has_cap(DMA_INTERLEAVE, pcdev->dma_chan->device->cap_mask)) {
                 dev_err(&pdev->dev, "DMA does not support INTERLEAVE\n");
+                ret = -ENODEV;
                 goto rel_dma;
         }
 
diff --git a/drivers/media/platform/mtk-jpeg/Makefile b/drivers/media/platform/mtk-jpeg/Makefile
new file mode 100644
index 000000000000..b2e6069f3959
--- /dev/null
+++ b/drivers/media/platform/mtk-jpeg/Makefile
@@ -0,0 +1,2 @@
+mtk_jpeg-objs := mtk_jpeg_core.o mtk_jpeg_hw.o mtk_jpeg_parse.o
+obj-$(CONFIG_VIDEO_MEDIATEK_JPEG) += mtk_jpeg.o
diff --git a/drivers/media/platform/mtk-jpeg/mtk_jpeg_core.c b/drivers/media/platform/mtk-jpeg/mtk_jpeg_core.c
new file mode 100644
index 000000000000..451a54039e65
--- /dev/null
+++ b/drivers/media/platform/mtk-jpeg/mtk_jpeg_core.c
@@ -0,0 +1,1292 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ *         Rick Chang <rick.chang@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
+#include <soc/mediatek/smi.h>
+
+#include "mtk_jpeg_hw.h"
+#include "mtk_jpeg_core.h"
+#include "mtk_jpeg_parse.h"
+
+static struct mtk_jpeg_fmt mtk_jpeg_formats[] = {
+        {
+                .fourcc         = V4L2_PIX_FMT_JPEG,
+                .colplanes      = 1,
+                .flags          = MTK_JPEG_FMT_FLAG_DEC_OUTPUT,
+        },
+        {
+                .fourcc         = V4L2_PIX_FMT_YUV420M,
+                .h_sample       = {4, 2, 2},
+                .v_sample       = {4, 2, 2},
+                .colplanes      = 3,
+                .h_align        = 5,
+                .v_align        = 4,
+                .flags          = MTK_JPEG_FMT_FLAG_DEC_CAPTURE,
+        },
+        {
+                .fourcc         = V4L2_PIX_FMT_YUV422M,
+                .h_sample       = {4, 2, 2},
+                .v_sample       = {4, 4, 4},
+                .colplanes      = 3,
+                .h_align        = 5,
+                .v_align        = 3,
+                .flags          = MTK_JPEG_FMT_FLAG_DEC_CAPTURE,
+        },
+};
+
+#define MTK_JPEG_NUM_FORMATS ARRAY_SIZE(mtk_jpeg_formats)
+
+enum {
+        MTK_JPEG_BUF_FLAGS_INIT                 = 0,
+        MTK_JPEG_BUF_FLAGS_LAST_FRAME           = 1,
+};
+
+struct mtk_jpeg_src_buf {
+        struct vb2_v4l2_buffer b;
+        struct list_head list;
+        int flags;
+        struct mtk_jpeg_dec_param dec_param;
+};
+
+static int debug;
+module_param(debug, int, 0644);
+
+static inline struct mtk_jpeg_ctx *mtk_jpeg_fh_to_ctx(struct v4l2_fh *fh)
+{
+        return container_of(fh, struct mtk_jpeg_ctx, fh);
+}
+
+static inline struct mtk_jpeg_src_buf *mtk_jpeg_vb2_to_srcbuf(
+                                                        struct vb2_buffer *vb)
+{
+        return container_of(to_vb2_v4l2_buffer(vb), struct mtk_jpeg_src_buf, b);
+}
+
+static int mtk_jpeg_querycap(struct file *file, void *priv,
+                             struct v4l2_capability *cap)
+{
+        struct mtk_jpeg_dev *jpeg = video_drvdata(file);
+
+        strlcpy(cap->driver, MTK_JPEG_NAME " decoder", sizeof(cap->driver));
+        strlcpy(cap->card, MTK_JPEG_NAME " decoder", sizeof(cap->card));
+        snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+                 dev_name(jpeg->dev));
+
+        return 0;
+}
+
+static int mtk_jpeg_enum_fmt(struct mtk_jpeg_fmt *mtk_jpeg_formats, int n,
+                             struct v4l2_fmtdesc *f, u32 type)
+{
+        int i, num = 0;
+
+        for (i = 0; i < n; ++i) {
+                if (mtk_jpeg_formats[i].flags & type) {
+                        if (num == f->index)
+                                break;
+                        ++num;
+                }
+        }
+
+        if (i >= n)
+                return -EINVAL;
+
+        f->pixelformat = mtk_jpeg_formats[i].fourcc;
+
+        return 0;
+}
+
+static int mtk_jpeg_enum_fmt_vid_cap(struct file *file, void *priv,
+                                     struct v4l2_fmtdesc *f)
+{
+        return mtk_jpeg_enum_fmt(mtk_jpeg_formats, MTK_JPEG_NUM_FORMATS, f,
+                                 MTK_JPEG_FMT_FLAG_DEC_CAPTURE);
+}
+
+static int mtk_jpeg_enum_fmt_vid_out(struct file *file, void *priv,
+                                     struct v4l2_fmtdesc *f)
+{
+        return mtk_jpeg_enum_fmt(mtk_jpeg_formats, MTK_JPEG_NUM_FORMATS, f,
+                                 MTK_JPEG_FMT_FLAG_DEC_OUTPUT);
+}
+
+static struct mtk_jpeg_q_data *mtk_jpeg_get_q_data(struct mtk_jpeg_ctx *ctx,
+                                                   enum v4l2_buf_type type)
+{
+        if (V4L2_TYPE_IS_OUTPUT(type))
+                return &ctx->out_q;
+        return &ctx->cap_q;
+}
+
+static struct mtk_jpeg_fmt *mtk_jpeg_find_format(struct mtk_jpeg_ctx *ctx,
+                                                 u32 pixelformat,
+                                                 unsigned int fmt_type)
+{
+        unsigned int k, fmt_flag;
+
+        fmt_flag = (fmt_type == MTK_JPEG_FMT_TYPE_OUTPUT) ?
+                   MTK_JPEG_FMT_FLAG_DEC_OUTPUT :
+                   MTK_JPEG_FMT_FLAG_DEC_CAPTURE;
+
+        for (k = 0; k < MTK_JPEG_NUM_FORMATS; k++) {
+                struct mtk_jpeg_fmt *fmt = &mtk_jpeg_formats[k];
+
+                if (fmt->fourcc == pixelformat && fmt->flags & fmt_flag)
+                        return fmt;
+        }
+
+        return NULL;
+}
+
+static void mtk_jpeg_bound_align_image(u32 *w, unsigned int wmin,
+                                       unsigned int wmax, unsigned int walign,
+                                       u32 *h, unsigned int hmin,
+                                       unsigned int hmax, unsigned int halign)
+{
+        int width, height, w_step, h_step;
+
+        width = *w;
+        height = *h;
+        w_step = 1 << walign;
+        h_step = 1 << halign;
+
+        v4l_bound_align_image(w, wmin, wmax, walign, h, hmin, hmax, halign, 0);
+        if (*w < width && (*w + w_step) <= wmax)
+                *w += w_step;
+        if (*h < height && (*h + h_step) <= hmax)
+                *h += h_step;
+}
+
+static void mtk_jpeg_adjust_fmt_mplane(struct mtk_jpeg_ctx *ctx,
+                                       struct v4l2_format *f)
+{
+        struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+        struct mtk_jpeg_q_data *q_data;
+        int i;
+
+        q_data = mtk_jpeg_get_q_data(ctx, f->type);
+
+        pix_mp->width = q_data->w;
+        pix_mp->height = q_data->h;
+        pix_mp->pixelformat = q_data->fmt->fourcc;
+        pix_mp->num_planes = q_data->fmt->colplanes;
+
+        for (i = 0; i < pix_mp->num_planes; i++) {
+                pix_mp->plane_fmt[i].bytesperline = q_data->bytesperline[i];
+                pix_mp->plane_fmt[i].sizeimage = q_data->sizeimage[i];
+        }
+}
+
+static int mtk_jpeg_try_fmt_mplane(struct v4l2_format *f,
+                                   struct mtk_jpeg_fmt *fmt,
+                                   struct mtk_jpeg_ctx *ctx, int q_type)
+{
+        struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+        struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+        int i;
+
+        memset(pix_mp->reserved, 0, sizeof(pix_mp->reserved));
+        pix_mp->field = V4L2_FIELD_NONE;
+
+        if (ctx->state != MTK_JPEG_INIT) {
+                mtk_jpeg_adjust_fmt_mplane(ctx, f);
+                goto end;
+        }
+
+        pix_mp->num_planes = fmt->colplanes;
+        pix_mp->pixelformat = fmt->fourcc;
+
+        if (q_type == MTK_JPEG_FMT_TYPE_OUTPUT) {
+                struct v4l2_plane_pix_format *pfmt = &pix_mp->plane_fmt[0];
+
+                mtk_jpeg_bound_align_image(&pix_mp->width, MTK_JPEG_MIN_WIDTH,
+                                           MTK_JPEG_MAX_WIDTH, 0,
+                                           &pix_mp->height, MTK_JPEG_MIN_HEIGHT,
+                                           MTK_JPEG_MAX_HEIGHT, 0);
+
+                memset(pfmt->reserved, 0, sizeof(pfmt->reserved));
+                pfmt->bytesperline = 0;
+                /* Source size must be aligned to 128 */
+                pfmt->sizeimage = mtk_jpeg_align(pfmt->sizeimage, 128);
+                if (pfmt->sizeimage == 0)
+                        pfmt->sizeimage = MTK_JPEG_DEFAULT_SIZEIMAGE;
+                goto end;
+        }
+
+        /* type is MTK_JPEG_FMT_TYPE_CAPTURE */
+        mtk_jpeg_bound_align_image(&pix_mp->width, MTK_JPEG_MIN_WIDTH,
+                                   MTK_JPEG_MAX_WIDTH, fmt->h_align,
+                                   &pix_mp->height, MTK_JPEG_MIN_HEIGHT,
+                                   MTK_JPEG_MAX_HEIGHT, fmt->v_align);
+
+        for (i = 0; i < fmt->colplanes; i++) {
+                struct v4l2_plane_pix_format *pfmt = &pix_mp->plane_fmt[i];
+                u32 stride = pix_mp->width * fmt->h_sample[i] / 4;
+                u32 h = pix_mp->height * fmt->v_sample[i] / 4;
+
+                memset(pfmt->reserved, 0, sizeof(pfmt->reserved));
+                pfmt->bytesperline = stride;
+                pfmt->sizeimage = stride * h;
+        }
+end:
+        v4l2_dbg(2, debug, &jpeg->v4l2_dev, "wxh:%ux%u\n",
+                 pix_mp->width, pix_mp->height);
+        for (i = 0; i < pix_mp->num_planes; i++) {
+                v4l2_dbg(2, debug, &jpeg->v4l2_dev,
+                         "plane[%d] bpl=%u, size=%u\n",
+                         i,
+                         pix_mp->plane_fmt[i].bytesperline,
+                         pix_mp->plane_fmt[i].sizeimage);
+        }
+        return 0;
+}
+
+static int mtk_jpeg_g_fmt_vid_mplane(struct file *file, void *priv,
+                                     struct v4l2_format *f)
+{
+        struct vb2_queue *vq;
+        struct mtk_jpeg_q_data *q_data = NULL;
+        struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+        struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
+        struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+        int i;
+
+        vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+        if (!vq)
+                return -EINVAL;
+
+        q_data = mtk_jpeg_get_q_data(ctx, f->type);
+
+        memset(pix_mp->reserved, 0, sizeof(pix_mp->reserved));
+        pix_mp->width = q_data->w;
+        pix_mp->height = q_data->h;
+        pix_mp->field = V4L2_FIELD_NONE;
+        pix_mp->pixelformat = q_data->fmt->fourcc;
+        pix_mp->num_planes = q_data->fmt->colplanes;
+        pix_mp->colorspace = ctx->colorspace;
+        pix_mp->ycbcr_enc = ctx->ycbcr_enc;
+        pix_mp->xfer_func = ctx->xfer_func;
+        pix_mp->quantization = ctx->quantization;
+
+        v4l2_dbg(1, debug, &jpeg->v4l2_dev, "(%d) g_fmt:%c%c%c%c wxh:%ux%u\n",
+                 f->type,
+                 (pix_mp->pixelformat & 0xff),
+                 (pix_mp->pixelformat >>  8 & 0xff),
+                 (pix_mp->pixelformat >> 16 & 0xff),
+                 (pix_mp->pixelformat >> 24 & 0xff),
+                 pix_mp->width, pix_mp->height);
+
+        for (i = 0; i < pix_mp->num_planes; i++) {
+                struct v4l2_plane_pix_format *pfmt = &pix_mp->plane_fmt[i];
+
+                pfmt->bytesperline = q_data->bytesperline[i];
+                pfmt->sizeimage = q_data->sizeimage[i];
+                memset(pfmt->reserved, 0, sizeof(pfmt->reserved));
+
+                v4l2_dbg(1, debug, &jpeg->v4l2_dev,
+                         "plane[%d] bpl=%u, size=%u\n",
+                         i,
+                         pfmt->bytesperline,
+                         pfmt->sizeimage);
+        }
+        return 0;
+}
+
+static int mtk_jpeg_try_fmt_vid_cap_mplane(struct file *file, void *priv,
+                                           struct v4l2_format *f)
+{
+        struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
+        struct mtk_jpeg_fmt *fmt;
+
+        fmt = mtk_jpeg_find_format(ctx, f->fmt.pix_mp.pixelformat,
+                                   MTK_JPEG_FMT_TYPE_CAPTURE);
+        if (!fmt)
+                fmt = ctx->cap_q.fmt;
+
+        v4l2_dbg(2, debug, &ctx->jpeg->v4l2_dev, "(%d) try_fmt:%c%c%c%c\n",
+                 f->type,
+                 (fmt->fourcc & 0xff),
+                 (fmt->fourcc >>  8 & 0xff),
+                 (fmt->fourcc >> 16 & 0xff),
+                 (fmt->fourcc >> 24 & 0xff));
+
+        return mtk_jpeg_try_fmt_mplane(f, fmt, ctx, MTK_JPEG_FMT_TYPE_CAPTURE);
+}
+
+static int mtk_jpeg_try_fmt_vid_out_mplane(struct file *file, void *priv,
+                                           struct v4l2_format *f)
+{
+        struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
+        struct mtk_jpeg_fmt *fmt;
+
+        fmt = mtk_jpeg_find_format(ctx, f->fmt.pix_mp.pixelformat,
+                                   MTK_JPEG_FMT_TYPE_OUTPUT);
+        if (!fmt)
+                fmt = ctx->out_q.fmt;
+
+        v4l2_dbg(2, debug, &ctx->jpeg->v4l2_dev, "(%d) try_fmt:%c%c%c%c\n",
+                 f->type,
+                 (fmt->fourcc & 0xff),
+                 (fmt->fourcc >>  8 & 0xff),
+                 (fmt->fourcc >> 16 & 0xff),
+                 (fmt->fourcc >> 24 & 0xff));
+
+        return mtk_jpeg_try_fmt_mplane(f, fmt, ctx, MTK_JPEG_FMT_TYPE_OUTPUT);
+}
+
+static int mtk_jpeg_s_fmt_mplane(struct mtk_jpeg_ctx *ctx,
+                                 struct v4l2_format *f)
+{
+        struct vb2_queue *vq;
+        struct mtk_jpeg_q_data *q_data = NULL;
+        struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+        struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+        unsigned int f_type;
+        int i;
+
+        vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+        if (!vq)
+                return -EINVAL;
+
+        q_data = mtk_jpeg_get_q_data(ctx, f->type);
+
+        if (vb2_is_busy(vq)) {
+                v4l2_err(&jpeg->v4l2_dev, "queue busy\n");
+                return -EBUSY;
+        }
+
+        f_type = V4L2_TYPE_IS_OUTPUT(f->type) ?
+                         MTK_JPEG_FMT_TYPE_OUTPUT : MTK_JPEG_FMT_TYPE_CAPTURE;
+
+        q_data->fmt = mtk_jpeg_find_format(ctx, pix_mp->pixelformat, f_type);
+        q_data->w = pix_mp->width;
+        q_data->h = pix_mp->height;
+        ctx->colorspace = pix_mp->colorspace;
+        ctx->ycbcr_enc = pix_mp->ycbcr_enc;
+        ctx->xfer_func = pix_mp->xfer_func;
+        ctx->quantization = pix_mp->quantization;
+
+        v4l2_dbg(1, debug, &jpeg->v4l2_dev, "(%d) s_fmt:%c%c%c%c wxh:%ux%u\n",
+                 f->type,
+                 (q_data->fmt->fourcc & 0xff),
+                 (q_data->fmt->fourcc >>  8 & 0xff),
+                 (q_data->fmt->fourcc >> 16 & 0xff),
+                 (q_data->fmt->fourcc >> 24 & 0xff),
+                 q_data->w, q_data->h);
+
+        for (i = 0; i < q_data->fmt->colplanes; i++) {
+                q_data->bytesperline[i] = pix_mp->plane_fmt[i].bytesperline;
+                q_data->sizeimage[i] = pix_mp->plane_fmt[i].sizeimage;
+
+                v4l2_dbg(1, debug, &jpeg->v4l2_dev,
+                         "plane[%d] bpl=%u, size=%u\n",
+                         i, q_data->bytesperline[i], q_data->sizeimage[i]);
+        }
+
+        return 0;
+}
+
+static int mtk_jpeg_s_fmt_vid_out_mplane(struct file *file, void *priv,
+                                         struct v4l2_format *f)
+{
+        int ret;
+
+        ret = mtk_jpeg_try_fmt_vid_out_mplane(file, priv, f);
+        if (ret)
+                return ret;
+
+        return mtk_jpeg_s_fmt_mplane(mtk_jpeg_fh_to_ctx(priv), f);
+}
+
+static int mtk_jpeg_s_fmt_vid_cap_mplane(struct file *file, void *priv,
+                                         struct v4l2_format *f)
+{
+        int ret;
+
+        ret = mtk_jpeg_try_fmt_vid_cap_mplane(file, priv, f);
+        if (ret)
+                return ret;
+
+        return mtk_jpeg_s_fmt_mplane(mtk_jpeg_fh_to_ctx(priv), f);
+}
+
+static void mtk_jpeg_queue_src_chg_event(struct mtk_jpeg_ctx *ctx)
+{
+        static const struct v4l2_event ev_src_ch = {
+                .type = V4L2_EVENT_SOURCE_CHANGE,
+                .u.src_change.changes =
+                V4L2_EVENT_SRC_CH_RESOLUTION,
+        };
+
+        v4l2_event_queue_fh(&ctx->fh, &ev_src_ch);
+}
+
+static int mtk_jpeg_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);
+        default:
+                return -EINVAL;
+        }
+}
+
+static int mtk_jpeg_g_selection(struct file *file, void *priv,
+                                struct v4l2_selection *s)
+{
+        struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
+
+        if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+                return -EINVAL;
+
+        switch (s->target) {
+        case V4L2_SEL_TGT_COMPOSE:
+        case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+                s->r.width = ctx->out_q.w;
+                s->r.height = ctx->out_q.h;
+                s->r.left = 0;
+                s->r.top = 0;
+                break;
+        case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+        case V4L2_SEL_TGT_COMPOSE_PADDED:
+                s->r.width = ctx->cap_q.w;
+                s->r.height = ctx->cap_q.h;
+                s->r.left = 0;
+                s->r.top = 0;
+                break;
+        default:
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static int mtk_jpeg_s_selection(struct file *file, void *priv,
+                                struct v4l2_selection *s)
+{
+        struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
+
+        if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+                return -EINVAL;
+
+        switch (s->target) {
+        case V4L2_SEL_TGT_COMPOSE:
+                s->r.left = 0;
+                s->r.top = 0;
+                s->r.width = ctx->out_q.w;
+                s->r.height = ctx->out_q.h;
+                break;
+        default:
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static int mtk_jpeg_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+        struct v4l2_fh *fh = file->private_data;
+        struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
+        struct vb2_queue *vq;
+        struct vb2_buffer *vb;
+        struct mtk_jpeg_src_buf *jpeg_src_buf;
+
+        if (buf->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+                goto end;
+
+        vq = v4l2_m2m_get_vq(fh->m2m_ctx, buf->type);
+        if (buf->index >= vq->num_buffers) {
+                dev_err(ctx->jpeg->dev, "buffer index out of range\n");
+                return -EINVAL;
+        }
+
+        vb = vq->bufs[buf->index];
+        jpeg_src_buf = mtk_jpeg_vb2_to_srcbuf(vb);
+        jpeg_src_buf->flags = (buf->m.planes[0].bytesused == 0) ?
+                MTK_JPEG_BUF_FLAGS_LAST_FRAME : MTK_JPEG_BUF_FLAGS_INIT;
+end:
+        return v4l2_m2m_qbuf(file, fh->m2m_ctx, buf);
+}
+
+static const struct v4l2_ioctl_ops mtk_jpeg_ioctl_ops = {
+        .vidioc_querycap                = mtk_jpeg_querycap,
+        .vidioc_enum_fmt_vid_cap_mplane = mtk_jpeg_enum_fmt_vid_cap,
+        .vidioc_enum_fmt_vid_out_mplane = mtk_jpeg_enum_fmt_vid_out,
+        .vidioc_try_fmt_vid_cap_mplane  = mtk_jpeg_try_fmt_vid_cap_mplane,
+        .vidioc_try_fmt_vid_out_mplane  = mtk_jpeg_try_fmt_vid_out_mplane,
+        .vidioc_g_fmt_vid_cap_mplane    = mtk_jpeg_g_fmt_vid_mplane,
+        .vidioc_g_fmt_vid_out_mplane    = mtk_jpeg_g_fmt_vid_mplane,
+        .vidioc_s_fmt_vid_cap_mplane    = mtk_jpeg_s_fmt_vid_cap_mplane,
+        .vidioc_s_fmt_vid_out_mplane    = mtk_jpeg_s_fmt_vid_out_mplane,
+        .vidioc_qbuf                    = mtk_jpeg_qbuf,
+        .vidioc_subscribe_event         = mtk_jpeg_subscribe_event,
+        .vidioc_g_selection             = mtk_jpeg_g_selection,
+        .vidioc_s_selection             = mtk_jpeg_s_selection,
+
+        .vidioc_create_bufs             = v4l2_m2m_ioctl_create_bufs,
+        .vidioc_prepare_buf             = v4l2_m2m_ioctl_prepare_buf,
+        .vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
+        .vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
+        .vidioc_dqbuf                   = v4l2_m2m_ioctl_dqbuf,
+        .vidioc_expbuf                  = v4l2_m2m_ioctl_expbuf,
+        .vidioc_streamon                = v4l2_m2m_ioctl_streamon,
+        .vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
+
+        .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
+};
+
+static int mtk_jpeg_queue_setup(struct vb2_queue *q,
+                                unsigned int *num_buffers,
+                                unsigned int *num_planes,
+                                unsigned int sizes[],
+                                struct device *alloc_ctxs[])
+{
+        struct mtk_jpeg_ctx *ctx = vb2_get_drv_priv(q);
+        struct mtk_jpeg_q_data *q_data = NULL;
+        struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+        int i;
+
+        v4l2_dbg(1, debug, &jpeg->v4l2_dev, "(%d) buf_req count=%u\n",
+                 q->type, *num_buffers);
+
+        q_data = mtk_jpeg_get_q_data(ctx, q->type);
+        if (!q_data)
+                return -EINVAL;
+
+        *num_planes = q_data->fmt->colplanes;
+        for (i = 0; i < q_data->fmt->colplanes; i++) {
+                sizes[i] = q_data->sizeimage[i];
+                v4l2_dbg(1, debug, &jpeg->v4l2_dev, "sizeimage[%d]=%u\n",
+                         i, sizes[i]);
+        }
+
+        return 0;
+}
+
+static int mtk_jpeg_buf_prepare(struct vb2_buffer *vb)
+{
+        struct mtk_jpeg_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+        struct mtk_jpeg_q_data *q_data = NULL;
+        int i;
+
+        q_data = mtk_jpeg_get_q_data(ctx, vb->vb2_queue->type);
+        if (!q_data)
+                return -EINVAL;
+
+        for (i = 0; i < q_data->fmt->colplanes; i++)
+                vb2_set_plane_payload(vb, i, q_data->sizeimage[i]);
+
+        return 0;
+}
+
+static bool mtk_jpeg_check_resolution_change(struct mtk_jpeg_ctx *ctx,
+                                             struct mtk_jpeg_dec_param *param)
+{
+        struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+        struct mtk_jpeg_q_data *q_data;
+
+        q_data = &ctx->out_q;
+        if (q_data->w != param->pic_w || q_data->h != param->pic_h) {
+                v4l2_dbg(1, debug, &jpeg->v4l2_dev, "Picture size change\n");
+                return true;
+        }
+
+        q_data = &ctx->cap_q;
+        if (q_data->fmt != mtk_jpeg_find_format(ctx, param->dst_fourcc,
+                                                MTK_JPEG_FMT_TYPE_CAPTURE)) {
+                v4l2_dbg(1, debug, &jpeg->v4l2_dev, "format change\n");
+                return true;
+        }
+        return false;
+}
+
+static void mtk_jpeg_set_queue_data(struct mtk_jpeg_ctx *ctx,
+                                    struct mtk_jpeg_dec_param *param)
+{
+        struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+        struct mtk_jpeg_q_data *q_data;
+        int i;
+
+        q_data = &ctx->out_q;
+        q_data->w = param->pic_w;
+        q_data->h = param->pic_h;
+
+        q_data = &ctx->cap_q;
+        q_data->w = param->dec_w;
+        q_data->h = param->dec_h;
+        q_data->fmt = mtk_jpeg_find_format(ctx,
+                                           param->dst_fourcc,
+                                           MTK_JPEG_FMT_TYPE_CAPTURE);
+
+        for (i = 0; i < q_data->fmt->colplanes; i++) {
+                q_data->bytesperline[i] = param->mem_stride[i];
+                q_data->sizeimage[i] = param->comp_size[i];
+        }
+
+        v4l2_dbg(1, debug, &jpeg->v4l2_dev,
+                 "set_parse cap:%c%c%c%c pic(%u, %u), buf(%u, %u)\n",
+                 (param->dst_fourcc & 0xff),
+                 (param->dst_fourcc >>  8 & 0xff),
+                 (param->dst_fourcc >> 16 & 0xff),
+                 (param->dst_fourcc >> 24 & 0xff),
+                 param->pic_w, param->pic_h,
+                 param->dec_w, param->dec_h);
+}
+
+static void mtk_jpeg_buf_queue(struct vb2_buffer *vb)
+{
+        struct mtk_jpeg_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+        struct mtk_jpeg_dec_param *param;
+        struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+        struct mtk_jpeg_src_buf *jpeg_src_buf;
+        bool header_valid;
+
+        v4l2_dbg(2, debug, &jpeg->v4l2_dev, "(%d) buf_q id=%d, vb=%p\n",
+                 vb->vb2_queue->type, vb->index, vb);
+
+        if (vb->vb2_queue->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+                goto end;
+
+        jpeg_src_buf = mtk_jpeg_vb2_to_srcbuf(vb);
+        param = &jpeg_src_buf->dec_param;
+        memset(param, 0, sizeof(*param));
+
+        if (jpeg_src_buf->flags & MTK_JPEG_BUF_FLAGS_LAST_FRAME) {
+                v4l2_dbg(1, debug, &jpeg->v4l2_dev, "Got eos\n");
+                goto end;
+        }
+        header_valid = mtk_jpeg_parse(param, (u8 *)vb2_plane_vaddr(vb, 0),
+                                      vb2_get_plane_payload(vb, 0));
+        if (!header_valid) {
+                v4l2_err(&jpeg->v4l2_dev, "Header invalid.\n");
+                vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+                return;
+        }
+
+        if (ctx->state == MTK_JPEG_INIT) {
+                struct vb2_queue *dst_vq = v4l2_m2m_get_vq(
+                        ctx->fh.m2m_ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+
+                mtk_jpeg_queue_src_chg_event(ctx);
+                mtk_jpeg_set_queue_data(ctx, param);
+                ctx->state = vb2_is_streaming(dst_vq) ?
+                                MTK_JPEG_SOURCE_CHANGE : MTK_JPEG_RUNNING;
+        }
+end:
+        v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, to_vb2_v4l2_buffer(vb));
+}
+
+static void *mtk_jpeg_buf_remove(struct mtk_jpeg_ctx *ctx,
+                                 enum v4l2_buf_type type)
+{
+        if (V4L2_TYPE_IS_OUTPUT(type))
+                return v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+        else
+                return v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+}
+
+static int mtk_jpeg_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+        struct mtk_jpeg_ctx *ctx = vb2_get_drv_priv(q);
+        struct vb2_buffer *vb;
+        int ret = 0;
+
+        ret = pm_runtime_get_sync(ctx->jpeg->dev);
+        if (ret < 0)
+                goto err;
+
+        return 0;
+err:
+        while ((vb = mtk_jpeg_buf_remove(ctx, q->type)))
+                v4l2_m2m_buf_done(to_vb2_v4l2_buffer(vb), VB2_BUF_STATE_QUEUED);
+        return ret;
+}
+
+static void mtk_jpeg_stop_streaming(struct vb2_queue *q)
+{
+        struct mtk_jpeg_ctx *ctx = vb2_get_drv_priv(q);
+        struct vb2_buffer *vb;
+
+        /*
+         * STREAMOFF is an acknowledgment for source change event.
+         * Before STREAMOFF, we still have to return the old resolution and
+         * subsampling. Update capture queue when the stream is off.
+         */
+        if (ctx->state == MTK_JPEG_SOURCE_CHANGE &&
+            !V4L2_TYPE_IS_OUTPUT(q->type)) {
+                struct mtk_jpeg_src_buf *src_buf;
+
+                vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+                src_buf = mtk_jpeg_vb2_to_srcbuf(vb);
+                mtk_jpeg_set_queue_data(ctx, &src_buf->dec_param);
+                ctx->state = MTK_JPEG_RUNNING;
+        } else if (V4L2_TYPE_IS_OUTPUT(q->type)) {
+                ctx->state = MTK_JPEG_INIT;
+        }
+
+        while ((vb = mtk_jpeg_buf_remove(ctx, q->type)))
+                v4l2_m2m_buf_done(to_vb2_v4l2_buffer(vb), VB2_BUF_STATE_ERROR);
+
+        pm_runtime_put_sync(ctx->jpeg->dev);
+}
+
+static struct vb2_ops mtk_jpeg_qops = {
+        .queue_setup        = mtk_jpeg_queue_setup,
+        .buf_prepare        = mtk_jpeg_buf_prepare,
+        .buf_queue          = mtk_jpeg_buf_queue,
+        .wait_prepare       = vb2_ops_wait_prepare,
+        .wait_finish        = vb2_ops_wait_finish,
+        .start_streaming    = mtk_jpeg_start_streaming,
+        .stop_streaming     = mtk_jpeg_stop_streaming,
+};
+
+static void mtk_jpeg_set_dec_src(struct mtk_jpeg_ctx *ctx,
+                                 struct vb2_buffer *src_buf,
+                                 struct mtk_jpeg_bs *bs)
+{
+        bs->str_addr = vb2_dma_contig_plane_dma_addr(src_buf, 0);
+        bs->end_addr = bs->str_addr +
+                         mtk_jpeg_align(vb2_get_plane_payload(src_buf, 0), 16);
+        bs->size = mtk_jpeg_align(vb2_plane_size(src_buf, 0), 128);
+}
+
+static int mtk_jpeg_set_dec_dst(struct mtk_jpeg_ctx *ctx,
+                                struct mtk_jpeg_dec_param *param,
+                                struct vb2_buffer *dst_buf,
+                                struct mtk_jpeg_fb *fb)
+{
+        int i;
+
+        if (param->comp_num != dst_buf->num_planes) {
+                dev_err(ctx->jpeg->dev, "plane number mismatch (%u != %u)\n",
+                        param->comp_num, dst_buf->num_planes);
+                return -EINVAL;
+        }
+
+        for (i = 0; i < dst_buf->num_planes; i++) {
+                if (vb2_plane_size(dst_buf, i) < param->comp_size[i]) {
+                        dev_err(ctx->jpeg->dev,
+                                "buffer size is underflow (%lu < %u)\n",
+                                vb2_plane_size(dst_buf, 0),
+                                param->comp_size[i]);
+                        return -EINVAL;
+                }
+                fb->plane_addr[i] = vb2_dma_contig_plane_dma_addr(dst_buf, i);
+        }
+
+        return 0;
+}
+
+static void mtk_jpeg_device_run(void *priv)
+{
+        struct mtk_jpeg_ctx *ctx = priv;
+        struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+        struct vb2_buffer *src_buf, *dst_buf;
+        enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
+        unsigned long flags;
+        struct mtk_jpeg_src_buf *jpeg_src_buf;
+        struct mtk_jpeg_bs bs;
+        struct mtk_jpeg_fb fb;
+        int i;
+
+        src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+        dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+        jpeg_src_buf = mtk_jpeg_vb2_to_srcbuf(src_buf);
+
+        if (jpeg_src_buf->flags & MTK_JPEG_BUF_FLAGS_LAST_FRAME) {
+                for (i = 0; i < dst_buf->num_planes; i++)
+                        vb2_set_plane_payload(dst_buf, i, 0);
+                buf_state = VB2_BUF_STATE_DONE;
+                goto dec_end;
+        }
+
+        if (mtk_jpeg_check_resolution_change(ctx, &jpeg_src_buf->dec_param)) {
+                mtk_jpeg_queue_src_chg_event(ctx);
+                ctx->state = MTK_JPEG_SOURCE_CHANGE;
+                v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
+                return;
+        }
+
+        mtk_jpeg_set_dec_src(ctx, src_buf, &bs);
+        if (mtk_jpeg_set_dec_dst(ctx, &jpeg_src_buf->dec_param, dst_buf, &fb))
+                goto dec_end;
+
+        spin_lock_irqsave(&jpeg->hw_lock, flags);
+        mtk_jpeg_dec_reset(jpeg->dec_reg_base);
+        mtk_jpeg_dec_set_config(jpeg->dec_reg_base,
+                                &jpeg_src_buf->dec_param, &bs, &fb);
+
+        mtk_jpeg_dec_start(jpeg->dec_reg_base);
+        spin_unlock_irqrestore(&jpeg->hw_lock, flags);
+        return;
+
+dec_end:
+        v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+        v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+        v4l2_m2m_buf_done(to_vb2_v4l2_buffer(src_buf), buf_state);
+        v4l2_m2m_buf_done(to_vb2_v4l2_buffer(dst_buf), buf_state);
+        v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
+}
+
+static int mtk_jpeg_job_ready(void *priv)
+{
+        struct mtk_jpeg_ctx *ctx = priv;
+
+        return (ctx->state == MTK_JPEG_RUNNING) ? 1 : 0;
+}
+
+static void mtk_jpeg_job_abort(void *priv)
+{
+}
+
+static struct v4l2_m2m_ops mtk_jpeg_m2m_ops = {
+        .device_run = mtk_jpeg_device_run,
+        .job_ready  = mtk_jpeg_job_ready,
+        .job_abort  = mtk_jpeg_job_abort,
+};
+
+static int mtk_jpeg_queue_init(void *priv, struct vb2_queue *src_vq,
+                               struct vb2_queue *dst_vq)
+{
+        struct mtk_jpeg_ctx *ctx = priv;
+        int ret;
+
+        src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+        src_vq->io_modes = VB2_DMABUF | VB2_MMAP;
+        src_vq->drv_priv = ctx;
+        src_vq->buf_struct_size = sizeof(struct mtk_jpeg_src_buf);
+        src_vq->ops = &mtk_jpeg_qops;
+        src_vq->mem_ops = &vb2_dma_contig_memops;
+        src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+        src_vq->lock = &ctx->jpeg->lock;
+        src_vq->dev = ctx->jpeg->dev;
+        ret = vb2_queue_init(src_vq);
+        if (ret)
+                return ret;
+
+        dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+        dst_vq->io_modes = VB2_DMABUF | VB2_MMAP;
+        dst_vq->drv_priv = ctx;
+        dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+        dst_vq->ops = &mtk_jpeg_qops;
+        dst_vq->mem_ops = &vb2_dma_contig_memops;
+        dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+        dst_vq->lock = &ctx->jpeg->lock;
+        dst_vq->dev = ctx->jpeg->dev;
+        ret = vb2_queue_init(dst_vq);
+
+        return ret;
+}
+
+static void mtk_jpeg_clk_on(struct mtk_jpeg_dev *jpeg)
+{
+        int ret;
+
+        ret = mtk_smi_larb_get(jpeg->larb);
+        if (ret)
+                dev_err(jpeg->dev, "mtk_smi_larb_get larbvdec fail %d\n", ret);
+        clk_prepare_enable(jpeg->clk_jdec_smi);
+        clk_prepare_enable(jpeg->clk_jdec);
+}
+
+static void mtk_jpeg_clk_off(struct mtk_jpeg_dev *jpeg)
+{
+        clk_disable_unprepare(jpeg->clk_jdec);
+        clk_disable_unprepare(jpeg->clk_jdec_smi);
+        mtk_smi_larb_put(jpeg->larb);
+}
+
+static irqreturn_t mtk_jpeg_dec_irq(int irq, void *priv)
+{
+        struct mtk_jpeg_dev *jpeg = priv;
+        struct mtk_jpeg_ctx *ctx;
+        struct vb2_buffer *src_buf, *dst_buf;
+        struct mtk_jpeg_src_buf *jpeg_src_buf;
+        enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
+        u32     dec_irq_ret;
+        u32 dec_ret;
+        int i;
+
+        dec_ret = mtk_jpeg_dec_get_int_status(jpeg->dec_reg_base);
+        dec_irq_ret = mtk_jpeg_dec_enum_result(dec_ret);
+        ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
+        if (!ctx) {
+                v4l2_err(&jpeg->v4l2_dev, "Context is NULL\n");
+                return IRQ_HANDLED;
+        }
+
+        src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+        dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+        jpeg_src_buf = mtk_jpeg_vb2_to_srcbuf(src_buf);
+
+        if (dec_irq_ret >= MTK_JPEG_DEC_RESULT_UNDERFLOW)
+                mtk_jpeg_dec_reset(jpeg->dec_reg_base);
+
+        if (dec_irq_ret != MTK_JPEG_DEC_RESULT_EOF_DONE) {
+                dev_err(jpeg->dev, "decode failed\n");
+                goto dec_end;
+        }
+
+        for (i = 0; i < dst_buf->num_planes; i++)
+                vb2_set_plane_payload(dst_buf, i,
+                                      jpeg_src_buf->dec_param.comp_size[i]);
+
+        buf_state = VB2_BUF_STATE_DONE;
+
+dec_end:
+        v4l2_m2m_buf_done(to_vb2_v4l2_buffer(src_buf), buf_state);
+        v4l2_m2m_buf_done(to_vb2_v4l2_buffer(dst_buf), buf_state);
+        v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
+        return IRQ_HANDLED;
+}
+
+static void mtk_jpeg_set_default_params(struct mtk_jpeg_ctx *ctx)
+{
+        struct mtk_jpeg_q_data *q = &ctx->out_q;
+        int i;
+
+        ctx->colorspace = V4L2_COLORSPACE_JPEG,
+        ctx->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+        ctx->quantization = V4L2_QUANTIZATION_DEFAULT;
+        ctx->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+
+        q->fmt = mtk_jpeg_find_format(ctx, V4L2_PIX_FMT_JPEG,
+                                              MTK_JPEG_FMT_TYPE_OUTPUT);
+        q->w = MTK_JPEG_MIN_WIDTH;
+        q->h = MTK_JPEG_MIN_HEIGHT;
+        q->bytesperline[0] = 0;
+        q->sizeimage[0] = MTK_JPEG_DEFAULT_SIZEIMAGE;
+
+        q = &ctx->cap_q;
+        q->fmt = mtk_jpeg_find_format(ctx, V4L2_PIX_FMT_YUV420M,
+                                              MTK_JPEG_FMT_TYPE_CAPTURE);
+        q->w = MTK_JPEG_MIN_WIDTH;
+        q->h = MTK_JPEG_MIN_HEIGHT;
+
+        for (i = 0; i < q->fmt->colplanes; i++) {
+                u32 stride = q->w * q->fmt->h_sample[i] / 4;
+                u32 h = q->h * q->fmt->v_sample[i] / 4;
+
+                q->bytesperline[i] = stride;
+                q->sizeimage[i] = stride * h;
+        }
+}
+
+static int mtk_jpeg_open(struct file *file)
+{
+        struct mtk_jpeg_dev *jpeg = video_drvdata(file);
+        struct video_device *vfd = video_devdata(file);
+        struct mtk_jpeg_ctx *ctx;
+        int ret = 0;
+
+        ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+        if (!ctx)
+                return -ENOMEM;
+
+        if (mutex_lock_interruptible(&jpeg->lock)) {
+                ret = -ERESTARTSYS;
+                goto free;
+        }
+
+        v4l2_fh_init(&ctx->fh, vfd);
+        file->private_data = &ctx->fh;
+        v4l2_fh_add(&ctx->fh);
+
+        ctx->jpeg = jpeg;
+        ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(jpeg->m2m_dev, ctx,
+                                            mtk_jpeg_queue_init);
+        if (IS_ERR(ctx->fh.m2m_ctx)) {
+                ret = PTR_ERR(ctx->fh.m2m_ctx);
+                goto error;
+        }
+
+        mtk_jpeg_set_default_params(ctx);
+        mutex_unlock(&jpeg->lock);
+        return 0;
+
+error:
+        v4l2_fh_del(&ctx->fh);
+        v4l2_fh_exit(&ctx->fh);
+        mutex_unlock(&jpeg->lock);
+free:
+        kfree(ctx);
+        return ret;
+}
+
+static int mtk_jpeg_release(struct file *file)
+{
+        struct mtk_jpeg_dev *jpeg = video_drvdata(file);
+        struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(file->private_data);
+
+        mutex_lock(&jpeg->lock);
+        v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+        v4l2_fh_del(&ctx->fh);
+        v4l2_fh_exit(&ctx->fh);
+        kfree(ctx);
+        mutex_unlock(&jpeg->lock);
+        return 0;
+}
+
+static const struct v4l2_file_operations mtk_jpeg_fops = {
+        .owner          = THIS_MODULE,
+        .open           = mtk_jpeg_open,
+        .release        = mtk_jpeg_release,
+        .poll           = v4l2_m2m_fop_poll,
+        .unlocked_ioctl = video_ioctl2,
+        .mmap           = v4l2_m2m_fop_mmap,
+};
+
+static int mtk_jpeg_clk_init(struct mtk_jpeg_dev *jpeg)
+{
+        struct device_node *node;
+        struct platform_device *pdev;
+
+        node = of_parse_phandle(jpeg->dev->of_node, "mediatek,larb", 0);
+        if (!node)
+                return -EINVAL;
+        pdev = of_find_device_by_node(node);
+        if (WARN_ON(!pdev)) {
+                of_node_put(node);
+                return -EINVAL;
+        }
+        of_node_put(node);
+
+        jpeg->larb = &pdev->dev;
+
+        jpeg->clk_jdec = devm_clk_get(jpeg->dev, "jpgdec");
+        if (IS_ERR(jpeg->clk_jdec))
+                return -EINVAL;
+
+        jpeg->clk_jdec_smi = devm_clk_get(jpeg->dev, "jpgdec-smi");
+        if (IS_ERR(jpeg->clk_jdec_smi))
+                return -EINVAL;
+
+        return 0;
+}
+
+static int mtk_jpeg_probe(struct platform_device *pdev)
+{
+        struct mtk_jpeg_dev *jpeg;
+        struct resource *res;
+        int dec_irq;
+        int ret;
+
+        jpeg = devm_kzalloc(&pdev->dev, sizeof(*jpeg), GFP_KERNEL);
+        if (!jpeg)
+                return -ENOMEM;
+
+        mutex_init(&jpeg->lock);
+        spin_lock_init(&jpeg->hw_lock);
+        jpeg->dev = &pdev->dev;
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        jpeg->dec_reg_base = devm_ioremap_resource(&pdev->dev, res);
+        if (IS_ERR(jpeg->dec_reg_base)) {
+                ret = PTR_ERR(jpeg->dec_reg_base);
+                return ret;
+        }
+
+        res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+        dec_irq = platform_get_irq(pdev, 0);
+        if (!res || dec_irq < 0) {
+                dev_err(&pdev->dev, "Failed to get dec_irq %d.\n", dec_irq);
+                ret = -EINVAL;
+                return ret;
+        }
+
+        ret = devm_request_irq(&pdev->dev, dec_irq, mtk_jpeg_dec_irq, 0,
+                               pdev->name, jpeg);
+        if (ret) {
+                dev_err(&pdev->dev, "Failed to request dec_irq %d (%d)\n",
+                        dec_irq, ret);
+                ret = -EINVAL;
+                goto err_req_irq;
+        }
+
+        ret = mtk_jpeg_clk_init(jpeg);
+        if (ret) {
+                dev_err(&pdev->dev, "Failed to init clk, err %d\n", ret);
+                goto err_clk_init;
+        }
+
+        ret = v4l2_device_register(&pdev->dev, &jpeg->v4l2_dev);
+        if (ret) {
+                dev_err(&pdev->dev, "Failed to register v4l2 device\n");
+                ret = -EINVAL;
+                goto err_dev_register;
+        }
+
+        jpeg->m2m_dev = v4l2_m2m_init(&mtk_jpeg_m2m_ops);
+        if (IS_ERR(jpeg->m2m_dev)) {
+                v4l2_err(&jpeg->v4l2_dev, "Failed to init mem2mem device\n");
+                ret = PTR_ERR(jpeg->m2m_dev);
+                goto err_m2m_init;
+        }
+
+        jpeg->dec_vdev = video_device_alloc();
+        if (!jpeg->dec_vdev) {
+                ret = -ENOMEM;
+                goto err_dec_vdev_alloc;
+        }
+        snprintf(jpeg->dec_vdev->name, sizeof(jpeg->dec_vdev->name),
+                 "%s-dec", MTK_JPEG_NAME);
+        jpeg->dec_vdev->fops = &mtk_jpeg_fops;
+        jpeg->dec_vdev->ioctl_ops = &mtk_jpeg_ioctl_ops;
+        jpeg->dec_vdev->minor = -1;
+        jpeg->dec_vdev->release = video_device_release;
+        jpeg->dec_vdev->lock = &jpeg->lock;
+        jpeg->dec_vdev->v4l2_dev = &jpeg->v4l2_dev;
+        jpeg->dec_vdev->vfl_dir = VFL_DIR_M2M;
+        jpeg->dec_vdev->device_caps = V4L2_CAP_STREAMING |
+                                      V4L2_CAP_VIDEO_M2M_MPLANE;
+
+        ret = video_register_device(jpeg->dec_vdev, VFL_TYPE_GRABBER, 3);
+        if (ret) {
+                v4l2_err(&jpeg->v4l2_dev, "Failed to register video device\n");
+                goto err_dec_vdev_register;
+        }
+
+        video_set_drvdata(jpeg->dec_vdev, jpeg);
+        v4l2_info(&jpeg->v4l2_dev,
+                  "decoder device registered as /dev/video%d (%d,%d)\n",
+                  jpeg->dec_vdev->num, VIDEO_MAJOR, jpeg->dec_vdev->minor);
+
+        platform_set_drvdata(pdev, jpeg);
+
+        pm_runtime_enable(&pdev->dev);
+
+        return 0;
+
+err_dec_vdev_register:
+        video_device_release(jpeg->dec_vdev);
+
+err_dec_vdev_alloc:
+        v4l2_m2m_release(jpeg->m2m_dev);
+
+err_m2m_init:
+        v4l2_device_unregister(&jpeg->v4l2_dev);
+
+err_dev_register:
+
+err_clk_init:
+
+err_req_irq:
+
+        return ret;
+}
+
+static int mtk_jpeg_remove(struct platform_device *pdev)
+{
+        struct mtk_jpeg_dev *jpeg = platform_get_drvdata(pdev);
+
+        pm_runtime_disable(&pdev->dev);
+        video_unregister_device(jpeg->dec_vdev);
+        video_device_release(jpeg->dec_vdev);
+        v4l2_m2m_release(jpeg->m2m_dev);
+        v4l2_device_unregister(&jpeg->v4l2_dev);
+
+        return 0;
+}
+
+static __maybe_unused int mtk_jpeg_pm_suspend(struct device *dev)
+{
+        struct mtk_jpeg_dev *jpeg = dev_get_drvdata(dev);
+
+        mtk_jpeg_dec_reset(jpeg->dec_reg_base);
+        mtk_jpeg_clk_off(jpeg);
+
+        return 0;
+}
+
+static __maybe_unused int mtk_jpeg_pm_resume(struct device *dev)
+{
+        struct mtk_jpeg_dev *jpeg = dev_get_drvdata(dev);
+
+        mtk_jpeg_clk_on(jpeg);
+        mtk_jpeg_dec_reset(jpeg->dec_reg_base);
+
+        return 0;
+}
+
+static __maybe_unused int mtk_jpeg_suspend(struct device *dev)
+{
+        int ret;
+
+        if (pm_runtime_suspended(dev))
+                return 0;
+
+        ret = mtk_jpeg_pm_suspend(dev);
+        return ret;
+}
+
+static __maybe_unused int mtk_jpeg_resume(struct device *dev)
+{
+        int ret;
+
+        if (pm_runtime_suspended(dev))
+                return 0;
+
+        ret = mtk_jpeg_pm_resume(dev);
+
+        return ret;
+}
+
+static const struct dev_pm_ops mtk_jpeg_pm_ops = {
+        SET_SYSTEM_SLEEP_PM_OPS(mtk_jpeg_suspend, mtk_jpeg_resume)
+        SET_RUNTIME_PM_OPS(mtk_jpeg_pm_suspend, mtk_jpeg_pm_resume, NULL)
+};
+
+static const struct of_device_id mtk_jpeg_match[] = {
+        {
+                .compatible = "mediatek,mt8173-jpgdec",
+                .data       = NULL,
+        },
+        {
+                .compatible = "mediatek,mt2701-jpgdec",
+                .data       = NULL,
+        },
+        {},
+};
+
+MODULE_DEVICE_TABLE(of, mtk_jpeg_match);
+
+static struct platform_driver mtk_jpeg_driver = {
+        .probe = mtk_jpeg_probe,
+        .remove = mtk_jpeg_remove,
+        .driver = {
+                .name           = MTK_JPEG_NAME,
+                .of_match_table = mtk_jpeg_match,
+                .pm             = &mtk_jpeg_pm_ops,
+        },
+};
+
+module_platform_driver(mtk_jpeg_driver);
+
+MODULE_DESCRIPTION("MediaTek JPEG codec driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/mtk-jpeg/mtk_jpeg_core.h b/drivers/media/platform/mtk-jpeg/mtk_jpeg_core.h
new file mode 100644
index 000000000000..1a6cdfd4ea70
--- /dev/null
+++ b/drivers/media/platform/mtk-jpeg/mtk_jpeg_core.h
@@ -0,0 +1,139 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ *         Rick Chang <rick.chang@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef _MTK_JPEG_CORE_H
+#define _MTK_JPEG_CORE_H
+
+#include <linux/interrupt.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
+
+#define MTK_JPEG_NAME           "mtk-jpeg"
+
+#define MTK_JPEG_FMT_FLAG_DEC_OUTPUT    BIT(0)
+#define MTK_JPEG_FMT_FLAG_DEC_CAPTURE   BIT(1)
+
+#define MTK_JPEG_FMT_TYPE_OUTPUT        1
+#define MTK_JPEG_FMT_TYPE_CAPTURE       2
+
+#define MTK_JPEG_MIN_WIDTH      32
+#define MTK_JPEG_MIN_HEIGHT     32
+#define MTK_JPEG_MAX_WIDTH      8192
+#define MTK_JPEG_MAX_HEIGHT     8192
+
+#define MTK_JPEG_DEFAULT_SIZEIMAGE      (1 * 1024 * 1024)
+
+enum mtk_jpeg_ctx_state {
+        MTK_JPEG_INIT = 0,
+        MTK_JPEG_RUNNING,
+        MTK_JPEG_SOURCE_CHANGE,
+};
+
+/**
+ * struct mt_jpeg - JPEG IP abstraction
+ * @lock:               the mutex protecting this structure
+ * @hw_lock:            spinlock protecting the hw device resource
+ * @workqueue:          decode work queue
+ * @dev:                JPEG device
+ * @v4l2_dev:           v4l2 device for mem2mem mode
+ * @m2m_dev:            v4l2 mem2mem device data
+ * @alloc_ctx:          videobuf2 memory allocator's context
+ * @dec_vdev:           video device node for decoder mem2mem mode
+ * @dec_reg_base:       JPEG registers mapping
+ * @clk_jdec:           JPEG hw working clock
+ * @clk_jdec_smi:       JPEG SMI bus clock
+ * @larb:               SMI device
+ */
+struct mtk_jpeg_dev {
+        struct mutex            lock;
+        spinlock_t              hw_lock;
+        struct workqueue_struct *workqueue;
+        struct device           *dev;
+        struct v4l2_device      v4l2_dev;
+        struct v4l2_m2m_dev     *m2m_dev;
+        void                    *alloc_ctx;
+        struct video_device     *dec_vdev;
+        void __iomem            *dec_reg_base;
+        struct clk              *clk_jdec;
+        struct clk              *clk_jdec_smi;
+        struct device           *larb;
+};
+
+/**
+ * struct jpeg_fmt - driver's internal color format data
+ * @fourcc:     the fourcc code, 0 if not applicable
+ * @h_sample:   horizontal sample count of plane in 4 * 4 pixel image
+ * @v_sample:   vertical sample count of plane in 4 * 4 pixel image
+ * @colplanes:  number of color planes (1 for packed formats)
+ * @h_align:    horizontal alignment order (align to 2^h_align)
+ * @v_align:    vertical alignment order (align to 2^v_align)
+ * @flags:      flags describing format applicability
+ */
+struct mtk_jpeg_fmt {
+        u32     fourcc;
+        int     h_sample[VIDEO_MAX_PLANES];
+        int     v_sample[VIDEO_MAX_PLANES];
+        int     colplanes;
+        int     h_align;
+        int     v_align;
+        u32     flags;
+};
+
+/**
+ * mtk_jpeg_q_data - parameters of one queue
+ * @fmt:          driver-specific format of this queue
+ * @w:            image width
+ * @h:            image height
+ * @bytesperline: distance in bytes between the leftmost pixels in two adjacent
+ *                lines
+ * @sizeimage:    image buffer size in bytes
+ */
+struct mtk_jpeg_q_data {
+        struct mtk_jpeg_fmt     *fmt;
+        u32                     w;
+        u32                     h;
+        u32                     bytesperline[VIDEO_MAX_PLANES];
+        u32                     sizeimage[VIDEO_MAX_PLANES];
+};
+
+/**
+ * mtk_jpeg_ctx - the device context data
+ * @jpeg:               JPEG IP device for this context
+ * @out_q:              source (output) queue information
+ * @cap_q:              destination (capture) queue queue information
+ * @fh:                 V4L2 file handle
+ * @dec_param           parameters for HW decoding
+ * @state:              state of the context
+ * @header_valid:       set if header has been parsed and valid
+ * @colorspace: enum v4l2_colorspace; supplemental to pixelformat
+ * @ycbcr_enc: enum v4l2_ycbcr_encoding, Y'CbCr encoding
+ * @quantization: enum v4l2_quantization, colorspace quantization
+ * @xfer_func: enum v4l2_xfer_func, colorspace transfer function
+ */
+struct mtk_jpeg_ctx {
+        struct mtk_jpeg_dev             *jpeg;
+        struct mtk_jpeg_q_data          out_q;
+        struct mtk_jpeg_q_data          cap_q;
+        struct v4l2_fh                  fh;
+        enum mtk_jpeg_ctx_state         state;
+
+        enum v4l2_colorspace colorspace;
+        enum v4l2_ycbcr_encoding ycbcr_enc;
+        enum v4l2_quantization quantization;
+        enum v4l2_xfer_func xfer_func;
+};
+
+#endif /* _MTK_JPEG_CORE_H */
diff --git a/drivers/media/platform/mtk-jpeg/mtk_jpeg_hw.c b/drivers/media/platform/mtk-jpeg/mtk_jpeg_hw.c
new file mode 100644
index 000000000000..77b4cc6a8873
--- /dev/null
+++ b/drivers/media/platform/mtk-jpeg/mtk_jpeg_hw.c
@@ -0,0 +1,417 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ *         Rick Chang <rick.chang@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <media/videobuf2-core.h>
+
+#include "mtk_jpeg_hw.h"
+
+#define MTK_JPEG_DUNUM_MASK(val)        (((val) - 1) & 0x3)
+
+enum mtk_jpeg_color {
+        MTK_JPEG_COLOR_420              = 0x00221111,
+        MTK_JPEG_COLOR_422              = 0x00211111,
+        MTK_JPEG_COLOR_444              = 0x00111111,
+        MTK_JPEG_COLOR_422V             = 0x00121111,
+        MTK_JPEG_COLOR_422X2            = 0x00412121,
+        MTK_JPEG_COLOR_422VX2           = 0x00222121,
+        MTK_JPEG_COLOR_400              = 0x00110000
+};
+
+static inline int mtk_jpeg_verify_align(u32 val, int align, u32 reg)
+{
+        if (val & (align - 1)) {
+                pr_err("mtk-jpeg: write reg %x without %d align\n", reg, align);
+                return -1;
+        }
+
+        return 0;
+}
+
+static int mtk_jpeg_decide_format(struct mtk_jpeg_dec_param *param)
+{
+        param->src_color = (param->sampling_w[0] << 20) |
+                           (param->sampling_h[0] << 16) |
+                           (param->sampling_w[1] << 12) |
+                           (param->sampling_h[1] << 8) |
+                           (param->sampling_w[2] << 4) |
+                           (param->sampling_h[2]);
+
+        param->uv_brz_w = 0;
+        switch (param->src_color) {
+        case MTK_JPEG_COLOR_444:
+                param->uv_brz_w = 1;
+                param->dst_fourcc = V4L2_PIX_FMT_YUV422M;
+                break;
+        case MTK_JPEG_COLOR_422X2:
+        case MTK_JPEG_COLOR_422:
+                param->dst_fourcc = V4L2_PIX_FMT_YUV422M;
+                break;
+        case MTK_JPEG_COLOR_422V:
+        case MTK_JPEG_COLOR_422VX2:
+                param->uv_brz_w = 1;
+                param->dst_fourcc = V4L2_PIX_FMT_YUV420M;
+                break;
+        case MTK_JPEG_COLOR_420:
+                param->dst_fourcc = V4L2_PIX_FMT_YUV420M;
+                break;
+        case MTK_JPEG_COLOR_400:
+                param->dst_fourcc = V4L2_PIX_FMT_GREY;
+                break;
+        default:
+                param->dst_fourcc = 0;
+                return -1;
+        }
+
+        return 0;
+}
+
+static void mtk_jpeg_calc_mcu(struct mtk_jpeg_dec_param *param)
+{
+        u32 factor_w, factor_h;
+        u32 i, comp, blk;
+
+        factor_w = 2 + param->sampling_w[0];
+        factor_h = 2 + param->sampling_h[0];
+        param->mcu_w = (param->pic_w + (1 << factor_w) - 1) >> factor_w;
+        param->mcu_h = (param->pic_h + (1 << factor_h) - 1) >> factor_h;
+        param->total_mcu = param->mcu_w * param->mcu_h;
+        param->unit_num = ((param->pic_w + 7) >> 3) * ((param->pic_h + 7) >> 3);
+        param->blk_num = 0;
+        for (i = 0; i < MTK_JPEG_COMP_MAX; i++) {
+                param->blk_comp[i] = 0;
+                if (i >= param->comp_num)
+                        continue;
+                param->blk_comp[i] = param->sampling_w[i] *
+                                     param->sampling_h[i];
+                param->blk_num += param->blk_comp[i];
+        }
+
+        param->membership = 0;
+        for (i = 0, blk = 0, comp = 0; i < MTK_JPEG_BLOCK_MAX; i++) {
+                if (i < param->blk_num && comp < param->comp_num) {
+                        u32 tmp;
+
+                        tmp = (0x04 + (comp & 0x3));
+                        param->membership |= tmp << (i * 3);
+                        if (++blk == param->blk_comp[comp]) {
+                                comp++;
+                                blk = 0;
+                        }
+                } else {
+                        param->membership |=  7 << (i * 3);
+                }
+        }
+}
+
+static void mtk_jpeg_calc_dma_group(struct mtk_jpeg_dec_param *param)
+{
+        u32 factor_mcu = 3;
+
+        if (param->src_color == MTK_JPEG_COLOR_444 &&
+            param->dst_fourcc == V4L2_PIX_FMT_YUV422M)
+                factor_mcu = 4;
+        else if (param->src_color == MTK_JPEG_COLOR_422V &&
+                 param->dst_fourcc == V4L2_PIX_FMT_YUV420M)
+                factor_mcu = 4;
+        else if (param->src_color == MTK_JPEG_COLOR_422X2 &&
+                 param->dst_fourcc == V4L2_PIX_FMT_YUV422M)
+                factor_mcu = 2;
+        else if (param->src_color == MTK_JPEG_COLOR_400 ||
+                 (param->src_color & 0x0FFFF) == 0)
+                factor_mcu = 4;
+
+        param->dma_mcu = 1 << factor_mcu;
+        param->dma_group = param->mcu_w / param->dma_mcu;
+        param->dma_last_mcu = param->mcu_w % param->dma_mcu;
+        if (param->dma_last_mcu)
+                param->dma_group++;
+        else
+                param->dma_last_mcu = param->dma_mcu;
+}
+
+static int mtk_jpeg_calc_dst_size(struct mtk_jpeg_dec_param *param)
+{
+        u32 i, padding_w;
+        u32 ds_row_h[3];
+        u32 brz_w[3];
+
+        brz_w[0] = 0;
+        brz_w[1] = param->uv_brz_w;
+        brz_w[2] = brz_w[1];
+
+        for (i = 0; i < param->comp_num; i++) {
+                if (brz_w[i] > 3)
+                        return -1;
+
+                padding_w = param->mcu_w * MTK_JPEG_DCTSIZE *
+                                param->sampling_w[i];
+                /* output format is 420/422 */
+                param->comp_w[i] = padding_w >> brz_w[i];
+                param->comp_w[i] = mtk_jpeg_align(param->comp_w[i],
+                                                  MTK_JPEG_DCTSIZE);
+                param->img_stride[i] = i ? mtk_jpeg_align(param->comp_w[i], 16)
+                                        : mtk_jpeg_align(param->comp_w[i], 32);
+                ds_row_h[i] = (MTK_JPEG_DCTSIZE * param->sampling_h[i]);
+        }
+        param->dec_w = param->img_stride[0];
+        param->dec_h = ds_row_h[0] * param->mcu_h;
+
+        for (i = 0; i < MTK_JPEG_COMP_MAX; i++) {
+                /* They must be equal in frame mode. */
+                param->mem_stride[i] = param->img_stride[i];
+                param->comp_size[i] = param->mem_stride[i] * ds_row_h[i] *
+                                      param->mcu_h;
+        }
+
+        param->y_size = param->comp_size[0];
+        param->uv_size = param->comp_size[1];
+        param->dec_size = param->y_size + (param->uv_size << 1);
+
+        return 0;
+}
+
+int mtk_jpeg_dec_fill_param(struct mtk_jpeg_dec_param *param)
+{
+        if (mtk_jpeg_decide_format(param))
+                return -1;
+
+        mtk_jpeg_calc_mcu(param);
+        mtk_jpeg_calc_dma_group(param);
+        if (mtk_jpeg_calc_dst_size(param))
+                return -2;
+
+        return 0;
+}
+
+u32 mtk_jpeg_dec_get_int_status(void __iomem *base)
+{
+        u32 ret;
+
+        ret = readl(base + JPGDEC_REG_INTERRUPT_STATUS) & BIT_INQST_MASK_ALLIRQ;
+        if (ret)
+                writel(ret, base + JPGDEC_REG_INTERRUPT_STATUS);
+
+        return ret;
+}
+
+u32 mtk_jpeg_dec_enum_result(u32 irq_result)
+{
+        if (irq_result & BIT_INQST_MASK_EOF)
+                return MTK_JPEG_DEC_RESULT_EOF_DONE;
+        if (irq_result & BIT_INQST_MASK_PAUSE)
+                return MTK_JPEG_DEC_RESULT_PAUSE;
+        if (irq_result & BIT_INQST_MASK_UNDERFLOW)
+                return MTK_JPEG_DEC_RESULT_UNDERFLOW;
+        if (irq_result & BIT_INQST_MASK_OVERFLOW)
+                return MTK_JPEG_DEC_RESULT_OVERFLOW;
+        if (irq_result & BIT_INQST_MASK_ERROR_BS)
+                return MTK_JPEG_DEC_RESULT_ERROR_BS;
+
+        return MTK_JPEG_DEC_RESULT_ERROR_UNKNOWN;
+}
+
+void mtk_jpeg_dec_start(void __iomem *base)
+{
+        writel(0, base + JPGDEC_REG_TRIG);
+}
+
+static void mtk_jpeg_dec_soft_reset(void __iomem *base)
+{
+        writel(0x0000FFFF, base + JPGDEC_REG_INTERRUPT_STATUS);
+        writel(0x00, base + JPGDEC_REG_RESET);
+        writel(0x01, base + JPGDEC_REG_RESET);
+}
+
+static void mtk_jpeg_dec_hard_reset(void __iomem *base)
+{
+        writel(0x00, base + JPGDEC_REG_RESET);
+        writel(0x10, base + JPGDEC_REG_RESET);
+}
+
+void mtk_jpeg_dec_reset(void __iomem *base)
+{
+        mtk_jpeg_dec_soft_reset(base);
+        mtk_jpeg_dec_hard_reset(base);
+}
+
+static void mtk_jpeg_dec_set_brz_factor(void __iomem *base, u8 yscale_w,
+                                        u8 yscale_h, u8 uvscale_w, u8 uvscale_h)
+{
+        u32 val;
+
+        val = (uvscale_h << 12) | (uvscale_w << 8) |
+              (yscale_h << 4) | 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)
+{
+        mtk_jpeg_verify_align(addr_y, 16, JPGDEC_REG_DEST_ADDR0_Y);
+        writel(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);
+        mtk_jpeg_verify_align(addr_v, 16, JPGDEC_REG_DEST_ADDR0_V);
+        writel(addr_v, base + JPGDEC_REG_DEST_ADDR0_V);
+}
+
+static void mtk_jpeg_dec_set_dst_bank1(void __iomem *base, u32 addr_y,
+                                       u32 addr_u, u32 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);
+}
+
+static void mtk_jpeg_dec_set_mem_stride(void __iomem *base, u32 stride_y,
+                                        u32 stride_uv)
+{
+        writel((stride_y & 0xFFFF), base + JPGDEC_REG_STRIDE_Y);
+        writel((stride_uv & 0xFFFF), base + JPGDEC_REG_STRIDE_UV);
+}
+
+static void mtk_jpeg_dec_set_img_stride(void __iomem *base, u32 stride_y,
+                                        u32 stride_uv)
+{
+        writel((stride_y & 0xFFFF), base + JPGDEC_REG_IMG_STRIDE_Y);
+        writel((stride_uv & 0xFFFF), base + JPGDEC_REG_IMG_STRIDE_UV);
+}
+
+static void mtk_jpeg_dec_set_pause_mcu_idx(void __iomem *base, u32 idx)
+{
+        writel(idx & 0x0003FFFFFF, base + JPGDEC_REG_PAUSE_MCU_NUM);
+}
+
+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)
+{
+        mtk_jpeg_verify_align(ptr, 16, JPGDEC_REG_FILE_BRP);
+        writel(ptr, base + JPGDEC_REG_FILE_BRP);
+}
+
+static void mtk_jpeg_dec_set_bs_info(void __iomem *base, u32 addr, u32 size)
+{
+        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(size, base + JPGDEC_REG_FILE_TOTAL_SIZE);
+}
+
+static void mtk_jpeg_dec_set_comp_id(void __iomem *base, u32 id_y, u32 id_u,
+                                     u32 id_v)
+{
+        u32 val;
+
+        val = ((id_y & 0x00FF) << 24) | ((id_u & 0x00FF) << 16) |
+              ((id_v & 0x00FF) << 8);
+        writel(val, base + JPGDEC_REG_COMP_ID);
+}
+
+static void mtk_jpeg_dec_set_total_mcu(void __iomem *base, u32 num)
+{
+        writel(num - 1, base + JPGDEC_REG_TOTAL_MCU_NUM);
+}
+
+static void mtk_jpeg_dec_set_comp0_du(void __iomem *base, u32 num)
+{
+        writel(num - 1, base + JPGDEC_REG_COMP0_DATA_UNIT_NUM);
+}
+
+static void mtk_jpeg_dec_set_du_membership(void __iomem *base, u32 member,
+                                           u32 gmc, u32 isgray)
+{
+        if (isgray)
+                member = 0x3FFFFFFC;
+        member |= (isgray << 31) | (gmc << 30);
+        writel(member, base + JPGDEC_REG_DU_CTRL);
+}
+
+static void mtk_jpeg_dec_set_q_table(void __iomem *base, u32 id0, u32 id1,
+                                     u32 id2)
+{
+        u32 val;
+
+        val = ((id0 & 0x0f) << 8) | ((id1 & 0x0f) << 4) | ((id2 & 0x0f) << 0);
+        writel(val, base + JPGDEC_REG_QT_ID);
+}
+
+static void mtk_jpeg_dec_set_dma_group(void __iomem *base, u32 mcu_group,
+                                       u32 group_num, u32 last_mcu)
+{
+        u32 val;
+
+        val = (((mcu_group - 1) & 0x00FF) << 16) |
+              (((group_num - 1) & 0x007F) << 8) |
+              ((last_mcu - 1) & 0x00FF);
+        writel(val, base + JPGDEC_REG_WDMA_CTRL);
+}
+
+static void mtk_jpeg_dec_set_sampling_factor(void __iomem *base, u32 comp_num,
+                                             u32 y_w, u32 y_h, u32 u_w,
+                                             u32 u_h, u32 v_w, u32 v_h)
+{
+        u32 val;
+        u32 y_wh = (MTK_JPEG_DUNUM_MASK(y_w) << 2) | MTK_JPEG_DUNUM_MASK(y_h);
+        u32 u_wh = (MTK_JPEG_DUNUM_MASK(u_w) << 2) | MTK_JPEG_DUNUM_MASK(u_h);
+        u32 v_wh = (MTK_JPEG_DUNUM_MASK(v_w) << 2) | MTK_JPEG_DUNUM_MASK(v_h);
+
+        if (comp_num == 1)
+                val = 0;
+        else
+                val = (y_wh << 8) | (u_wh << 4) | v_wh;
+        writel(val, base + JPGDEC_REG_DU_NUM);
+}
+
+void mtk_jpeg_dec_set_config(void __iomem *base,
+                             struct mtk_jpeg_dec_param *config,
+                             struct mtk_jpeg_bs *bs,
+                             struct mtk_jpeg_fb *fb)
+{
+        mtk_jpeg_dec_set_brz_factor(base, 0, 0, config->uv_brz_w, 0);
+        mtk_jpeg_dec_set_dec_mode(base, 0);
+        mtk_jpeg_dec_set_comp0_du(base, config->unit_num);
+        mtk_jpeg_dec_set_total_mcu(base, config->total_mcu);
+        mtk_jpeg_dec_set_bs_info(base, bs->str_addr, bs->size);
+        mtk_jpeg_dec_set_bs_write_ptr(base, bs->end_addr);
+        mtk_jpeg_dec_set_du_membership(base, config->membership, 1,
+                                       (config->comp_num == 1) ? 1 : 0);
+        mtk_jpeg_dec_set_comp_id(base, config->comp_id[0], config->comp_id[1],
+                                 config->comp_id[2]);
+        mtk_jpeg_dec_set_q_table(base, config->qtbl_num[0],
+                                 config->qtbl_num[1], config->qtbl_num[2]);
+        mtk_jpeg_dec_set_sampling_factor(base, config->comp_num,
+                                         config->sampling_w[0],
+                                         config->sampling_h[0],
+                                         config->sampling_w[1],
+                                         config->sampling_h[1],
+                                         config->sampling_w[2],
+                                         config->sampling_h[2]);
+        mtk_jpeg_dec_set_mem_stride(base, config->mem_stride[0],
+                                    config->mem_stride[1]);
+        mtk_jpeg_dec_set_img_stride(base, config->img_stride[0],
+                                    config->img_stride[1]);
+        mtk_jpeg_dec_set_dst_bank0(base, 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_dma_group(base, config->dma_mcu, config->dma_group,
+                                   config->dma_last_mcu);
+        mtk_jpeg_dec_set_pause_mcu_idx(base, config->total_mcu);
+}
diff --git a/drivers/media/platform/mtk-jpeg/mtk_jpeg_hw.h b/drivers/media/platform/mtk-jpeg/mtk_jpeg_hw.h
new file mode 100644
index 000000000000..37152a630dea
--- /dev/null
+++ b/drivers/media/platform/mtk-jpeg/mtk_jpeg_hw.h
@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ *         Rick Chang <rick.chang@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef _MTK_JPEG_HW_H
+#define _MTK_JPEG_HW_H
+
+#include <media/videobuf2-core.h>
+
+#include "mtk_jpeg_core.h"
+#include "mtk_jpeg_reg.h"
+
+enum {
+        MTK_JPEG_DEC_RESULT_EOF_DONE            = 0,
+        MTK_JPEG_DEC_RESULT_PAUSE               = 1,
+        MTK_JPEG_DEC_RESULT_UNDERFLOW           = 2,
+        MTK_JPEG_DEC_RESULT_OVERFLOW            = 3,
+        MTK_JPEG_DEC_RESULT_ERROR_BS            = 4,
+        MTK_JPEG_DEC_RESULT_ERROR_UNKNOWN       = 6
+};
+
+struct mtk_jpeg_dec_param {
+        u32 pic_w;
+        u32 pic_h;
+        u32 dec_w;
+        u32 dec_h;
+        u32 src_color;
+        u32 dst_fourcc;
+        u32 mcu_w;
+        u32 mcu_h;
+        u32 total_mcu;
+        u32 unit_num;
+        u32 comp_num;
+        u32 comp_id[MTK_JPEG_COMP_MAX];
+        u32 sampling_w[MTK_JPEG_COMP_MAX];
+        u32 sampling_h[MTK_JPEG_COMP_MAX];
+        u32 qtbl_num[MTK_JPEG_COMP_MAX];
+        u32 blk_num;
+        u32 blk_comp[MTK_JPEG_COMP_MAX];
+        u32 membership;
+        u32 dma_mcu;
+        u32 dma_group;
+        u32 dma_last_mcu;
+        u32 img_stride[MTK_JPEG_COMP_MAX];
+        u32 mem_stride[MTK_JPEG_COMP_MAX];
+        u32 comp_w[MTK_JPEG_COMP_MAX];
+        u32 comp_size[MTK_JPEG_COMP_MAX];
+        u32 y_size;
+        u32 uv_size;
+        u32 dec_size;
+        u8 uv_brz_w;
+};
+
+static inline u32 mtk_jpeg_align(u32 val, u32 align)
+{
+        return (val + align - 1) & ~(align - 1);
+}
+
+struct mtk_jpeg_bs {
+        dma_addr_t      str_addr;
+        dma_addr_t      end_addr;
+        size_t          size;
+};
+
+struct mtk_jpeg_fb {
+        dma_addr_t      plane_addr[MTK_JPEG_COMP_MAX];
+        size_t          size;
+};
+
+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,
+                             struct mtk_jpeg_dec_param *config,
+                             struct mtk_jpeg_bs *bs,
+                             struct mtk_jpeg_fb *fb);
+void mtk_jpeg_dec_reset(void __iomem *dec_reg_base);
+void mtk_jpeg_dec_start(void __iomem *dec_reg_base);
+
+#endif /* _MTK_JPEG_HW_H */
diff --git a/drivers/media/platform/mtk-jpeg/mtk_jpeg_parse.c b/drivers/media/platform/mtk-jpeg/mtk_jpeg_parse.c
new file mode 100644
index 000000000000..38868547f5d4
--- /dev/null
+++ b/drivers/media/platform/mtk-jpeg/mtk_jpeg_parse.c
@@ -0,0 +1,160 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ *         Rick Chang <rick.chang@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/videodev2.h>
+
+#include "mtk_jpeg_parse.h"
+
+#define TEM     0x01
+#define SOF0    0xc0
+#define RST     0xd0
+#define SOI     0xd8
+#define EOI     0xd9
+
+struct mtk_jpeg_stream {
+        u8 *addr;
+        u32 size;
+        u32 curr;
+};
+
+static int read_byte(struct mtk_jpeg_stream *stream)
+{
+        if (stream->curr >= stream->size)
+                return -1;
+        return stream->addr[stream->curr++];
+}
+
+static int read_word_be(struct mtk_jpeg_stream *stream, u32 *word)
+{
+        u32 temp;
+        int byte;
+
+        byte = read_byte(stream);
+        if (byte == -1)
+                return -1;
+        temp = byte << 8;
+        byte = read_byte(stream);
+        if (byte == -1)
+                return -1;
+        *word = (u32)byte | temp;
+
+        return 0;
+}
+
+static void read_skip(struct mtk_jpeg_stream *stream, long len)
+{
+        if (len <= 0)
+                return;
+        while (len--)
+                read_byte(stream);
+}
+
+static bool mtk_jpeg_do_parse(struct mtk_jpeg_dec_param *param, u8 *src_addr_va,
+                              u32 src_size)
+{
+        bool notfound = true;
+        struct mtk_jpeg_stream stream;
+
+        stream.addr = src_addr_va;
+        stream.size = src_size;
+        stream.curr = 0;
+
+        while (notfound) {
+                int i, length, byte;
+                u32 word;
+
+                byte = read_byte(&stream);
+                if (byte == -1)
+                        return false;
+                if (byte != 0xff)
+                        continue;
+                do
+                        byte = read_byte(&stream);
+                while (byte == 0xff);
+                if (byte == -1)
+                        return false;
+                if (byte == 0)
+                        continue;
+
+                length = 0;
+                switch (byte) {
+                case SOF0:
+                        /* length */
+                        if (read_word_be(&stream, &word))
+                                break;
+
+                        /* precision */
+                        if (read_byte(&stream) == -1)
+                                break;
+
+                        if (read_word_be(&stream, &word))
+                                break;
+                        param->pic_h = word;
+
+                        if (read_word_be(&stream, &word))
+                                break;
+                        param->pic_w = word;
+
+                        param->comp_num = read_byte(&stream);
+                        if (param->comp_num != 1 && param->comp_num != 3)
+                                break;
+
+                        for (i = 0; i < param->comp_num; i++) {
+                                param->comp_id[i] = read_byte(&stream);
+                                if (param->comp_id[i] == -1)
+                                        break;
+
+                                /* sampling */
+                                byte = read_byte(&stream);
+                                if (byte == -1)
+                                        break;
+                                param->sampling_w[i] = (byte >> 4) & 0x0F;
+                                param->sampling_h[i] = byte & 0x0F;
+
+                                param->qtbl_num[i] = read_byte(&stream);
+                                if (param->qtbl_num[i] == -1)
+                                        break;
+                        }
+
+                        notfound = !(i == param->comp_num);
+                        break;
+                case RST ... RST + 7:
+                case SOI:
+                case EOI:
+                case TEM:
+                        break;
+                default:
+                        if (read_word_be(&stream, &word))
+                                break;
+                        length = (long)word - 2;
+                        read_skip(&stream, length);
+                        break;
+                }
+        }
+
+        return !notfound;
+}
+
+bool mtk_jpeg_parse(struct mtk_jpeg_dec_param *param, u8 *src_addr_va,
+                    u32 src_size)
+{
+        if (!mtk_jpeg_do_parse(param, src_addr_va, src_size))
+                return false;
+        if (mtk_jpeg_dec_fill_param(param))
+                return false;
+
+        return true;
+}
diff --git a/drivers/media/platform/mtk-jpeg/mtk_jpeg_parse.h b/drivers/media/platform/mtk-jpeg/mtk_jpeg_parse.h
new file mode 100644
index 000000000000..5d92340ea81b
--- /dev/null
+++ b/drivers/media/platform/mtk-jpeg/mtk_jpeg_parse.h
@@ -0,0 +1,25 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ *         Rick Chang <rick.chang@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef _MTK_JPEG_PARSE_H
+#define _MTK_JPEG_PARSE_H
+
+#include "mtk_jpeg_hw.h"
+
+bool mtk_jpeg_parse(struct mtk_jpeg_dec_param *param, u8 *src_addr_va,
+                    u32 src_size);
+
+#endif /* _MTK_JPEG_PARSE_H */
+
diff --git a/drivers/media/platform/mtk-jpeg/mtk_jpeg_reg.h b/drivers/media/platform/mtk-jpeg/mtk_jpeg_reg.h
new file mode 100644
index 000000000000..fc490d62b012
--- /dev/null
+++ b/drivers/media/platform/mtk-jpeg/mtk_jpeg_reg.h
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ *         Rick Chang <rick.chang@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef _MTK_JPEG_REG_H
+#define _MTK_JPEG_REG_H
+
+#define MTK_JPEG_COMP_MAX               3
+#define MTK_JPEG_BLOCK_MAX              10
+#define MTK_JPEG_DCTSIZE                8
+
+#define BIT_INQST_MASK_ERROR_BS         0x20
+#define BIT_INQST_MASK_PAUSE            0x10
+#define BIT_INQST_MASK_OVERFLOW         0x04
+#define BIT_INQST_MASK_UNDERFLOW        0x02
+#define BIT_INQST_MASK_EOF              0x01
+#define BIT_INQST_MASK_ALLIRQ           0x37
+
+#define JPGDEC_REG_RESET                0x0090
+#define JPGDEC_REG_BRZ_FACTOR           0x00F8
+#define JPGDEC_REG_DU_NUM               0x00FC
+#define JPGDEC_REG_DEST_ADDR0_Y         0x0140
+#define JPGDEC_REG_DEST_ADDR0_U         0x0144
+#define JPGDEC_REG_DEST_ADDR0_V         0x0148
+#define JPGDEC_REG_DEST_ADDR1_Y         0x014C
+#define JPGDEC_REG_DEST_ADDR1_U         0x0150
+#define JPGDEC_REG_DEST_ADDR1_V         0x0154
+#define JPGDEC_REG_STRIDE_Y             0x0158
+#define JPGDEC_REG_STRIDE_UV            0x015C
+#define JPGDEC_REG_IMG_STRIDE_Y         0x0160
+#define JPGDEC_REG_IMG_STRIDE_UV        0x0164
+#define JPGDEC_REG_WDMA_CTRL            0x016C
+#define JPGDEC_REG_PAUSE_MCU_NUM        0x0170
+#define JPGDEC_REG_OPERATION_MODE       0x017C
+#define JPGDEC_REG_FILE_ADDR            0x0200
+#define JPGDEC_REG_COMP_ID              0x020C
+#define JPGDEC_REG_TOTAL_MCU_NUM        0x0210
+#define JPGDEC_REG_COMP0_DATA_UNIT_NUM  0x0224
+#define JPGDEC_REG_DU_CTRL              0x023C
+#define JPGDEC_REG_TRIG                 0x0240
+#define JPGDEC_REG_FILE_BRP             0x0248
+#define JPGDEC_REG_FILE_TOTAL_SIZE      0x024C
+#define JPGDEC_REG_QT_ID                0x0270
+#define JPGDEC_REG_INTERRUPT_STATUS     0x0274
+#define JPGDEC_REG_STATUS               0x0278
+
+#endif /* _MTK_JPEG_REG_H */
diff --git a/drivers/media/platform/mtk-vcodec/mtk_vcodec_dec.c b/drivers/media/platform/mtk-vcodec/mtk_vcodec_dec.c
index 502877a4b1df..a60b538686ea 100644
--- a/drivers/media/platform/mtk-vcodec/mtk_vcodec_dec.c
+++ b/drivers/media/platform/mtk-vcodec/mtk_vcodec_dec.c
@@ -420,6 +420,11 @@ static void mtk_vdec_worker(struct work_struct *work)
                         dst_buf->index,
                         ret, res_chg);
                 src_buf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
+                if (ret == -EIO) {
+                        mutex_lock(&ctx->lock);
+                        src_buf_info->error = true;
+                        mutex_unlock(&ctx->lock);
+                }
                 v4l2_m2m_buf_done(&src_buf_info->vb, VB2_BUF_STATE_ERROR);
         } else if (res_chg == false) {
                 /*
@@ -1170,8 +1175,16 @@ static void vb2ops_vdec_buf_queue(struct vb2_buffer *vb)
                  */
 
                 src_buf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
-                v4l2_m2m_buf_done(to_vb2_v4l2_buffer(src_buf),
-                                        VB2_BUF_STATE_DONE);
+                if (ret == -EIO) {
+                        mtk_v4l2_err("[%d] Unrecoverable error in vdec_if_decode.",
+                                        ctx->id);
+                        ctx->state = MTK_STATE_ABORT;
+                        v4l2_m2m_buf_done(to_vb2_v4l2_buffer(src_buf),
+                                                VB2_BUF_STATE_ERROR);
+                } else {
+                        v4l2_m2m_buf_done(to_vb2_v4l2_buffer(src_buf),
+                                                VB2_BUF_STATE_DONE);
+                }
                 mtk_v4l2_debug(ret ? 0 : 1,
                                "[%d] vdec_if_decode() src_buf=%d, size=%zu, fail=%d, res_chg=%d",
                                ctx->id, src_buf->index,
@@ -1216,16 +1229,22 @@ static void vb2ops_vdec_buf_finish(struct vb2_buffer *vb)
         struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
         struct vb2_v4l2_buffer *vb2_v4l2;
         struct mtk_video_dec_buf *buf;
-
-        if (vb->vb2_queue->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
-                return;
+        bool buf_error;
 
         vb2_v4l2 = container_of(vb, struct vb2_v4l2_buffer, vb2_buf);
         buf = container_of(vb2_v4l2, struct mtk_video_dec_buf, vb);
         mutex_lock(&ctx->lock);
-        buf->queued_in_v4l2 = false;
-        buf->queued_in_vb2 = false;
+        if (vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                buf->queued_in_v4l2 = false;
+                buf->queued_in_vb2 = false;
+        }
+        buf_error = buf->error;
         mutex_unlock(&ctx->lock);
+
+        if (buf_error) {
+                mtk_v4l2_err("Unrecoverable error on buffer.");
+                ctx->state = MTK_STATE_ABORT;
+        }
 }
 
 static int vb2ops_vdec_buf_init(struct vb2_buffer *vb)
diff --git a/drivers/media/platform/mtk-vcodec/mtk_vcodec_dec.h b/drivers/media/platform/mtk-vcodec/mtk_vcodec_dec.h
index 362f5a85762e..dc4fc1df63c5 100644
--- a/drivers/media/platform/mtk-vcodec/mtk_vcodec_dec.h
+++ b/drivers/media/platform/mtk-vcodec/mtk_vcodec_dec.h
@@ -50,6 +50,7 @@ struct vdec_fb {
  * @queued_in_v4l2:     Capture buffer is in v4l2 driver, but not in vb2
  *                      queue yet
  * @lastframe:          Intput buffer is last buffer - EOS
+ * @error:              An unrecoverable error occurs on this buffer.
  * @frame_buffer:       Decode status, and buffer information of Capture buffer
  *
  * Note : These status information help us track and debug buffer state
@@ -63,6 +64,7 @@ struct mtk_video_dec_buf {
         bool    queued_in_vb2;
         bool    queued_in_v4l2;
         bool    lastframe;
+        bool    error;
         struct vdec_fb  frame_buffer;
 };
 
diff --git a/drivers/media/platform/mtk-vcodec/mtk_vcodec_enc_drv.c b/drivers/media/platform/mtk-vcodec/mtk_vcodec_enc_drv.c
index aa81f3ce9463..83f859e8509c 100644
--- a/drivers/media/platform/mtk-vcodec/mtk_vcodec_enc_drv.c
+++ b/drivers/media/platform/mtk-vcodec/mtk_vcodec_enc_drv.c
@@ -269,11 +269,6 @@ static int mtk_vcodec_probe(struct platform_device *pdev)
 
         for (i = VENC_SYS, j = 0; i < NUM_MAX_VCODEC_REG_BASE; i++, j++) {
                 res = platform_get_resource(pdev, IORESOURCE_MEM, j);
-                if (res == NULL) {
-                        dev_err(&pdev->dev, "get memory resource failed.");
-                        ret = -ENXIO;
-                        goto err_res;
-                }
                 dev->reg_base[i] = devm_ioremap_resource(&pdev->dev, res);
                 if (IS_ERR((__force void *)dev->reg_base[i])) {
                         ret = PTR_ERR((__force void *)dev->reg_base[i]);
diff --git a/drivers/media/platform/mtk-vcodec/mtk_vcodec_util.h b/drivers/media/platform/mtk-vcodec/mtk_vcodec_util.h
index 7d55975d3185..237e144c194f 100644
--- a/drivers/media/platform/mtk-vcodec/mtk_vcodec_util.h
+++ b/drivers/media/platform/mtk-vcodec/mtk_vcodec_util.h
@@ -31,7 +31,6 @@ struct mtk_vcodec_dev;
 extern int mtk_v4l2_dbg_level;
 extern bool mtk_vcodec_dbg;
 
-#define DEBUG   1
 
 #if defined(DEBUG)
 
@@ -67,15 +66,15 @@ extern bool mtk_vcodec_dbg;
 
 #else
 
-#define mtk_v4l2_debug(level, fmt, args...)
-#define mtk_v4l2_err(fmt, args...)
-#define mtk_v4l2_debug_enter()
-#define mtk_v4l2_debug_leave()
+#define mtk_v4l2_debug(level, fmt, args...) {}
+#define mtk_v4l2_err(fmt, args...) {}
+#define mtk_v4l2_debug_enter() {}
+#define mtk_v4l2_debug_leave() {}
 
-#define mtk_vcodec_debug(h, fmt, args...)
-#define mtk_vcodec_err(h, fmt, args...)
-#define mtk_vcodec_debug_enter(h)
-#define mtk_vcodec_debug_leave(h)
+#define mtk_vcodec_debug(h, fmt, args...) {}
+#define mtk_vcodec_err(h, fmt, args...) {}
+#define mtk_vcodec_debug_enter(h) {}
+#define mtk_vcodec_debug_leave(h) {}
 
 #endif
 
diff --git a/drivers/media/platform/mtk-vcodec/vdec/vdec_vp9_if.c b/drivers/media/platform/mtk-vcodec/vdec/vdec_vp9_if.c
index e91a3b425b0c..5539b1853f16 100644
--- a/drivers/media/platform/mtk-vcodec/vdec/vdec_vp9_if.c
+++ b/drivers/media/platform/mtk-vcodec/vdec/vdec_vp9_if.c
@@ -718,6 +718,26 @@ static void get_free_fb(struct vdec_vp9_inst *inst, struct vdec_fb **out_fb)
         *out_fb = 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) {
+                mtk_vcodec_err(inst, "Invalid vsi->sf_frm_idx=%u.",
+                                vsi->sf_frm_idx);
+                return -EIO;
+        }
+        if (vsi->frm_to_show_idx >= VP9_MAX_FRM_BUF_NUM) {
+                mtk_vcodec_err(inst, "Invalid vsi->frm_to_show_idx=%u.",
+                                vsi->frm_to_show_idx);
+                return -EIO;
+        }
+        if (vsi->new_fb_idx >= VP9_MAX_FRM_BUF_NUM) {
+                mtk_vcodec_err(inst, "Invalid vsi->new_fb_idx=%u.",
+                                vsi->new_fb_idx);
+                return -EIO;
+        }
+        return 0;
+}
+
 static void vdec_vp9_deinit(unsigned long h_vdec)
 {
         struct vdec_vp9_inst *inst = (struct vdec_vp9_inst *)h_vdec;
@@ -834,6 +854,12 @@ static int vdec_vp9_decode(unsigned long h_vdec, struct mtk_vcodec_mem *bs,
                         goto DECODE_ERROR;
                 }
 
+                ret = validate_vsi_array_indexes(inst, vsi);
+                if (ret) {
+                        mtk_vcodec_err(inst, "Invalid values from VPU.");
+                        goto DECODE_ERROR;
+                }
+
                 if (vsi->resolution_changed) {
                         if (!vp9_alloc_work_buf(inst)) {
                                 ret = -EINVAL;
diff --git a/drivers/media/platform/mtk-vcodec/vdec_drv_if.h b/drivers/media/platform/mtk-vcodec/vdec_drv_if.h
index db6b5205ffb1..ded1154481cd 100644
--- a/drivers/media/platform/mtk-vcodec/vdec_drv_if.h
+++ b/drivers/media/platform/mtk-vcodec/vdec_drv_if.h
@@ -85,6 +85,8 @@ void vdec_if_deinit(struct mtk_vcodec_ctx *ctx);
  * @res_chg     : [out] resolution change happens if current bs have different
  *      picture width/height
  * Note: To flush the decoder when reaching EOF, set input bitstream as NULL.
+ *
+ * Return: 0 on success. -EIO on unrecoverable error.
  */
 int vdec_if_decode(struct mtk_vcodec_ctx *ctx, struct mtk_vcodec_mem *bs,
                    struct vdec_fb *fb, bool *res_chg);
diff --git a/drivers/media/platform/mtk-vcodec/venc/venc_vp8_if.c b/drivers/media/platform/mtk-vcodec/venc/venc_vp8_if.c
index a6fa145f2c54..acb639c4abd2 100644
--- a/drivers/media/platform/mtk-vcodec/venc/venc_vp8_if.c
+++ b/drivers/media/platform/mtk-vcodec/venc/venc_vp8_if.c
@@ -155,7 +155,7 @@ static void vp8_enc_free_work_buf(struct venc_vp8_inst *inst)
 
         /* Buffers need to be freed by AP. */
         for (i = 0; i < VENC_VP8_VPU_WORK_BUF_MAX; i++) {
-                if ((inst->work_bufs[i].size == 0))
+                if (inst->work_bufs[i].size == 0)
                         continue;
                 mtk_vcodec_mem_free(inst->ctx, &inst->work_bufs[i]);
         }
@@ -172,7 +172,7 @@ static int vp8_enc_alloc_work_buf(struct venc_vp8_inst *inst)
         mtk_vcodec_debug_enter(inst);
 
         for (i = 0; i < VENC_VP8_VPU_WORK_BUF_MAX; i++) {
-                if ((wb[i].size == 0))
+                if (wb[i].size == 0)
                         continue;
                 /*
                  * This 'wb' structure is set by VPU side and shared to AP for
diff --git a/drivers/media/platform/s5p-cec/Makefile b/drivers/media/platform/s5p-cec/Makefile
new file mode 100644
index 000000000000..0e2cf457825a
--- /dev/null
+++ b/drivers/media/platform/s5p-cec/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_CEC)     += s5p-cec.o
+s5p-cec-y += s5p_cec.o exynos_hdmi_cecctrl.o
diff --git a/drivers/media/platform/s5p-cec/exynos_hdmi_cec.h b/drivers/media/platform/s5p-cec/exynos_hdmi_cec.h
new file mode 100644
index 000000000000..7d9453505dce
--- /dev/null
+++ b/drivers/media/platform/s5p-cec/exynos_hdmi_cec.h
@@ -0,0 +1,37 @@
+/* drivers/media/platform/s5p-cec/exynos_hdmi_cec.h
+ *
+ * Copyright (c) 2010, 2014 Samsung Electronics
+ *              http://www.samsung.com/
+ *
+ * Header file for interface of Samsung Exynos hdmi cec hardware
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _EXYNOS_HDMI_CEC_H_
+#define _EXYNOS_HDMI_CEC_H_ __FILE__
+
+#include <linux/regmap.h>
+#include "s5p_cec.h"
+
+void s5p_cec_set_divider(struct s5p_cec_dev *cec);
+void s5p_cec_enable_rx(struct s5p_cec_dev *cec);
+void s5p_cec_mask_rx_interrupts(struct s5p_cec_dev *cec);
+void s5p_cec_unmask_rx_interrupts(struct s5p_cec_dev *cec);
+void s5p_cec_mask_tx_interrupts(struct s5p_cec_dev *cec);
+void s5p_cec_unmask_tx_interrupts(struct s5p_cec_dev *cec);
+void s5p_cec_reset(struct s5p_cec_dev *cec);
+void s5p_cec_tx_reset(struct s5p_cec_dev *cec);
+void s5p_cec_rx_reset(struct s5p_cec_dev *cec);
+void s5p_cec_threshold(struct s5p_cec_dev *cec);
+void s5p_cec_copy_packet(struct s5p_cec_dev *cec, char *data,
+                         size_t count, u8 retries);
+void s5p_cec_set_addr(struct s5p_cec_dev *cec, u32 addr);
+u32 s5p_cec_get_status(struct s5p_cec_dev *cec);
+void s5p_clr_pending_tx(struct s5p_cec_dev *cec);
+void s5p_clr_pending_rx(struct s5p_cec_dev *cec);
+void s5p_cec_get_rx_buf(struct s5p_cec_dev *cec, u32 size, u8 *buffer);
+
+#endif /* _EXYNOS_HDMI_CEC_H_ */
diff --git a/drivers/media/platform/s5p-cec/exynos_hdmi_cecctrl.c b/drivers/media/platform/s5p-cec/exynos_hdmi_cecctrl.c
new file mode 100644
index 000000000000..1edf667d562a
--- /dev/null
+++ b/drivers/media/platform/s5p-cec/exynos_hdmi_cecctrl.c
@@ -0,0 +1,208 @@
+/* drivers/media/platform/s5p-cec/exynos_hdmi_cecctrl.c
+ *
+ * Copyright (c) 2009, 2014 Samsung Electronics
+ *              http://www.samsung.com/
+ *
+ * cec ftn file for Samsung TVOUT driver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/io.h>
+#include <linux/device.h>
+
+#include "exynos_hdmi_cec.h"
+#include "regs-cec.h"
+
+#define S5P_HDMI_FIN                    24000000
+#define CEC_DIV_RATIO                   320000
+
+#define CEC_MESSAGE_BROADCAST_MASK      0x0F
+#define CEC_MESSAGE_BROADCAST           0x0F
+#define CEC_FILTER_THRESHOLD            0x15
+
+void s5p_cec_set_divider(struct s5p_cec_dev *cec)
+{
+        u32 div_ratio, div_val;
+        unsigned int reg;
+
+        div_ratio  = S5P_HDMI_FIN / CEC_DIV_RATIO - 1;
+
+        if (regmap_read(cec->pmu, EXYNOS_HDMI_PHY_CONTROL, &reg)) {
+                dev_err(cec->dev, "failed to read phy control\n");
+                return;
+        }
+
+        reg = (reg & ~(0x3FF << 16)) | (div_ratio << 16);
+
+        if (regmap_write(cec->pmu, EXYNOS_HDMI_PHY_CONTROL, reg)) {
+                dev_err(cec->dev, "failed to write phy control\n");
+                return;
+        }
+
+        div_val = CEC_DIV_RATIO * 0.00005 - 1;
+
+        writeb(0x0, cec->reg + S5P_CEC_DIVISOR_3);
+        writeb(0x0, cec->reg + S5P_CEC_DIVISOR_2);
+        writeb(0x0, cec->reg + S5P_CEC_DIVISOR_1);
+        writeb(div_val, cec->reg + S5P_CEC_DIVISOR_0);
+}
+
+void s5p_cec_enable_rx(struct s5p_cec_dev *cec)
+{
+        u8 reg;
+
+        reg = readb(cec->reg + S5P_CEC_RX_CTRL);
+        reg |= S5P_CEC_RX_CTRL_ENABLE;
+        writeb(reg, cec->reg + S5P_CEC_RX_CTRL);
+}
+
+void s5p_cec_mask_rx_interrupts(struct s5p_cec_dev *cec)
+{
+        u8 reg;
+
+        reg = readb(cec->reg + S5P_CEC_IRQ_MASK);
+        reg |= S5P_CEC_IRQ_RX_DONE;
+        reg |= S5P_CEC_IRQ_RX_ERROR;
+        writeb(reg, cec->reg + S5P_CEC_IRQ_MASK);
+}
+
+void s5p_cec_unmask_rx_interrupts(struct s5p_cec_dev *cec)
+{
+        u8 reg;
+
+        reg = readb(cec->reg + S5P_CEC_IRQ_MASK);
+        reg &= ~S5P_CEC_IRQ_RX_DONE;
+        reg &= ~S5P_CEC_IRQ_RX_ERROR;
+        writeb(reg, cec->reg + S5P_CEC_IRQ_MASK);
+}
+
+void s5p_cec_mask_tx_interrupts(struct s5p_cec_dev *cec)
+{
+        u8 reg;
+
+        reg = readb(cec->reg + S5P_CEC_IRQ_MASK);
+        reg |= S5P_CEC_IRQ_TX_DONE;
+        reg |= S5P_CEC_IRQ_TX_ERROR;
+        writeb(reg, cec->reg + S5P_CEC_IRQ_MASK);
+}
+
+void s5p_cec_unmask_tx_interrupts(struct s5p_cec_dev *cec)
+{
+        u8 reg;
+
+        reg = readb(cec->reg + S5P_CEC_IRQ_MASK);
+        reg &= ~S5P_CEC_IRQ_TX_DONE;
+        reg &= ~S5P_CEC_IRQ_TX_ERROR;
+        writeb(reg, cec->reg + S5P_CEC_IRQ_MASK);
+}
+
+void s5p_cec_reset(struct s5p_cec_dev *cec)
+{
+        u8 reg;
+
+        writeb(S5P_CEC_RX_CTRL_RESET, cec->reg + S5P_CEC_RX_CTRL);
+        writeb(S5P_CEC_TX_CTRL_RESET, cec->reg + S5P_CEC_TX_CTRL);
+
+        reg = readb(cec->reg + 0xc4);
+        reg &= ~0x1;
+        writeb(reg, cec->reg + 0xc4);
+}
+
+void s5p_cec_tx_reset(struct s5p_cec_dev *cec)
+{
+        writeb(S5P_CEC_TX_CTRL_RESET, cec->reg + S5P_CEC_TX_CTRL);
+}
+
+void s5p_cec_rx_reset(struct s5p_cec_dev *cec)
+{
+        u8 reg;
+
+        writeb(S5P_CEC_RX_CTRL_RESET, cec->reg + S5P_CEC_RX_CTRL);
+
+        reg = readb(cec->reg + 0xc4);
+        reg &= ~0x1;
+        writeb(reg, cec->reg + 0xc4);
+}
+
+void s5p_cec_threshold(struct s5p_cec_dev *cec)
+{
+        writeb(CEC_FILTER_THRESHOLD, cec->reg + S5P_CEC_RX_FILTER_TH);
+        writeb(0, cec->reg + S5P_CEC_RX_FILTER_CTRL);
+}
+
+void s5p_cec_copy_packet(struct s5p_cec_dev *cec, char *data,
+                         size_t count, u8 retries)
+{
+        int i = 0;
+        u8 reg;
+
+        while (i < count) {
+                writeb(data[i], cec->reg + (S5P_CEC_TX_BUFF0 + (i * 4)));
+                i++;
+        }
+
+        writeb(count, cec->reg + S5P_CEC_TX_BYTES);
+        reg = readb(cec->reg + S5P_CEC_TX_CTRL);
+        reg |= S5P_CEC_TX_CTRL_START;
+        reg &= ~0x70;
+        reg |= retries << 4;
+
+        if ((data[0] & CEC_MESSAGE_BROADCAST_MASK) == CEC_MESSAGE_BROADCAST) {
+                dev_dbg(cec->dev, "Broadcast");
+                reg |= S5P_CEC_TX_CTRL_BCAST;
+        } else {
+                dev_dbg(cec->dev, "No Broadcast");
+                reg &= ~S5P_CEC_TX_CTRL_BCAST;
+        }
+
+        writeb(reg, cec->reg + S5P_CEC_TX_CTRL);
+        dev_dbg(cec->dev, "cec-tx: cec count (%zu): %*ph", count,
+                (int)count, data);
+}
+
+void s5p_cec_set_addr(struct s5p_cec_dev *cec, u32 addr)
+{
+        writeb(addr & 0x0F, cec->reg + S5P_CEC_LOGIC_ADDR);
+}
+
+u32 s5p_cec_get_status(struct s5p_cec_dev *cec)
+{
+        u32 status = 0;
+
+        status = readb(cec->reg + S5P_CEC_STATUS_0);
+        status |= readb(cec->reg + S5P_CEC_STATUS_1) << 8;
+        status |= readb(cec->reg + S5P_CEC_STATUS_2) << 16;
+        status |= readb(cec->reg + S5P_CEC_STATUS_3) << 24;
+
+        dev_dbg(cec->dev, "status = 0x%x!\n", status);
+
+        return status;
+}
+
+void s5p_clr_pending_tx(struct s5p_cec_dev *cec)
+{
+        writeb(S5P_CEC_IRQ_TX_DONE | S5P_CEC_IRQ_TX_ERROR,
+               cec->reg + S5P_CEC_IRQ_CLEAR);
+}
+
+void s5p_clr_pending_rx(struct s5p_cec_dev *cec)
+{
+        writeb(S5P_CEC_IRQ_RX_DONE | S5P_CEC_IRQ_RX_ERROR,
+               cec->reg + S5P_CEC_IRQ_CLEAR);
+}
+
+void s5p_cec_get_rx_buf(struct s5p_cec_dev *cec, u32 size, u8 *buffer)
+{
+        u32 i = 0;
+        char debug[40];
+
+        while (i < size) {
+                buffer[i] = readb(cec->reg + S5P_CEC_RX_BUFF0 + (i * 4));
+                sprintf(debug + i * 2, "%02x ", buffer[i]);
+                i++;
+        }
+        dev_dbg(cec->dev, "cec-rx: cec size(%d): %s", size, debug);
+}
diff --git a/drivers/media/platform/s5p-cec/regs-cec.h b/drivers/media/platform/s5p-cec/regs-cec.h
new file mode 100644
index 000000000000..b2e7e129920e
--- /dev/null
+++ b/drivers/media/platform/s5p-cec/regs-cec.h
@@ -0,0 +1,96 @@
+/* drivers/media/platform/s5p-cec/regs-cec.h
+ *
+ * Copyright (c) 2010 Samsung Electronics
+ *              http://www.samsung.com/
+ *
+ *  register header file for Samsung TVOUT driver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __EXYNOS_REGS__H
+#define __EXYNOS_REGS__H
+
+/*
+ * Register part
+ */
+#define S5P_CEC_STATUS_0                        (0x0000)
+#define S5P_CEC_STATUS_1                        (0x0004)
+#define S5P_CEC_STATUS_2                        (0x0008)
+#define S5P_CEC_STATUS_3                        (0x000C)
+#define S5P_CEC_IRQ_MASK                        (0x0010)
+#define S5P_CEC_IRQ_CLEAR                       (0x0014)
+#define S5P_CEC_LOGIC_ADDR                      (0x0020)
+#define S5P_CEC_DIVISOR_0                       (0x0030)
+#define S5P_CEC_DIVISOR_1                       (0x0034)
+#define S5P_CEC_DIVISOR_2                       (0x0038)
+#define S5P_CEC_DIVISOR_3                       (0x003C)
+
+#define S5P_CEC_TX_CTRL                         (0x0040)
+#define S5P_CEC_TX_BYTES                        (0x0044)
+#define S5P_CEC_TX_STAT0                        (0x0060)
+#define S5P_CEC_TX_STAT1                        (0x0064)
+#define S5P_CEC_TX_BUFF0                        (0x0080)
+#define S5P_CEC_TX_BUFF1                        (0x0084)
+#define S5P_CEC_TX_BUFF2                        (0x0088)
+#define S5P_CEC_TX_BUFF3                        (0x008C)
+#define S5P_CEC_TX_BUFF4                        (0x0090)
+#define S5P_CEC_TX_BUFF5                        (0x0094)
+#define S5P_CEC_TX_BUFF6                        (0x0098)
+#define S5P_CEC_TX_BUFF7                        (0x009C)
+#define S5P_CEC_TX_BUFF8                        (0x00A0)
+#define S5P_CEC_TX_BUFF9                        (0x00A4)
+#define S5P_CEC_TX_BUFF10                       (0x00A8)
+#define S5P_CEC_TX_BUFF11                       (0x00AC)
+#define S5P_CEC_TX_BUFF12                       (0x00B0)
+#define S5P_CEC_TX_BUFF13                       (0x00B4)
+#define S5P_CEC_TX_BUFF14                       (0x00B8)
+#define S5P_CEC_TX_BUFF15                       (0x00BC)
+
+#define S5P_CEC_RX_CTRL                         (0x00C0)
+#define S5P_CEC_RX_STAT0                        (0x00E0)
+#define S5P_CEC_RX_STAT1                        (0x00E4)
+#define S5P_CEC_RX_BUFF0                        (0x0100)
+#define S5P_CEC_RX_BUFF1                        (0x0104)
+#define S5P_CEC_RX_BUFF2                        (0x0108)
+#define S5P_CEC_RX_BUFF3                        (0x010C)
+#define S5P_CEC_RX_BUFF4                        (0x0110)
+#define S5P_CEC_RX_BUFF5                        (0x0114)
+#define S5P_CEC_RX_BUFF6                        (0x0118)
+#define S5P_CEC_RX_BUFF7                        (0x011C)
+#define S5P_CEC_RX_BUFF8                        (0x0120)
+#define S5P_CEC_RX_BUFF9                        (0x0124)
+#define S5P_CEC_RX_BUFF10                       (0x0128)
+#define S5P_CEC_RX_BUFF11                       (0x012C)
+#define S5P_CEC_RX_BUFF12                       (0x0130)
+#define S5P_CEC_RX_BUFF13                       (0x0134)
+#define S5P_CEC_RX_BUFF14                       (0x0138)
+#define S5P_CEC_RX_BUFF15                       (0x013C)
+
+#define S5P_CEC_RX_FILTER_CTRL                  (0x0180)
+#define S5P_CEC_RX_FILTER_TH                    (0x0184)
+
+/*
+ * Bit definition part
+ */
+#define S5P_CEC_IRQ_TX_DONE                     (1<<0)
+#define S5P_CEC_IRQ_TX_ERROR                    (1<<1)
+#define S5P_CEC_IRQ_RX_DONE                     (1<<4)
+#define S5P_CEC_IRQ_RX_ERROR                    (1<<5)
+
+#define S5P_CEC_TX_CTRL_START                   (1<<0)
+#define S5P_CEC_TX_CTRL_BCAST                   (1<<1)
+#define S5P_CEC_TX_CTRL_RETRY                   (0x04<<4)
+#define S5P_CEC_TX_CTRL_RESET                   (1<<7)
+
+#define S5P_CEC_RX_CTRL_ENABLE                  (1<<0)
+#define S5P_CEC_RX_CTRL_RESET                   (1<<7)
+
+#define S5P_CEC_LOGIC_ADDR_MASK                 (0xF)
+
+/* PMU Registers for PHY */
+#define EXYNOS_HDMI_PHY_CONTROL                 0x700
+
+#endif  /* __EXYNOS_REGS__H     */
diff --git a/drivers/media/platform/s5p-cec/s5p_cec.c b/drivers/media/platform/s5p-cec/s5p_cec.c
new file mode 100644
index 000000000000..664937b61fa4
--- /dev/null
+++ b/drivers/media/platform/s5p-cec/s5p_cec.c
@@ -0,0 +1,304 @@
+/* drivers/media/platform/s5p-cec/s5p_cec.c
+ *
+ * Samsung S5P CEC driver
+ *
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This driver is based on the "cec interface driver for exynos soc" by
+ * SangPil Moon.
+ */
+
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/timer.h>
+#include <linux/workqueue.h>
+#include <media/cec.h>
+#include <media/cec-notifier.h>
+
+#include "exynos_hdmi_cec.h"
+#include "regs-cec.h"
+#include "s5p_cec.h"
+
+#define CEC_NAME        "s5p-cec"
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "debug level (0-2)");
+
+static int s5p_cec_adap_enable(struct cec_adapter *adap, bool enable)
+{
+        struct s5p_cec_dev *cec = cec_get_drvdata(adap);
+
+        if (enable) {
+                pm_runtime_get_sync(cec->dev);
+
+                s5p_cec_reset(cec);
+
+                s5p_cec_set_divider(cec);
+                s5p_cec_threshold(cec);
+
+                s5p_cec_unmask_tx_interrupts(cec);
+                s5p_cec_unmask_rx_interrupts(cec);
+                s5p_cec_enable_rx(cec);
+        } else {
+                s5p_cec_mask_tx_interrupts(cec);
+                s5p_cec_mask_rx_interrupts(cec);
+                pm_runtime_disable(cec->dev);
+        }
+
+        return 0;
+}
+
+static int s5p_cec_adap_log_addr(struct cec_adapter *adap, u8 addr)
+{
+        struct s5p_cec_dev *cec = cec_get_drvdata(adap);
+
+        s5p_cec_set_addr(cec, addr);
+        return 0;
+}
+
+static int s5p_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
+                                 u32 signal_free_time, struct cec_msg *msg)
+{
+        struct s5p_cec_dev *cec = cec_get_drvdata(adap);
+
+        /*
+         * Unclear if 0 retries are allowed by the hardware, so have 1 as
+         * the minimum.
+         */
+        s5p_cec_copy_packet(cec, msg->msg, msg->len, max(1, attempts - 1));
+        return 0;
+}
+
+static irqreturn_t s5p_cec_irq_handler(int irq, void *priv)
+{
+        struct s5p_cec_dev *cec = priv;
+        u32 status = 0;
+
+        status = s5p_cec_get_status(cec);
+
+        dev_dbg(cec->dev, "irq received\n");
+
+        if (status & CEC_STATUS_TX_DONE) {
+                if (status & CEC_STATUS_TX_ERROR) {
+                        dev_dbg(cec->dev, "CEC_STATUS_TX_ERROR set\n");
+                        cec->tx = STATE_ERROR;
+                } else {
+                        dev_dbg(cec->dev, "CEC_STATUS_TX_DONE\n");
+                        cec->tx = STATE_DONE;
+                }
+                s5p_clr_pending_tx(cec);
+        }
+
+        if (status & CEC_STATUS_RX_DONE) {
+                if (status & CEC_STATUS_RX_ERROR) {
+                        dev_dbg(cec->dev, "CEC_STATUS_RX_ERROR set\n");
+                        s5p_cec_rx_reset(cec);
+                        s5p_cec_enable_rx(cec);
+                } else {
+                        dev_dbg(cec->dev, "CEC_STATUS_RX_DONE set\n");
+                        if (cec->rx != STATE_IDLE)
+                                dev_dbg(cec->dev, "Buffer overrun (worker did not process previous message)\n");
+                        cec->rx = STATE_BUSY;
+                        cec->msg.len = status >> 24;
+                        cec->msg.rx_status = CEC_RX_STATUS_OK;
+                        s5p_cec_get_rx_buf(cec, cec->msg.len,
+                                        cec->msg.msg);
+                        cec->rx = STATE_DONE;
+                        s5p_cec_enable_rx(cec);
+                }
+                /* Clear interrupt pending bit */
+                s5p_clr_pending_rx(cec);
+        }
+        return IRQ_WAKE_THREAD;
+}
+
+static irqreturn_t s5p_cec_irq_handler_thread(int irq, void *priv)
+{
+        struct s5p_cec_dev *cec = priv;
+
+        dev_dbg(cec->dev, "irq processing thread\n");
+        switch (cec->tx) {
+        case STATE_DONE:
+                cec_transmit_done(cec->adap, CEC_TX_STATUS_OK, 0, 0, 0, 0);
+                cec->tx = STATE_IDLE;
+                break;
+        case STATE_ERROR:
+                cec_transmit_done(cec->adap,
+                        CEC_TX_STATUS_MAX_RETRIES | CEC_TX_STATUS_ERROR,
+                        0, 0, 0, 1);
+                cec->tx = STATE_IDLE;
+                break;
+        case STATE_BUSY:
+                dev_err(cec->dev, "state set to busy, this should not occur here\n");
+                break;
+        default:
+                break;
+        }
+
+        switch (cec->rx) {
+        case STATE_DONE:
+                cec_received_msg(cec->adap, &cec->msg);
+                cec->rx = STATE_IDLE;
+                break;
+        default:
+                break;
+        }
+
+        return IRQ_HANDLED;
+}
+
+static const struct cec_adap_ops s5p_cec_adap_ops = {
+        .adap_enable = s5p_cec_adap_enable,
+        .adap_log_addr = s5p_cec_adap_log_addr,
+        .adap_transmit = s5p_cec_adap_transmit,
+};
+
+static int s5p_cec_probe(struct platform_device *pdev)
+{
+        struct device *dev = &pdev->dev;
+        struct device_node *np;
+        struct platform_device *hdmi_dev;
+        struct resource *res;
+        struct s5p_cec_dev *cec;
+        int ret;
+
+        np = of_parse_phandle(pdev->dev.of_node, "hdmi-phandle", 0);
+
+        if (!np) {
+                dev_err(&pdev->dev, "Failed to find hdmi node in device tree\n");
+                return -ENODEV;
+        }
+        hdmi_dev = of_find_device_by_node(np);
+        if (hdmi_dev == NULL)
+                return -EPROBE_DEFER;
+
+        cec = devm_kzalloc(&pdev->dev, sizeof(*cec), GFP_KERNEL);
+        if (!cec)
+                return -ENOMEM;
+
+        cec->dev = dev;
+
+        cec->irq = platform_get_irq(pdev, 0);
+        if (cec->irq < 0)
+                return cec->irq;
+
+        ret = devm_request_threaded_irq(dev, cec->irq, s5p_cec_irq_handler,
+                s5p_cec_irq_handler_thread, 0, pdev->name, cec);
+        if (ret)
+                return ret;
+
+        cec->clk = devm_clk_get(dev, "hdmicec");
+        if (IS_ERR(cec->clk))
+                return PTR_ERR(cec->clk);
+
+        cec->pmu = syscon_regmap_lookup_by_phandle(dev->of_node,
+                                                 "samsung,syscon-phandle");
+        if (IS_ERR(cec->pmu))
+                return -EPROBE_DEFER;
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        cec->reg = devm_ioremap_resource(dev, res);
+        if (IS_ERR(cec->reg))
+                return PTR_ERR(cec->reg);
+
+        cec->notifier = cec_notifier_get(&hdmi_dev->dev);
+        if (cec->notifier == NULL)
+                return -ENOMEM;
+
+        cec->adap = cec_allocate_adapter(&s5p_cec_adap_ops, cec,
+                CEC_NAME,
+                CEC_CAP_LOG_ADDRS | CEC_CAP_TRANSMIT |
+                CEC_CAP_PASSTHROUGH | CEC_CAP_RC, 1);
+        ret = PTR_ERR_OR_ZERO(cec->adap);
+        if (ret)
+                return ret;
+
+        ret = cec_register_adapter(cec->adap, &pdev->dev);
+        if (ret)
+                goto err_delete_adapter;
+
+        cec_register_cec_notifier(cec->adap, cec->notifier);
+
+        platform_set_drvdata(pdev, cec);
+        pm_runtime_enable(dev);
+
+        dev_dbg(dev, "successfuly probed\n");
+        return 0;
+
+err_delete_adapter:
+        cec_delete_adapter(cec->adap);
+        return ret;
+}
+
+static int s5p_cec_remove(struct platform_device *pdev)
+{
+        struct s5p_cec_dev *cec = platform_get_drvdata(pdev);
+
+        cec_unregister_adapter(cec->adap);
+        cec_notifier_put(cec->notifier);
+        pm_runtime_disable(&pdev->dev);
+        return 0;
+}
+
+static int __maybe_unused s5p_cec_runtime_suspend(struct device *dev)
+{
+        struct s5p_cec_dev *cec = dev_get_drvdata(dev);
+
+        clk_disable_unprepare(cec->clk);
+        return 0;
+}
+
+static int __maybe_unused s5p_cec_runtime_resume(struct device *dev)
+{
+        struct s5p_cec_dev *cec = dev_get_drvdata(dev);
+        int ret;
+
+        ret = clk_prepare_enable(cec->clk);
+        if (ret < 0)
+                return ret;
+        return 0;
+}
+
+static const struct dev_pm_ops s5p_cec_pm_ops = {
+        SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+                                pm_runtime_force_resume)
+        SET_RUNTIME_PM_OPS(s5p_cec_runtime_suspend, s5p_cec_runtime_resume,
+                           NULL)
+};
+
+static const struct of_device_id s5p_cec_match[] = {
+        {
+                .compatible     = "samsung,s5p-cec",
+        },
+        {},
+};
+MODULE_DEVICE_TABLE(of, s5p_cec_match);
+
+static struct platform_driver s5p_cec_pdrv = {
+        .probe  = s5p_cec_probe,
+        .remove = s5p_cec_remove,
+        .driver = {
+                .name           = CEC_NAME,
+                .of_match_table = s5p_cec_match,
+                .pm             = &s5p_cec_pm_ops,
+        },
+};
+
+module_platform_driver(s5p_cec_pdrv);
+
+MODULE_AUTHOR("Kamil Debski <kamil@wypas.org>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Samsung S5P CEC driver");
diff --git a/drivers/media/platform/s5p-cec/s5p_cec.h b/drivers/media/platform/s5p-cec/s5p_cec.h
new file mode 100644
index 000000000000..7015845c1caa
--- /dev/null
+++ b/drivers/media/platform/s5p-cec/s5p_cec.h
@@ -0,0 +1,79 @@
+/* drivers/media/platform/s5p-cec/s5p_cec.h
+ *
+ * Samsung S5P HDMI CEC driver
+ *
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef _S5P_CEC_H_
+#define _S5P_CEC_H_ __FILE__
+
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/timer.h>
+#include <linux/version.h>
+#include <linux/workqueue.h>
+#include <media/cec.h>
+
+#include "exynos_hdmi_cec.h"
+#include "regs-cec.h"
+#include "s5p_cec.h"
+
+#define CEC_NAME        "s5p-cec"
+
+#define CEC_STATUS_TX_RUNNING           (1 << 0)
+#define CEC_STATUS_TX_TRANSFERRING      (1 << 1)
+#define CEC_STATUS_TX_DONE              (1 << 2)
+#define CEC_STATUS_TX_ERROR             (1 << 3)
+#define CEC_STATUS_TX_BYTES             (0xFF << 8)
+#define CEC_STATUS_RX_RUNNING           (1 << 16)
+#define CEC_STATUS_RX_RECEIVING         (1 << 17)
+#define CEC_STATUS_RX_DONE              (1 << 18)
+#define CEC_STATUS_RX_ERROR             (1 << 19)
+#define CEC_STATUS_RX_BCAST             (1 << 20)
+#define CEC_STATUS_RX_BYTES             (0xFF << 24)
+
+#define CEC_WORKER_TX_DONE              (1 << 0)
+#define CEC_WORKER_RX_MSG               (1 << 1)
+
+/* CEC Rx buffer size */
+#define CEC_RX_BUFF_SIZE                16
+/* CEC Tx buffer size */
+#define CEC_TX_BUFF_SIZE                16
+
+enum cec_state {
+        STATE_IDLE,
+        STATE_BUSY,
+        STATE_DONE,
+        STATE_ERROR
+};
+
+struct cec_notifier;
+
+struct s5p_cec_dev {
+        struct cec_adapter      *adap;
+        struct clk              *clk;
+        struct device           *dev;
+        struct mutex            lock;
+        struct regmap           *pmu;
+        struct cec_notifier     *notifier;
+        int                     irq;
+        void __iomem            *reg;
+
+        enum cec_state          rx;
+        enum cec_state          tx;
+        struct cec_msg          msg;
+};
+
+#endif /* _S5P_CEC_H_ */
diff --git a/drivers/media/platform/s5p-g2d/g2d.c b/drivers/media/platform/s5p-g2d/g2d.c
index 62c0dec30b59..81ed5cd5cd5d 100644
--- a/drivers/media/platform/s5p-g2d/g2d.c
+++ b/drivers/media/platform/s5p-g2d/g2d.c
@@ -679,7 +679,7 @@ static int g2d_probe(struct platform_device *pdev)
                                                 0, pdev->name, dev);
         if (ret) {
                 dev_err(&pdev->dev, "failed to install IRQ\n");
-                goto put_clk_gate;
+                goto unprep_clk_gate;
         }
 
         vb2_dma_contig_set_max_seg_size(&pdev->dev, DMA_BIT_MASK(32));
diff --git a/drivers/media/platform/s5p-mfc/regs-mfc-v6.h b/drivers/media/platform/s5p-mfc/regs-mfc-v6.h
index d2cd35916dc5..c0166ee9a455 100644
--- a/drivers/media/platform/s5p-mfc/regs-mfc-v6.h
+++ b/drivers/media/platform/s5p-mfc/regs-mfc-v6.h
@@ -403,7 +403,7 @@
 #define MFC_OTHER_ENC_CTX_BUF_SIZE_V6   (12 * SZ_1K)    /*  12KB */
 
 /* MFCv6 variant defines */
-#define MAX_FW_SIZE_V6                  (SZ_1M)         /* 1MB */
+#define MAX_FW_SIZE_V6                  (SZ_512K)       /* 512KB */
 #define MAX_CPB_SIZE_V6                 (3 * SZ_1M)     /* 3MB */
 #define MFC_VERSION_V6                  0x61
 #define MFC_NUM_PORTS_V6                1
diff --git a/drivers/media/platform/s5p-mfc/regs-mfc-v7.h b/drivers/media/platform/s5p-mfc/regs-mfc-v7.h
index 1a5c6fdf7846..9f220769d970 100644
--- a/drivers/media/platform/s5p-mfc/regs-mfc-v7.h
+++ b/drivers/media/platform/s5p-mfc/regs-mfc-v7.h
@@ -34,7 +34,7 @@
 #define S5P_FIMV_E_VP8_NUM_T_LAYER_V7                   0xfdc4
 
 /* MFCv7 variant defines */
-#define MAX_FW_SIZE_V7                  (SZ_1M)         /* 1MB */
+#define MAX_FW_SIZE_V7                  (SZ_512K)       /* 512KB */
 #define MAX_CPB_SIZE_V7                 (3 * SZ_1M)     /* 3MB */
 #define MFC_VERSION_V7                  0x72
 #define MFC_NUM_PORTS_V7                1
diff --git a/drivers/media/platform/s5p-mfc/regs-mfc-v8.h b/drivers/media/platform/s5p-mfc/regs-mfc-v8.h
index 4d1c3750eb5e..75f5f7511d72 100644
--- a/drivers/media/platform/s5p-mfc/regs-mfc-v8.h
+++ b/drivers/media/platform/s5p-mfc/regs-mfc-v8.h
@@ -116,7 +116,7 @@
 #define S5P_FIMV_D_ALIGN_PLANE_SIZE_V8  64
 
 /* MFCv8 variant defines */
-#define MAX_FW_SIZE_V8                  (SZ_1M)         /* 1MB */
+#define MAX_FW_SIZE_V8                  (SZ_512K)       /* 512KB */
 #define MAX_CPB_SIZE_V8                 (3 * SZ_1M)     /* 3MB */
 #define MFC_VERSION_V8                  0x80
 #define MFC_NUM_PORTS_V8                1
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc.c b/drivers/media/platform/s5p-mfc/s5p_mfc.c
index bb0a5887c9a9..1afde5021ca6 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc.c
@@ -22,6 +22,7 @@
 #include <media/v4l2-event.h>
 #include <linux/workqueue.h>
 #include <linux/of.h>
+#include <linux/of_device.h>
 #include <linux/of_reserved_mem.h>
 #include <media/videobuf2-v4l2.h>
 #include "s5p_mfc_common.h"
@@ -42,6 +43,10 @@ int mfc_debug_level;
 module_param_named(debug, mfc_debug_level, int, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(debug, "Debug level - higher value produces more verbose messages");
 
+static char *mfc_mem_size;
+module_param_named(mem, mfc_mem_size, charp, 0644);
+MODULE_PARM_DESC(mem, "Preallocated memory size for the firmware and context buffers");
+
 /* Helper functions for interrupt processing */
 
 /* Remove from hw execution round robin */
@@ -206,6 +211,7 @@ static void s5p_mfc_watchdog_worker(struct work_struct *work)
                 }
                 s5p_mfc_clock_on();
                 ret = s5p_mfc_init_hw(dev);
+                s5p_mfc_clock_off();
                 if (ret)
                         mfc_err("Failed to reinit FW\n");
         }
@@ -666,9 +672,9 @@ static irqreturn_t s5p_mfc_irq(int irq, void *priv)
                                 break;
                         }
                         s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
-                        wake_up_ctx(ctx, reason, err);
                         WARN_ON(test_and_clear_bit(0, &dev->hw_lock) == 0);
                         s5p_mfc_clock_off();
+                        wake_up_ctx(ctx, reason, err);
                         s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
                 } else {
                         s5p_mfc_handle_frame(ctx, reason, err);
@@ -682,15 +688,11 @@ static irqreturn_t s5p_mfc_irq(int irq, void *priv)
         case S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET:
                 ctx->inst_no = s5p_mfc_hw_call(dev->mfc_ops, get_inst_no, dev);
                 ctx->state = MFCINST_GOT_INST;
-                clear_work_bit(ctx);
-                wake_up(&ctx->queue);
                 goto irq_cleanup_hw;
 
         case S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET:
-                clear_work_bit(ctx);
                 ctx->inst_no = MFC_NO_INSTANCE_SET;
                 ctx->state = MFCINST_FREE;
-                wake_up(&ctx->queue);
                 goto irq_cleanup_hw;
 
         case S5P_MFC_R2H_CMD_SYS_INIT_RET:
@@ -700,9 +702,9 @@ static irqreturn_t s5p_mfc_irq(int irq, void *priv)
                 if (ctx)
                         clear_work_bit(ctx);
                 s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
-                wake_up_dev(dev, reason, err);
                 clear_bit(0, &dev->hw_lock);
                 clear_bit(0, &dev->enter_suspend);
+                wake_up_dev(dev, reason, err);
                 break;
 
         case S5P_MFC_R2H_CMD_INIT_BUFFERS_RET:
@@ -717,9 +719,7 @@ static irqreturn_t s5p_mfc_irq(int irq, void *priv)
                 break;
 
         case S5P_MFC_R2H_CMD_DPB_FLUSH_RET:
-                clear_work_bit(ctx);
                 ctx->state = MFCINST_RUNNING;
-                wake_up(&ctx->queue);
                 goto irq_cleanup_hw;
 
         default:
@@ -738,6 +738,8 @@ irq_cleanup_hw:
                 mfc_err("Failed to unlock hw\n");
 
         s5p_mfc_clock_off();
+        clear_work_bit(ctx);
+        wake_up(&ctx->queue);
 
         s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
         spin_unlock(&dev->irqlock);
@@ -764,6 +766,7 @@ static int s5p_mfc_open(struct file *file)
                 ret = -ENOMEM;
                 goto err_alloc;
         }
+        init_waitqueue_head(&ctx->queue);
         v4l2_fh_init(&ctx->fh, vdev);
         file->private_data = &ctx->fh;
         v4l2_fh_add(&ctx->fh);
@@ -866,7 +869,6 @@ static int s5p_mfc_open(struct file *file)
         /* Init videobuf2 queue for OUTPUT */
         q = &ctx->vq_src;
         q->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
-        q->io_modes = VB2_MMAP;
         q->drv_priv = &ctx->fh;
         q->lock = &dev->mfc_mutex;
         if (vdev == dev->vfd_dec) {
@@ -899,7 +901,6 @@ static int s5p_mfc_open(struct file *file)
                 mfc_err("Failed to initialize videobuf2 queue(output)\n");
                 goto err_queue_init;
         }
-        init_waitqueue_head(&ctx->queue);
         mutex_unlock(&dev->mfc_mutex);
         mfc_debug_leave();
         return ret;
@@ -1106,58 +1107,158 @@ static struct device *s5p_mfc_alloc_memdev(struct device *dev,
         return NULL;
 }
 
-static int s5p_mfc_configure_dma_memory(struct s5p_mfc_dev *mfc_dev)
+static int s5p_mfc_configure_2port_memory(struct s5p_mfc_dev *mfc_dev)
 {
         struct device *dev = &mfc_dev->plat_dev->dev;
-
-        /*
-         * When IOMMU is available, we cannot use the default configuration,
-         * because of MFC firmware requirements: address space limited to
-         * 256M and non-zero default start address.
-         * This is still simplified, not optimal configuration, but for now
-         * IOMMU core doesn't allow to configure device's IOMMUs channel
-         * separately.
-         */
-        if (exynos_is_iommu_available(dev)) {
-                int ret = exynos_configure_iommu(dev, S5P_MFC_IOMMU_DMA_BASE,
-                                                 S5P_MFC_IOMMU_DMA_SIZE);
-                if (ret == 0)
-                        mfc_dev->mem_dev_l = mfc_dev->mem_dev_r = dev;
-                return ret;
-        }
+        void *bank2_virt;
+        dma_addr_t bank2_dma_addr;
+        unsigned long align_size = 1 << MFC_BASE_ALIGN_ORDER;
+        int ret;
 
         /*
          * Create and initialize virtual devices for accessing
          * reserved memory regions.
          */
-        mfc_dev->mem_dev_l = s5p_mfc_alloc_memdev(dev, "left",
-                                                  MFC_BANK1_ALLOC_CTX);
-        if (!mfc_dev->mem_dev_l)
+        mfc_dev->mem_dev[BANK_L_CTX] = s5p_mfc_alloc_memdev(dev, "left",
+                                                           BANK_L_CTX);
+        if (!mfc_dev->mem_dev[BANK_L_CTX])
                 return -ENODEV;
-        mfc_dev->mem_dev_r = s5p_mfc_alloc_memdev(dev, "right",
-                                                  MFC_BANK2_ALLOC_CTX);
-        if (!mfc_dev->mem_dev_r) {
-                device_unregister(mfc_dev->mem_dev_l);
+        mfc_dev->mem_dev[BANK_R_CTX] = s5p_mfc_alloc_memdev(dev, "right",
+                                                           BANK_R_CTX);
+        if (!mfc_dev->mem_dev[BANK_R_CTX]) {
+                device_unregister(mfc_dev->mem_dev[BANK_L_CTX]);
                 return -ENODEV;
         }
 
+        /* Allocate memory for firmware and initialize both banks addresses */
+        ret = s5p_mfc_alloc_firmware(mfc_dev);
+        if (ret) {
+                device_unregister(mfc_dev->mem_dev[BANK_R_CTX]);
+                device_unregister(mfc_dev->mem_dev[BANK_L_CTX]);
+                return ret;
+        }
+
+        mfc_dev->dma_base[BANK_L_CTX] = mfc_dev->fw_buf.dma;
+
+        bank2_virt = dma_alloc_coherent(mfc_dev->mem_dev[BANK_R_CTX],
+                                       align_size, &bank2_dma_addr, GFP_KERNEL);
+        if (!bank2_virt) {
+                mfc_err("Allocating bank2 base failed\n");
+                s5p_mfc_release_firmware(mfc_dev);
+                device_unregister(mfc_dev->mem_dev[BANK_R_CTX]);
+                device_unregister(mfc_dev->mem_dev[BANK_L_CTX]);
+                return -ENOMEM;
+        }
+
+        /* Valid buffers passed to MFC encoder with LAST_FRAME command
+         * should not have address of bank2 - MFC will treat it as a null frame.
+         * To avoid such situation we set bank2 address below the pool address.
+         */
+        mfc_dev->dma_base[BANK_R_CTX] = bank2_dma_addr - align_size;
+
+        dma_free_coherent(mfc_dev->mem_dev[BANK_R_CTX], align_size, bank2_virt,
+                          bank2_dma_addr);
+
+        vb2_dma_contig_set_max_seg_size(mfc_dev->mem_dev[BANK_L_CTX],
+                                        DMA_BIT_MASK(32));
+        vb2_dma_contig_set_max_seg_size(mfc_dev->mem_dev[BANK_R_CTX],
+                                        DMA_BIT_MASK(32));
+
         return 0;
 }
 
-static void s5p_mfc_unconfigure_dma_memory(struct s5p_mfc_dev *mfc_dev)
+static void s5p_mfc_unconfigure_2port_memory(struct s5p_mfc_dev *mfc_dev)
+{
+        device_unregister(mfc_dev->mem_dev[BANK_L_CTX]);
+        device_unregister(mfc_dev->mem_dev[BANK_R_CTX]);
+        vb2_dma_contig_clear_max_seg_size(mfc_dev->mem_dev[BANK_L_CTX]);
+        vb2_dma_contig_clear_max_seg_size(mfc_dev->mem_dev[BANK_R_CTX]);
+}
+
+static int s5p_mfc_configure_common_memory(struct s5p_mfc_dev *mfc_dev)
 {
         struct device *dev = &mfc_dev->plat_dev->dev;
+        unsigned long mem_size = SZ_4M;
+        unsigned int bitmap_size;
 
-        if (exynos_is_iommu_available(dev)) {
-                exynos_unconfigure_iommu(dev);
-                return;
+        if (IS_ENABLED(CONFIG_DMA_CMA) || exynos_is_iommu_available(dev))
+                mem_size = SZ_8M;
+
+        if (mfc_mem_size)
+                mem_size = memparse(mfc_mem_size, NULL);
+
+        bitmap_size = BITS_TO_LONGS(mem_size >> PAGE_SHIFT) * sizeof(long);
+
+        mfc_dev->mem_bitmap = kzalloc(bitmap_size, GFP_KERNEL);
+        if (!mfc_dev->mem_bitmap)
+                return -ENOMEM;
+
+        mfc_dev->mem_virt = dma_alloc_coherent(dev, mem_size,
+                                               &mfc_dev->mem_base, GFP_KERNEL);
+        if (!mfc_dev->mem_virt) {
+                kfree(mfc_dev->mem_bitmap);
+                dev_err(dev, "failed to preallocate %ld MiB for the firmware and context buffers\n",
+                        (mem_size / SZ_1M));
+                return -ENOMEM;
         }
+        mfc_dev->mem_size = mem_size;
+        mfc_dev->dma_base[BANK_L_CTX] = mfc_dev->mem_base;
+        mfc_dev->dma_base[BANK_R_CTX] = mfc_dev->mem_base;
+
+        /*
+         * MFC hardware cannot handle 0 as a base address, so mark first 128K
+         * as used (to keep required base alignment) and adjust base address
+         */
+        if (mfc_dev->mem_base == (dma_addr_t)0) {
+                unsigned int offset = 1 << MFC_BASE_ALIGN_ORDER;
+
+                bitmap_set(mfc_dev->mem_bitmap, 0, offset >> PAGE_SHIFT);
+                mfc_dev->dma_base[BANK_L_CTX] += offset;
+                mfc_dev->dma_base[BANK_R_CTX] += offset;
+        }
+
+        /* Firmware allocation cannot fail in this case */
+        s5p_mfc_alloc_firmware(mfc_dev);
 
-        device_unregister(mfc_dev->mem_dev_l);
-        device_unregister(mfc_dev->mem_dev_r);
+        mfc_dev->mem_dev[BANK_L_CTX] = mfc_dev->mem_dev[BANK_R_CTX] = dev;
+        vb2_dma_contig_set_max_seg_size(dev, DMA_BIT_MASK(32));
+
+        dev_info(dev, "preallocated %ld MiB buffer for the firmware and context buffers\n",
+                 (mem_size / SZ_1M));
+
+        return 0;
+}
+
+static void s5p_mfc_unconfigure_common_memory(struct s5p_mfc_dev *mfc_dev)
+{
+        struct device *dev = &mfc_dev->plat_dev->dev;
+
+        dma_free_coherent(dev, mfc_dev->mem_size, mfc_dev->mem_virt,
+                          mfc_dev->mem_base);
+        kfree(mfc_dev->mem_bitmap);
+        vb2_dma_contig_clear_max_seg_size(dev);
+}
+
+static int s5p_mfc_configure_dma_memory(struct s5p_mfc_dev *mfc_dev)
+{
+        struct device *dev = &mfc_dev->plat_dev->dev;
+
+        if (exynos_is_iommu_available(dev) || !IS_TWOPORT(mfc_dev))
+                return s5p_mfc_configure_common_memory(mfc_dev);
+        else
+                return s5p_mfc_configure_2port_memory(mfc_dev);
 }
 
-static void *mfc_get_drv_data(struct platform_device *pdev);
+static void s5p_mfc_unconfigure_dma_memory(struct s5p_mfc_dev *mfc_dev)
+{
+        struct device *dev = &mfc_dev->plat_dev->dev;
+
+        s5p_mfc_release_firmware(mfc_dev);
+        if (exynos_is_iommu_available(dev) || !IS_TWOPORT(mfc_dev))
+                s5p_mfc_unconfigure_common_memory(mfc_dev);
+        else
+                s5p_mfc_unconfigure_2port_memory(mfc_dev);
+}
 
 /* MFC probe function */
 static int s5p_mfc_probe(struct platform_device *pdev)
@@ -1182,7 +1283,7 @@ static int s5p_mfc_probe(struct platform_device *pdev)
                 return -ENODEV;
         }
 
-        dev->variant = mfc_get_drv_data(pdev);
+        dev->variant = of_device_get_match_data(&pdev->dev);
 
         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
         dev->regs_base = devm_ioremap_resource(&pdev->dev, res);
@@ -1214,19 +1315,18 @@ static int s5p_mfc_probe(struct platform_device *pdev)
                 goto err_dma;
         }
 
-        vb2_dma_contig_set_max_seg_size(dev->mem_dev_l, DMA_BIT_MASK(32));
-        vb2_dma_contig_set_max_seg_size(dev->mem_dev_r, DMA_BIT_MASK(32));
-
         mutex_init(&dev->mfc_mutex);
-
-        ret = s5p_mfc_alloc_firmware(dev);
-        if (ret)
-                goto err_res;
+        init_waitqueue_head(&dev->queue);
+        dev->hw_lock = 0;
+        INIT_WORK(&dev->watchdog_work, s5p_mfc_watchdog_worker);
+        atomic_set(&dev->watchdog_cnt, 0);
+        init_timer(&dev->watchdog_timer);
+        dev->watchdog_timer.data = (unsigned long)dev;
+        dev->watchdog_timer.function = s5p_mfc_watchdog;
 
         ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
         if (ret)
                 goto err_v4l2_dev_reg;
-        init_waitqueue_head(&dev->queue);
 
         /* decoder */
         vfd = video_device_alloc();
@@ -1263,13 +1363,6 @@ static int s5p_mfc_probe(struct platform_device *pdev)
         video_set_drvdata(vfd, dev);
         platform_set_drvdata(pdev, dev);
 
-        dev->hw_lock = 0;
-        INIT_WORK(&dev->watchdog_work, s5p_mfc_watchdog_worker);
-        atomic_set(&dev->watchdog_cnt, 0);
-        init_timer(&dev->watchdog_timer);
-        dev->watchdog_timer.data = (unsigned long)dev;
-        dev->watchdog_timer.function = s5p_mfc_watchdog;
-
         /* Initialize HW ops and commands based on MFC version */
         s5p_mfc_init_hw_ops(dev);
         s5p_mfc_init_hw_cmds(dev);
@@ -1305,8 +1398,6 @@ err_enc_alloc:
 err_dec_alloc:
         v4l2_device_unregister(&dev->v4l2_dev);
 err_v4l2_dev_reg:
-        s5p_mfc_release_firmware(dev);
-err_res:
         s5p_mfc_final_pm(dev);
 err_dma:
         s5p_mfc_unconfigure_dma_memory(dev);
@@ -1348,10 +1439,7 @@ static int s5p_mfc_remove(struct platform_device *pdev)
         video_device_release(dev->vfd_enc);
         video_device_release(dev->vfd_dec);
         v4l2_device_unregister(&dev->v4l2_dev);
-        s5p_mfc_release_firmware(dev);
         s5p_mfc_unconfigure_dma_memory(dev);
-        vb2_dma_contig_clear_max_seg_size(dev->mem_dev_l);
-        vb2_dma_contig_clear_max_seg_size(dev->mem_dev_r);
 
         s5p_mfc_final_pm(dev);
         return 0;
@@ -1423,16 +1511,11 @@ static struct s5p_mfc_buf_size buf_size_v5 = {
         .priv   = &mfc_buf_size_v5,
 };
 
-static struct s5p_mfc_buf_align mfc_buf_align_v5 = {
-        .base = MFC_BASE_ALIGN_ORDER,
-};
-
 static struct s5p_mfc_variant mfc_drvdata_v5 = {
         .version        = MFC_VERSION,
         .version_bit    = MFC_V5_BIT,
         .port_num       = MFC_NUM_PORTS,
         .buf_size       = &buf_size_v5,
-        .buf_align      = &mfc_buf_align_v5,
         .fw_name[0]     = "s5p-mfc.fw",
         .clk_names      = {"mfc", "sclk_mfc"},
         .num_clocks     = 2,
@@ -1453,16 +1536,11 @@ static struct s5p_mfc_buf_size buf_size_v6 = {
         .priv   = &mfc_buf_size_v6,
 };
 
-static struct s5p_mfc_buf_align mfc_buf_align_v6 = {
-        .base = 0,
-};
-
 static struct s5p_mfc_variant mfc_drvdata_v6 = {
         .version        = MFC_VERSION_V6,
         .version_bit    = MFC_V6_BIT,
         .port_num       = MFC_NUM_PORTS_V6,
         .buf_size       = &buf_size_v6,
-        .buf_align      = &mfc_buf_align_v6,
         .fw_name[0]     = "s5p-mfc-v6.fw",
         /*
          * v6-v2 firmware contains bug fixes and interface change
@@ -1487,16 +1565,11 @@ static struct s5p_mfc_buf_size buf_size_v7 = {
         .priv   = &mfc_buf_size_v7,
 };
 
-static struct s5p_mfc_buf_align mfc_buf_align_v7 = {
-        .base = 0,
-};
-
 static struct s5p_mfc_variant mfc_drvdata_v7 = {
         .version        = MFC_VERSION_V7,
         .version_bit    = MFC_V7_BIT,
         .port_num       = MFC_NUM_PORTS_V7,
         .buf_size       = &buf_size_v7,
-        .buf_align      = &mfc_buf_align_v7,
         .fw_name[0]     = "s5p-mfc-v7.fw",
         .clk_names      = {"mfc", "sclk_mfc"},
         .num_clocks     = 2,
@@ -1516,16 +1589,11 @@ static struct s5p_mfc_buf_size buf_size_v8 = {
         .priv   = &mfc_buf_size_v8,
 };
 
-static struct s5p_mfc_buf_align mfc_buf_align_v8 = {
-        .base = 0,
-};
-
 static struct s5p_mfc_variant mfc_drvdata_v8 = {
         .version        = MFC_VERSION_V8,
         .version_bit    = MFC_V8_BIT,
         .port_num       = MFC_NUM_PORTS_V8,
         .buf_size       = &buf_size_v8,
-        .buf_align      = &mfc_buf_align_v8,
         .fw_name[0]     = "s5p-mfc-v8.fw",
         .clk_names      = {"mfc"},
         .num_clocks     = 1,
@@ -1536,7 +1604,6 @@ static struct s5p_mfc_variant mfc_drvdata_v8_5433 = {
         .version_bit    = MFC_V8_BIT,
         .port_num       = MFC_NUM_PORTS_V8,
         .buf_size       = &buf_size_v8,
-        .buf_align      = &mfc_buf_align_v8,
         .fw_name[0]     = "s5p-mfc-v8.fw",
         .clk_names      = {"pclk", "aclk", "aclk_xiu"},
         .num_clocks     = 3,
@@ -1563,18 +1630,6 @@ static const struct of_device_id exynos_mfc_match[] = {
 };
 MODULE_DEVICE_TABLE(of, exynos_mfc_match);
 
-static void *mfc_get_drv_data(struct platform_device *pdev)
-{
-        struct s5p_mfc_variant *driver_data = NULL;
-        const struct of_device_id *match;
-
-        match = of_match_node(exynos_mfc_match, pdev->dev.of_node);
-        if (match)
-                driver_data = (struct s5p_mfc_variant *)match->data;
-
-        return driver_data;
-}
-
 static struct platform_driver s5p_mfc_driver = {
         .probe          = s5p_mfc_probe,
         .remove         = s5p_mfc_remove,
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
index 8c4739ca16d6..4c80bb4243be 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
@@ -47,7 +47,7 @@ static int s5p_mfc_sys_init_cmd_v5(struct s5p_mfc_dev *dev)
         struct s5p_mfc_cmd_args h2r_args;
 
         memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
-        h2r_args.arg[0] = dev->fw_size;
+        h2r_args.arg[0] = dev->fw_buf.size;
         return s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_SYS_INIT,
                         &h2r_args);
 }
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
index ab23236aa942..4220914529b2 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
@@ -33,8 +33,9 @@
 *  while mmaping */
 #define DST_QUEUE_OFF_BASE      (1 << 30)
 
-#define MFC_BANK1_ALLOC_CTX     0
-#define MFC_BANK2_ALLOC_CTX     1
+#define BANK_L_CTX      0
+#define BANK_R_CTX      1
+#define BANK_CTX_NUM    2
 
 #define MFC_BANK1_ALIGN_ORDER   13
 #define MFC_BANK2_ALIGN_ORDER   13
@@ -44,14 +45,6 @@
 
 #include <media/videobuf2-dma-contig.h>
 
-static inline dma_addr_t s5p_mfc_mem_cookie(void *a, void *b)
-{
-        /* Same functionality as the vb2_dma_contig_plane_paddr */
-        dma_addr_t *paddr = vb2_dma_contig_memops.cookie(b);
-
-        return *paddr;
-}
-
 /* MFC definitions */
 #define MFC_MAX_EXTRA_DPB       5
 #define MFC_MAX_BUFFERS         32
@@ -200,7 +193,7 @@ struct s5p_mfc_buf {
  */
 struct s5p_mfc_pm {
         struct clk      *clock_gate;
-        const char      **clk_names;
+        const char * const *clk_names;
         struct clk      *clocks[MFC_MAX_CLOCKS];
         int             num_clocks;
         bool            use_clock_gating;
@@ -229,16 +222,11 @@ struct s5p_mfc_buf_size {
         void *priv;
 };
 
-struct s5p_mfc_buf_align {
-        unsigned int base;
-};
-
 struct s5p_mfc_variant {
         unsigned int version;
         unsigned int port_num;
         u32 version_bit;
         struct s5p_mfc_buf_size *buf_size;
-        struct s5p_mfc_buf_align *buf_align;
         char    *fw_name[MFC_FW_MAX_VERSIONS];
         const char      *clk_names[MFC_MAX_CLOCKS];
         int             num_clocks;
@@ -252,12 +240,14 @@ struct s5p_mfc_variant {
  *                      buffer accessed by driver
  * @dma:                DMA address, only valid when kernel DMA API used
  * @size:               size of the buffer
+ * @ctx:                memory context (bank) used for this allocation
  */
 struct s5p_mfc_priv_buf {
         unsigned long   ofs;
         void            *virt;
         dma_addr_t      dma;
         size_t          size;
+        unsigned int    ctx;
 };
 
 /**
@@ -267,8 +257,7 @@ struct s5p_mfc_priv_buf {
  * @vfd_dec:            video device for decoding
  * @vfd_enc:            video device for encoding
  * @plat_dev:           platform device
- * @mem_dev_l:          child device of the left memory bank (0)
- * @mem_dev_r:          child device of the right memory bank (1)
+ * @mem_dev[]:          child devices of the memory banks
  * @regs_base:          base address of the MFC hw registers
  * @irq:                irq resource
  * @dec_ctrl_handler:   control framework handler for decoding
@@ -286,8 +275,7 @@ struct s5p_mfc_priv_buf {
  * @queue:              waitqueue for waiting for completion of device commands
  * @fw_size:            size of firmware
  * @fw_virt_addr:       virtual firmware address
- * @bank1:              address of the beginning of bank 1 memory
- * @bank2:              address of the beginning of bank 2 memory
+ * @dma_base[]:         address of the beginning of memory banks
  * @hw_lock:            used for hardware locking
  * @ctx:                array of driver contexts
  * @curr_ctx:           number of the currently running context
@@ -310,14 +298,13 @@ struct s5p_mfc_dev {
         struct video_device     *vfd_dec;
         struct video_device     *vfd_enc;
         struct platform_device  *plat_dev;
-        struct device           *mem_dev_l;
-        struct device           *mem_dev_r;
+        struct device           *mem_dev[BANK_CTX_NUM];
         void __iomem            *regs_base;
         int                     irq;
         struct v4l2_ctrl_handler dec_ctrl_handler;
         struct v4l2_ctrl_handler enc_ctrl_handler;
         struct s5p_mfc_pm       pm;
-        struct s5p_mfc_variant  *variant;
+        const struct s5p_mfc_variant    *variant;
         int num_inst;
         spinlock_t irqlock;     /* lock when operating on context */
         spinlock_t condlock;    /* lock when changing/checking if a context is
@@ -327,10 +314,12 @@ struct s5p_mfc_dev {
         int int_type;
         unsigned int int_err;
         wait_queue_head_t queue;
-        size_t fw_size;
-        void *fw_virt_addr;
-        dma_addr_t bank1;
-        dma_addr_t bank2;
+        struct s5p_mfc_priv_buf fw_buf;
+        size_t mem_size;
+        dma_addr_t mem_base;
+        unsigned long *mem_bitmap;
+        void *mem_virt;
+        dma_addr_t dma_base[BANK_CTX_NUM];
         unsigned long hw_lock;
         struct s5p_mfc_ctx *ctx[MFC_NUM_CONTEXTS];
         int curr_ctx;
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
index cc888713b3b6..69ef9c23a99a 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
@@ -26,51 +26,22 @@
 /* Allocate memory for firmware */
 int s5p_mfc_alloc_firmware(struct s5p_mfc_dev *dev)
 {
-        void *bank2_virt;
-        dma_addr_t bank2_dma_addr;
+        struct s5p_mfc_priv_buf *fw_buf = &dev->fw_buf;
+        int err;
 
-        dev->fw_size = dev->variant->buf_size->fw;
+        fw_buf->size = dev->variant->buf_size->fw;
 
-        if (dev->fw_virt_addr) {
+        if (fw_buf->virt) {
                 mfc_err("Attempting to allocate firmware when it seems that it is already loaded\n");
                 return -ENOMEM;
         }
 
-        dev->fw_virt_addr = dma_alloc_coherent(dev->mem_dev_l, dev->fw_size,
-                                        &dev->bank1, GFP_KERNEL);
-
-        if (!dev->fw_virt_addr) {
+        err = s5p_mfc_alloc_priv_buf(dev, BANK_L_CTX, &dev->fw_buf);
+        if (err) {
                 mfc_err("Allocating bitprocessor buffer failed\n");
-                return -ENOMEM;
+                return err;
         }
 
-        if (HAS_PORTNUM(dev) && IS_TWOPORT(dev)) {
-                bank2_virt = dma_alloc_coherent(dev->mem_dev_r, 1 << MFC_BASE_ALIGN_ORDER,
-                                        &bank2_dma_addr, GFP_KERNEL);
-
-                if (!bank2_virt) {
-                        mfc_err("Allocating bank2 base failed\n");
-                        dma_free_coherent(dev->mem_dev_l, dev->fw_size,
-                                dev->fw_virt_addr, dev->bank1);
-                        dev->fw_virt_addr = NULL;
-                        return -ENOMEM;
-                }
-
-                /* Valid buffers passed to MFC encoder with LAST_FRAME command
-                 * should not have address of bank2 - MFC will treat it as a null frame.
-                 * To avoid such situation we set bank2 address below the pool address.
-                 */
-                dev->bank2 = bank2_dma_addr - (1 << MFC_BASE_ALIGN_ORDER);
-
-                dma_free_coherent(dev->mem_dev_r, 1 << MFC_BASE_ALIGN_ORDER,
-                        bank2_virt, bank2_dma_addr);
-
-        } else {
-                /* In this case bank2 can point to the same address as bank1.
-                 * Firmware will always occupy the beginning of this area so it is
-                 * impossible having a video frame buffer with zero address. */
-                dev->bank2 = dev->bank1;
-        }
         return 0;
 }
 
@@ -99,17 +70,17 @@ int s5p_mfc_load_firmware(struct s5p_mfc_dev *dev)
                 mfc_err("Firmware is not present in the /lib/firmware directory nor compiled in kernel\n");
                 return -EINVAL;
         }
-        if (fw_blob->size > dev->fw_size) {
+        if (fw_blob->size > dev->fw_buf.size) {
                 mfc_err("MFC firmware is too big to be loaded\n");
                 release_firmware(fw_blob);
                 return -ENOMEM;
         }
-        if (!dev->fw_virt_addr) {
+        if (!dev->fw_buf.virt) {
                 mfc_err("MFC firmware is not allocated\n");
                 release_firmware(fw_blob);
                 return -EINVAL;
         }
-        memcpy(dev->fw_virt_addr, fw_blob->data, fw_blob->size);
+        memcpy(dev->fw_buf.virt, fw_blob->data, fw_blob->size);
         wmb();
         release_firmware(fw_blob);
         mfc_debug_leave();
@@ -121,11 +92,7 @@ int s5p_mfc_release_firmware(struct s5p_mfc_dev *dev)
 {
         /* Before calling this function one has to make sure
          * that MFC is no longer processing */
-        if (!dev->fw_virt_addr)
-                return -EINVAL;
-        dma_free_coherent(dev->mem_dev_l, dev->fw_size, dev->fw_virt_addr,
-                                                dev->bank1);
-        dev->fw_virt_addr = NULL;
+        s5p_mfc_release_priv_buf(dev, &dev->fw_buf);
         return 0;
 }
 
@@ -210,13 +177,18 @@ int s5p_mfc_reset(struct s5p_mfc_dev *dev)
 static inline void s5p_mfc_init_memctrl(struct s5p_mfc_dev *dev)
 {
         if (IS_MFCV6_PLUS(dev)) {
-                mfc_write(dev, dev->bank1, S5P_FIMV_RISC_BASE_ADDRESS_V6);
-                mfc_debug(2, "Base Address : %pad\n", &dev->bank1);
+                mfc_write(dev, dev->dma_base[BANK_L_CTX],
+                          S5P_FIMV_RISC_BASE_ADDRESS_V6);
+                mfc_debug(2, "Base Address : %pad\n",
+                          &dev->dma_base[BANK_L_CTX]);
         } else {
-                mfc_write(dev, dev->bank1, S5P_FIMV_MC_DRAMBASE_ADR_A);
-                mfc_write(dev, dev->bank2, S5P_FIMV_MC_DRAMBASE_ADR_B);
+                mfc_write(dev, dev->dma_base[BANK_L_CTX],
+                          S5P_FIMV_MC_DRAMBASE_ADR_A);
+                mfc_write(dev, dev->dma_base[BANK_R_CTX],
+                          S5P_FIMV_MC_DRAMBASE_ADR_B);
                 mfc_debug(2, "Bank1: %pad, Bank2: %pad\n",
-                                &dev->bank1, &dev->bank2);
+                          &dev->dma_base[BANK_L_CTX],
+                          &dev->dma_base[BANK_R_CTX]);
         }
 }
 
@@ -240,7 +212,7 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
         int ret;
 
         mfc_debug_enter();
-        if (!dev->fw_virt_addr) {
+        if (!dev->fw_buf.virt) {
                 mfc_err("Firmware memory is not allocated.\n");
                 return -EINVAL;
         }
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h
index 8e5df041edf7..45c807bf19cc 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h
@@ -18,7 +18,6 @@
 int s5p_mfc_release_firmware(struct s5p_mfc_dev *dev);
 int s5p_mfc_alloc_firmware(struct s5p_mfc_dev *dev);
 int s5p_mfc_load_firmware(struct s5p_mfc_dev *dev);
-int s5p_mfc_reload_firmware(struct s5p_mfc_dev *dev);
 
 int s5p_mfc_init_hw(struct s5p_mfc_dev *dev);
 void s5p_mfc_deinit_hw(struct s5p_mfc_dev *dev);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
index 367ef8e8dbf0..8937b0af7cb3 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
@@ -931,14 +931,14 @@ static int s5p_mfc_queue_setup(struct vb2_queue *vq,
                 psize[1] = ctx->chroma_size;
 
                 if (IS_MFCV6_PLUS(dev))
-                        alloc_devs[0] = ctx->dev->mem_dev_l;
+                        alloc_devs[0] = ctx->dev->mem_dev[BANK_L_CTX];
                 else
-                        alloc_devs[0] = ctx->dev->mem_dev_r;
-                alloc_devs[1] = ctx->dev->mem_dev_l;
+                        alloc_devs[0] = ctx->dev->mem_dev[BANK_R_CTX];
+                alloc_devs[1] = ctx->dev->mem_dev[BANK_L_CTX];
         } else if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE &&
                    ctx->state == MFCINST_INIT) {
                 psize[0] = ctx->dec_src_buf_size;
-                alloc_devs[0] = ctx->dev->mem_dev_l;
+                alloc_devs[0] = ctx->dev->mem_dev[BANK_L_CTX];
         } else {
                 mfc_err("This video node is dedicated to decoding. Decoding not initialized\n");
                 return -EINVAL;
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
index e39d9e06e299..2a5fd7c42cd5 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
@@ -1832,7 +1832,7 @@ static int s5p_mfc_queue_setup(struct vb2_queue *vq,
                 if (*buf_count > MFC_MAX_BUFFERS)
                         *buf_count = MFC_MAX_BUFFERS;
                 psize[0] = ctx->enc_dst_buf_size;
-                alloc_devs[0] = ctx->dev->mem_dev_l;
+                alloc_devs[0] = ctx->dev->mem_dev[BANK_L_CTX];
         } else if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
                 if (ctx->src_fmt)
                         *plane_count = ctx->src_fmt->num_planes;
@@ -1848,11 +1848,11 @@ static int s5p_mfc_queue_setup(struct vb2_queue *vq,
                 psize[1] = ctx->chroma_size;
 
                 if (IS_MFCV6_PLUS(dev)) {
-                        alloc_devs[0] = ctx->dev->mem_dev_l;
-                        alloc_devs[1] = ctx->dev->mem_dev_l;
+                        alloc_devs[0] = ctx->dev->mem_dev[BANK_L_CTX];
+                        alloc_devs[1] = ctx->dev->mem_dev[BANK_L_CTX];
                 } else {
-                        alloc_devs[0] = ctx->dev->mem_dev_r;
-                        alloc_devs[1] = ctx->dev->mem_dev_r;
+                        alloc_devs[0] = ctx->dev->mem_dev[BANK_R_CTX];
+                        alloc_devs[1] = ctx->dev->mem_dev[BANK_R_CTX];
                 }
         } else {
                 mfc_err("invalid queue type: %d\n", vq->type);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_iommu.h b/drivers/media/platform/s5p-mfc/s5p_mfc_iommu.h
index 6962132ae8fa..76667924ee2a 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_iommu.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_iommu.h
@@ -11,54 +11,13 @@
 #ifndef S5P_MFC_IOMMU_H_
 #define S5P_MFC_IOMMU_H_
 
-#define S5P_MFC_IOMMU_DMA_BASE  0x20000000lu
-#define S5P_MFC_IOMMU_DMA_SIZE  SZ_256M
-
-#if defined(CONFIG_EXYNOS_IOMMU) && defined(CONFIG_ARM_DMA_USE_IOMMU)
-
-#include <asm/dma-iommu.h>
+#if defined(CONFIG_EXYNOS_IOMMU)
 
 static inline bool exynos_is_iommu_available(struct device *dev)
 {
         return dev->archdata.iommu != NULL;
 }
 
-static inline void exynos_unconfigure_iommu(struct device *dev)
-{
-        struct dma_iommu_mapping *mapping = to_dma_iommu_mapping(dev);
-
-        arm_iommu_detach_device(dev);
-        arm_iommu_release_mapping(mapping);
-}
-
-static inline int exynos_configure_iommu(struct device *dev,
-                                         unsigned int base, unsigned int size)
-{
-        struct dma_iommu_mapping *mapping = NULL;
-        int ret;
-
-        /* Disable the default mapping created by device core */
-        if (to_dma_iommu_mapping(dev))
-                exynos_unconfigure_iommu(dev);
-
-        mapping = arm_iommu_create_mapping(dev->bus, base, size);
-        if (IS_ERR(mapping)) {
-                pr_warn("Failed to create IOMMU mapping for device %s\n",
-                        dev_name(dev));
-                return PTR_ERR(mapping);
-        }
-
-        ret = arm_iommu_attach_device(dev, mapping);
-        if (ret) {
-                pr_warn("Failed to attached device %s to IOMMU_mapping\n",
-                                dev_name(dev));
-                arm_iommu_release_mapping(mapping);
-                return ret;
-        }
-
-        return 0;
-}
-
 #else
 
 static inline bool exynos_is_iommu_available(struct device *dev)
@@ -66,14 +25,6 @@ static inline bool exynos_is_iommu_available(struct device *dev)
         return false;
 }
 
-static inline int exynos_configure_iommu(struct device *dev,
-                                         unsigned int base, unsigned int size)
-{
-        return -ENOSYS;
-}
-
-static inline void exynos_unconfigure_iommu(struct device *dev) { }
-
 #endif
 
 #endif /* S5P_MFC_IOMMU_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
index 99f65a92a6be..7f33cf23947f 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
@@ -37,38 +37,91 @@ void s5p_mfc_init_regs(struct s5p_mfc_dev *dev)
                 dev->mfc_regs = s5p_mfc_init_regs_v6_plus(dev);
 }
 
-int s5p_mfc_alloc_priv_buf(struct device *dev, dma_addr_t base,
-                                        struct s5p_mfc_priv_buf *b)
+int s5p_mfc_alloc_priv_buf(struct s5p_mfc_dev *dev, unsigned int mem_ctx,
+                           struct s5p_mfc_priv_buf *b)
 {
+        unsigned int bits = dev->mem_size >> PAGE_SHIFT;
+        unsigned int count = b->size >> PAGE_SHIFT;
+        unsigned int align = (SZ_64K >> PAGE_SHIFT) - 1;
+        unsigned int start, offset;
+
         mfc_debug(3, "Allocating priv: %zu\n", b->size);
 
-        b->virt = dma_alloc_coherent(dev, b->size, &b->dma, GFP_KERNEL);
+        if (dev->mem_virt) {
+                start = bitmap_find_next_zero_area(dev->mem_bitmap, bits, 0, count, align);
+                if (start > bits)
+                        goto no_mem;
 
-        if (!b->virt) {
-                mfc_err("Allocating private buffer of size %zu failed\n",
-                        b->size);
-                return -ENOMEM;
-        }
+                bitmap_set(dev->mem_bitmap, start, count);
+                offset = start << PAGE_SHIFT;
+                b->virt = dev->mem_virt + offset;
+                b->dma = dev->mem_base + offset;
+        } else {
+                struct device *mem_dev = dev->mem_dev[mem_ctx];
+                dma_addr_t base = dev->dma_base[mem_ctx];
 
-        if (b->dma < base) {
-                mfc_err("Invalid memory configuration - buffer (%pad) is below base memory address(%pad)\n",
-                        &b->dma, &base);
-                dma_free_coherent(dev, b->size, b->virt, b->dma);
-                return -ENOMEM;
+                b->ctx = mem_ctx;
+                b->virt = dma_alloc_coherent(mem_dev, b->size, &b->dma, GFP_KERNEL);
+                if (!b->virt)
+                        goto no_mem;
+                if (b->dma < base) {
+                        mfc_err("Invalid memory configuration - buffer (%pad) is below base memory address(%pad)\n",
+                                &b->dma, &base);
+                        dma_free_coherent(mem_dev, b->size, b->virt, b->dma);
+                        return -ENOMEM;
+                }
         }
 
         mfc_debug(3, "Allocated addr %p %pad\n", b->virt, &b->dma);
         return 0;
+no_mem:
+        mfc_err("Allocating private buffer of size %zu failed\n", b->size);
+        return -ENOMEM;
+}
+
+int s5p_mfc_alloc_generic_buf(struct s5p_mfc_dev *dev, unsigned int mem_ctx,
+                           struct s5p_mfc_priv_buf *b)
+{
+        struct device *mem_dev = dev->mem_dev[mem_ctx];
+
+        mfc_debug(3, "Allocating generic buf: %zu\n", b->size);
+
+        b->ctx = mem_ctx;
+        b->virt = dma_alloc_coherent(mem_dev, b->size, &b->dma, GFP_KERNEL);
+        if (!b->virt)
+                goto no_mem;
+
+        mfc_debug(3, "Allocated addr %p %pad\n", b->virt, &b->dma);
+        return 0;
+no_mem:
+        mfc_err("Allocating generic buffer of size %zu failed\n", b->size);
+        return -ENOMEM;
 }
 
-void s5p_mfc_release_priv_buf(struct device *dev,
-                                                struct s5p_mfc_priv_buf *b)
+void s5p_mfc_release_priv_buf(struct s5p_mfc_dev *dev,
+                              struct s5p_mfc_priv_buf *b)
 {
-        if (b->virt) {
-                dma_free_coherent(dev, b->size, b->virt, b->dma);
-                b->virt = NULL;
-                b->dma = 0;
-                b->size = 0;
+        if (dev->mem_virt) {
+                unsigned int start = (b->dma - dev->mem_base) >> PAGE_SHIFT;
+                unsigned int count = b->size >> PAGE_SHIFT;
+
+                bitmap_clear(dev->mem_bitmap, start, count);
+        } else {
+                struct device *mem_dev = dev->mem_dev[b->ctx];
+
+                dma_free_coherent(mem_dev, b->size, b->virt, b->dma);
         }
+        b->virt = NULL;
+        b->dma = 0;
+        b->size = 0;
 }
 
+void s5p_mfc_release_generic_buf(struct s5p_mfc_dev *dev,
+                              struct s5p_mfc_priv_buf *b)
+{
+        struct device *mem_dev = dev->mem_dev[b->ctx];
+        dma_free_coherent(mem_dev, b->size, b->virt, b->dma);
+        b->virt = NULL;
+        b->dma = 0;
+        b->size = 0;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
index b6ac417ab63e..16d553fcff08 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
@@ -315,10 +315,14 @@ struct s5p_mfc_hw_ops {
 
 void s5p_mfc_init_hw_ops(struct s5p_mfc_dev *dev);
 void s5p_mfc_init_regs(struct s5p_mfc_dev *dev);
-int s5p_mfc_alloc_priv_buf(struct device *dev, dma_addr_t base,
-                                        struct s5p_mfc_priv_buf *b);
-void s5p_mfc_release_priv_buf(struct device *dev,
-                                        struct s5p_mfc_priv_buf *b);
+int s5p_mfc_alloc_priv_buf(struct s5p_mfc_dev *dev, unsigned int mem_ctx,
+                           struct s5p_mfc_priv_buf *b);
+void s5p_mfc_release_priv_buf(struct s5p_mfc_dev *dev,
+                              struct s5p_mfc_priv_buf *b);
+int s5p_mfc_alloc_generic_buf(struct s5p_mfc_dev *dev, unsigned int mem_ctx,
+                           struct s5p_mfc_priv_buf *b);
+void s5p_mfc_release_generic_buf(struct s5p_mfc_dev *dev,
+                              struct s5p_mfc_priv_buf *b);
 
 
 #endif /* S5P_MFC_OPR_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
index f4301d5bbd32..b41ee608c171 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
@@ -30,8 +30,8 @@
 #include <linux/mm.h>
 #include <linux/sched.h>
 
-#define OFFSETA(x)              (((x) - dev->bank1) >> MFC_OFFSET_SHIFT)
-#define OFFSETB(x)              (((x) - dev->bank2) >> MFC_OFFSET_SHIFT)
+#define OFFSETA(x)              (((x) - dev->dma_base[BANK_L_CTX]) >> MFC_OFFSET_SHIFT)
+#define OFFSETB(x)              (((x) - dev->dma_base[BANK_R_CTX]) >> MFC_OFFSET_SHIFT)
 
 /* Allocate temporary buffers for decoding */
 static int s5p_mfc_alloc_dec_temp_buffers_v5(struct s5p_mfc_ctx *ctx)
@@ -41,7 +41,7 @@ static int s5p_mfc_alloc_dec_temp_buffers_v5(struct s5p_mfc_ctx *ctx)
         int ret;
 
         ctx->dsc.size = buf_size->dsc;
-        ret =  s5p_mfc_alloc_priv_buf(dev->mem_dev_l, dev->bank1, &ctx->dsc);
+        ret =  s5p_mfc_alloc_priv_buf(dev, BANK_L_CTX, &ctx->dsc);
         if (ret) {
                 mfc_err("Failed to allocate temporary buffer\n");
                 return ret;
@@ -57,7 +57,7 @@ static int s5p_mfc_alloc_dec_temp_buffers_v5(struct s5p_mfc_ctx *ctx)
 /* Release temporary buffers for decoding */
 static void s5p_mfc_release_dec_desc_buffer_v5(struct s5p_mfc_ctx *ctx)
 {
-        s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->dsc);
+        s5p_mfc_release_priv_buf(ctx->dev, &ctx->dsc);
 }
 
 /* Allocate codec buffers */
@@ -172,8 +172,7 @@ static int s5p_mfc_alloc_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
         /* Allocate only if memory from bank 1 is necessary */
         if (ctx->bank1.size > 0) {
 
-                ret = s5p_mfc_alloc_priv_buf(dev->mem_dev_l, dev->bank1,
-                                             &ctx->bank1);
+                ret = s5p_mfc_alloc_priv_buf(dev, BANK_L_CTX, &ctx->bank1);
                 if (ret) {
                         mfc_err("Failed to allocate Bank1 temporary buffer\n");
                         return ret;
@@ -182,11 +181,10 @@ static int s5p_mfc_alloc_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
         }
         /* Allocate only if memory from bank 2 is necessary */
         if (ctx->bank2.size > 0) {
-                ret = s5p_mfc_alloc_priv_buf(dev->mem_dev_r, dev->bank2,
-                                             &ctx->bank2);
+                ret = s5p_mfc_alloc_priv_buf(dev, BANK_R_CTX, &ctx->bank2);
                 if (ret) {
                         mfc_err("Failed to allocate Bank2 temporary buffer\n");
-                        s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->bank1);
+                        s5p_mfc_release_priv_buf(ctx->dev, &ctx->bank1);
                         return ret;
                 }
                 BUG_ON(ctx->bank2.dma & ((1 << MFC_BANK2_ALIGN_ORDER) - 1));
@@ -197,8 +195,8 @@ static int s5p_mfc_alloc_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
 /* Release buffers allocated for codec */
 static void s5p_mfc_release_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
 {
-        s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->bank1);
-        s5p_mfc_release_priv_buf(ctx->dev->mem_dev_r, &ctx->bank2);
+        s5p_mfc_release_priv_buf(ctx->dev, &ctx->bank1);
+        s5p_mfc_release_priv_buf(ctx->dev, &ctx->bank2);
 }
 
 /* Allocate memory for instance data buffer */
@@ -214,7 +212,7 @@ static int s5p_mfc_alloc_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
         else
                 ctx->ctx.size = buf_size->non_h264_ctx;
 
-        ret = s5p_mfc_alloc_priv_buf(dev->mem_dev_l, dev->bank1, &ctx->ctx);
+        ret = s5p_mfc_alloc_priv_buf(dev, BANK_L_CTX, &ctx->ctx);
         if (ret) {
                 mfc_err("Failed to allocate instance buffer\n");
                 return ret;
@@ -227,15 +225,15 @@ static int s5p_mfc_alloc_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
 
         /* Initialize shared memory */
         ctx->shm.size = buf_size->shm;
-        ret = s5p_mfc_alloc_priv_buf(dev->mem_dev_l, dev->bank1, &ctx->shm);
+        ret = s5p_mfc_alloc_priv_buf(dev, BANK_L_CTX, &ctx->shm);
         if (ret) {
                 mfc_err("Failed to allocate shared memory buffer\n");
-                s5p_mfc_release_priv_buf(dev->mem_dev_l, &ctx->ctx);
+                s5p_mfc_release_priv_buf(dev, &ctx->ctx);
                 return ret;
         }
 
         /* shared memory offset only keeps the offset from base (port a) */
-        ctx->shm.ofs = ctx->shm.dma - dev->bank1;
+        ctx->shm.ofs = ctx->shm.dma - dev->dma_base[BANK_L_CTX];
         BUG_ON(ctx->shm.ofs & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
 
         memset(ctx->shm.virt, 0, buf_size->shm);
@@ -246,8 +244,8 @@ static int s5p_mfc_alloc_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
 /* Release instance buffer */
 static void s5p_mfc_release_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
 {
-        s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->ctx);
-        s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->shm);
+        s5p_mfc_release_priv_buf(ctx->dev, &ctx->ctx);
+        s5p_mfc_release_priv_buf(ctx->dev, &ctx->shm);
 }
 
 static int s5p_mfc_alloc_dev_context_buffer_v5(struct s5p_mfc_dev *dev)
@@ -534,10 +532,10 @@ static void s5p_mfc_get_enc_frame_buffer_v5(struct s5p_mfc_ctx *ctx,
 {
         struct s5p_mfc_dev *dev = ctx->dev;
 
-        *y_addr = dev->bank2 + (mfc_read(dev, S5P_FIMV_ENCODED_Y_ADDR)
-                                                        << MFC_OFFSET_SHIFT);
-        *c_addr = dev->bank2 + (mfc_read(dev, S5P_FIMV_ENCODED_C_ADDR)
-                                                        << MFC_OFFSET_SHIFT);
+        *y_addr = dev->dma_base[BANK_R_CTX] +
+                  (mfc_read(dev, S5P_FIMV_ENCODED_Y_ADDR) << MFC_OFFSET_SHIFT);
+        *c_addr = dev->dma_base[BANK_R_CTX] +
+                  (mfc_read(dev, S5P_FIMV_ENCODED_C_ADDR) << MFC_OFFSET_SHIFT);
 }
 
 /* Set encoding ref & codec buffer */
@@ -1214,7 +1212,8 @@ static int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
         }
         if (list_empty(&ctx->src_queue)) {
                 /* send null frame */
-                s5p_mfc_set_enc_frame_buffer_v5(ctx, dev->bank2, dev->bank2);
+                s5p_mfc_set_enc_frame_buffer_v5(ctx, dev->dma_base[BANK_R_CTX],
+                                                dev->dma_base[BANK_R_CTX]);
                 src_mb = NULL;
         } else {
                 src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf,
@@ -1222,8 +1221,9 @@ static int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
                 src_mb->flags |= MFC_BUF_FLAG_USED;
                 if (src_mb->b->vb2_buf.planes[0].bytesused == 0) {
                         /* send null frame */
-                        s5p_mfc_set_enc_frame_buffer_v5(ctx, dev->bank2,
-                                                                dev->bank2);
+                        s5p_mfc_set_enc_frame_buffer_v5(ctx,
+                                                dev->dma_base[BANK_R_CTX],
+                                                dev->dma_base[BANK_R_CTX]);
                         ctx->state = MFCINST_FINISHING;
                 } else {
                         src_y_addr = vb2_dma_contig_plane_dma_addr(
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
index d6f207e859ab..85880e9106be 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
@@ -239,8 +239,7 @@ static int s5p_mfc_alloc_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
 
         /* Allocate only if memory from bank 1 is necessary */
         if (ctx->bank1.size > 0) {
-                ret = s5p_mfc_alloc_priv_buf(dev->mem_dev_l, dev->bank1,
-                                             &ctx->bank1);
+                ret = s5p_mfc_alloc_generic_buf(dev, BANK_L_CTX, &ctx->bank1);
                 if (ret) {
                         mfc_err("Failed to allocate Bank1 memory\n");
                         return ret;
@@ -253,7 +252,7 @@ static int s5p_mfc_alloc_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
 /* Release buffers allocated for codec */
 static void s5p_mfc_release_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
 {
-        s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->bank1);
+        s5p_mfc_release_generic_buf(ctx->dev, &ctx->bank1);
 }
 
 /* Allocate memory for instance data buffer */
@@ -292,7 +291,7 @@ static int s5p_mfc_alloc_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
                 break;
         }
 
-        ret = s5p_mfc_alloc_priv_buf(dev->mem_dev_l, dev->bank1, &ctx->ctx);
+        ret = s5p_mfc_alloc_priv_buf(dev, BANK_L_CTX, &ctx->ctx);
         if (ret) {
                 mfc_err("Failed to allocate instance buffer\n");
                 return ret;
@@ -309,7 +308,7 @@ static int s5p_mfc_alloc_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
 /* Release instance buffer */
 static void s5p_mfc_release_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
 {
-        s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->ctx);
+        s5p_mfc_release_priv_buf(ctx->dev, &ctx->ctx);
 }
 
 /* Allocate context buffers for SYS_INIT */
@@ -321,8 +320,7 @@ static int s5p_mfc_alloc_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
         mfc_debug_enter();
 
         dev->ctx_buf.size = buf_size->dev_ctx;
-        ret = s5p_mfc_alloc_priv_buf(dev->mem_dev_l, dev->bank1,
-                                     &dev->ctx_buf);
+        ret = s5p_mfc_alloc_priv_buf(dev, BANK_L_CTX, &dev->ctx_buf);
         if (ret) {
                 mfc_err("Failed to allocate device context buffer\n");
                 return ret;
@@ -339,7 +337,7 @@ static int s5p_mfc_alloc_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
 /* Release context buffers for SYS_INIT */
 static void s5p_mfc_release_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
 {
-        s5p_mfc_release_priv_buf(dev->mem_dev_l, &dev->ctx_buf);
+        s5p_mfc_release_priv_buf(dev, &dev->ctx_buf);
 }
 
 static int calc_plane(int width, int height)
@@ -497,7 +495,7 @@ static int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
                 }
         }
 
-        mfc_debug(2, "Buf1: %zu, buf_size1: %d (frames %d)\n",
+        mfc_debug(2, "Buf1: %zx, buf_size1: %d (frames %d)\n",
                         buf_addr1, buf_size1, ctx->total_dpb_count);
         if (buf_size1 < 0) {
                 mfc_debug(2, "Not enough memory has been allocated.\n");
diff --git a/drivers/media/platform/sh_vou.c b/drivers/media/platform/sh_vou.c
index ef2a519bcd4c..992d61a8b961 100644
--- a/drivers/media/platform/sh_vou.c
+++ b/drivers/media/platform/sh_vou.c
@@ -813,8 +813,8 @@ static u32 sh_vou_ntsc_mode(enum sh_vou_bus_fmt bus_fmt)
 {
         switch (bus_fmt) {
         default:
-                pr_warning("%s(): Invalid bus-format code %d, using default 8-bit\n",
-                           __func__, bus_fmt);
+                pr_warn("%s(): Invalid bus-format code %d, using default 8-bit\n",
+                        __func__, bus_fmt);
         case SH_VOU_BUS_8BIT:
                 return 1;
         case SH_VOU_BUS_16BIT:
diff --git a/drivers/media/platform/soc_camera/Kconfig b/drivers/media/platform/soc_camera/Kconfig
index 86d74788544f..f5979c12ad61 100644
--- a/drivers/media/platform/soc_camera/Kconfig
+++ b/drivers/media/platform/soc_camera/Kconfig
@@ -1,7 +1,6 @@
 config SOC_CAMERA
         tristate "SoC camera support"
         depends on VIDEO_V4L2 && HAS_DMA && I2C
-        select VIDEOBUF_GEN
         select VIDEOBUF2_CORE
         help
           SoC Camera is a common API to several cameras, not connecting
@@ -26,14 +25,3 @@ config VIDEO_SH_MOBILE_CEU
         select SOC_CAMERA_SCALE_CROP
         ---help---
           This is a v4l2 driver for the SuperH Mobile CEU Interface
-
-config VIDEO_ATMEL_ISI
-        tristate "ATMEL Image Sensor Interface (ISI) support"
-        depends on VIDEO_DEV && SOC_CAMERA
-        depends on ARCH_AT91 || COMPILE_TEST
-        depends on HAS_DMA
-        select VIDEOBUF2_DMA_CONTIG
-        ---help---
-          This module makes the ATMEL Image Sensor Interface available
-          as a v4l2 device.
-
diff --git a/drivers/media/platform/soc_camera/Makefile b/drivers/media/platform/soc_camera/Makefile
index 7633a0f2f66f..07a451e8b228 100644
--- a/drivers/media/platform/soc_camera/Makefile
+++ b/drivers/media/platform/soc_camera/Makefile
@@ -6,5 +6,4 @@ obj-$(CONFIG_SOC_CAMERA_SCALE_CROP)	+= soc_scale_crop.o
 obj-$(CONFIG_SOC_CAMERA_PLATFORM)       += soc_camera_platform.o
 
 # soc-camera host drivers have to be linked after camera drivers
-obj-$(CONFIG_VIDEO_ATMEL_ISI)           += atmel-isi.o
 obj-$(CONFIG_VIDEO_SH_MOBILE_CEU)       += sh_mobile_ceu_camera.o
diff --git a/drivers/media/platform/soc_camera/atmel-isi.c b/drivers/media/platform/soc_camera/atmel-isi.c
deleted file mode 100644
index 46de657c3e6d..000000000000
--- a/drivers/media/platform/soc_camera/atmel-isi.c
+++ /dev/null
@@ -1,1167 +0,0 @@
-/*
- * Copyright (c) 2011 Atmel Corporation
- * Josh Wu, <josh.wu@atmel.com>
- *
- * Based on previous work by Lars Haring, <lars.haring@atmel.com>
- * and Sedji Gaouaou
- * Based on the bttv driver for Bt848 with respective copyright holders
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#include <linux/clk.h>
-#include <linux/completion.h>
-#include <linux/delay.h>
-#include <linux/fs.h>
-#include <linux/init.h>
-#include <linux/interrupt.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/pm_runtime.h>
-#include <linux/slab.h>
-
-#include <media/soc_camera.h>
-#include <media/drv-intf/soc_mediabus.h>
-#include <media/v4l2-of.h>
-#include <media/videobuf2-dma-contig.h>
-
-#include "atmel-isi.h"
-
-#define MAX_BUFFER_NUM                  32
-#define MAX_SUPPORT_WIDTH               2048
-#define MAX_SUPPORT_HEIGHT              2048
-#define VID_LIMIT_BYTES                 (16 * 1024 * 1024)
-#define MIN_FRAME_RATE                  15
-#define FRAME_INTERVAL_MILLI_SEC        (1000 / MIN_FRAME_RATE)
-
-/* Frame buffer descriptor */
-struct fbd {
-        /* Physical address of the frame buffer */
-        u32 fb_address;
-        /* DMA Control Register(only in HISI2) */
-        u32 dma_ctrl;
-        /* Physical address of the next fbd */
-        u32 next_fbd_address;
-};
-
-static void set_dma_ctrl(struct fbd *fb_desc, u32 ctrl)
-{
-        fb_desc->dma_ctrl = ctrl;
-}
-
-struct isi_dma_desc {
-        struct list_head list;
-        struct fbd *p_fbd;
-        dma_addr_t fbd_phys;
-};
-
-/* Frame buffer data */
-struct frame_buffer {
-        struct vb2_v4l2_buffer vb;
-        struct isi_dma_desc *p_dma_desc;
-        struct list_head list;
-};
-
-struct atmel_isi {
-        /* Protects the access of variables shared with the ISR */
-        spinlock_t                      lock;
-        void __iomem                    *regs;
-
-        int                             sequence;
-
-        /* Allocate descriptors for dma buffer use */
-        struct fbd                      *p_fb_descriptors;
-        dma_addr_t                      fb_descriptors_phys;
-        struct                          list_head dma_desc_head;
-        struct isi_dma_desc             dma_desc[MAX_BUFFER_NUM];
-        bool                            enable_preview_path;
-
-        struct completion               complete;
-        /* ISI peripherial clock */
-        struct clk                      *pclk;
-        unsigned int                    irq;
-
-        struct isi_platform_data        pdata;
-        u16                             width_flags;    /* max 12 bits */
-
-        struct list_head                video_buffer_list;
-        struct frame_buffer             *active;
-
-        struct soc_camera_host          soc_host;
-};
-
-static void isi_writel(struct atmel_isi *isi, u32 reg, u32 val)
-{
-        writel(val, isi->regs + reg);
-}
-static u32 isi_readl(struct atmel_isi *isi, u32 reg)
-{
-        return readl(isi->regs + reg);
-}
-
-static u32 setup_cfg2_yuv_swap(struct atmel_isi *isi,
-                const struct soc_camera_format_xlate *xlate)
-{
-        if (xlate->host_fmt->fourcc == V4L2_PIX_FMT_YUYV) {
-                /* all convert to YUYV */
-                switch (xlate->code) {
-                case MEDIA_BUS_FMT_VYUY8_2X8:
-                        return ISI_CFG2_YCC_SWAP_MODE_3;
-                case MEDIA_BUS_FMT_UYVY8_2X8:
-                        return ISI_CFG2_YCC_SWAP_MODE_2;
-                case MEDIA_BUS_FMT_YVYU8_2X8:
-                        return ISI_CFG2_YCC_SWAP_MODE_1;
-                }
-        } else if (xlate->host_fmt->fourcc == V4L2_PIX_FMT_RGB565) {
-                /*
-                 * Preview path is enabled, it will convert UYVY to RGB format.
-                 * But if sensor output format is not UYVY, we need to set
-                 * YCC_SWAP_MODE to convert it as UYVY.
-                 */
-                switch (xlate->code) {
-                case MEDIA_BUS_FMT_VYUY8_2X8:
-                        return ISI_CFG2_YCC_SWAP_MODE_1;
-                case MEDIA_BUS_FMT_YUYV8_2X8:
-                        return ISI_CFG2_YCC_SWAP_MODE_2;
-                case MEDIA_BUS_FMT_YVYU8_2X8:
-                        return ISI_CFG2_YCC_SWAP_MODE_3;
-                }
-        }
-
-        /*
-         * By default, no swap for the codec path of Atmel ISI. So codec
-         * output is same as sensor's output.
-         * For instance, if sensor's output is YUYV, then codec outputs YUYV.
-         * And if sensor's output is UYVY, then codec outputs UYVY.
-         */
-        return ISI_CFG2_YCC_SWAP_DEFAULT;
-}
-
-static void configure_geometry(struct atmel_isi *isi, u32 width,
-                u32 height, const struct soc_camera_format_xlate *xlate)
-{
-        u32 cfg2, psize;
-        u32 fourcc = xlate->host_fmt->fourcc;
-
-        isi->enable_preview_path = fourcc == V4L2_PIX_FMT_RGB565 ||
-                                   fourcc == V4L2_PIX_FMT_RGB32;
-
-        /* According to sensor's output format to set cfg2 */
-        switch (xlate->code) {
-        default:
-        /* Grey */
-        case MEDIA_BUS_FMT_Y8_1X8:
-                cfg2 = ISI_CFG2_GRAYSCALE | ISI_CFG2_COL_SPACE_YCbCr;
-                break;
-        /* YUV */
-        case MEDIA_BUS_FMT_VYUY8_2X8:
-        case MEDIA_BUS_FMT_UYVY8_2X8:
-        case MEDIA_BUS_FMT_YVYU8_2X8:
-        case MEDIA_BUS_FMT_YUYV8_2X8:
-                cfg2 = ISI_CFG2_COL_SPACE_YCbCr |
-                                setup_cfg2_yuv_swap(isi, xlate);
-                break;
-        /* RGB, TODO */
-        }
-
-        isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
-        /* Set width */
-        cfg2 |= ((width - 1) << ISI_CFG2_IM_HSIZE_OFFSET) &
-                        ISI_CFG2_IM_HSIZE_MASK;
-        /* Set height */
-        cfg2 |= ((height - 1) << ISI_CFG2_IM_VSIZE_OFFSET)
-                        & ISI_CFG2_IM_VSIZE_MASK;
-        isi_writel(isi, ISI_CFG2, cfg2);
-
-        /* No down sampling, preview size equal to sensor output size */
-        psize = ((width - 1) << ISI_PSIZE_PREV_HSIZE_OFFSET) &
-                ISI_PSIZE_PREV_HSIZE_MASK;
-        psize |= ((height - 1) << ISI_PSIZE_PREV_VSIZE_OFFSET) &
-                ISI_PSIZE_PREV_VSIZE_MASK;
-        isi_writel(isi, ISI_PSIZE, psize);
-        isi_writel(isi, ISI_PDECF, ISI_PDECF_NO_SAMPLING);
-
-        return;
-}
-
-static bool is_supported(struct soc_camera_device *icd,
-                const u32 pixformat)
-{
-        switch (pixformat) {
-        /* YUV, including grey */
-        case V4L2_PIX_FMT_GREY:
-        case V4L2_PIX_FMT_YUYV:
-        case V4L2_PIX_FMT_UYVY:
-        case V4L2_PIX_FMT_YVYU:
-        case V4L2_PIX_FMT_VYUY:
-        /* RGB */
-        case V4L2_PIX_FMT_RGB565:
-                return true;
-        default:
-                return false;
-        }
-}
-
-static irqreturn_t atmel_isi_handle_streaming(struct atmel_isi *isi)
-{
-        if (isi->active) {
-                struct vb2_v4l2_buffer *vbuf = &isi->active->vb;
-                struct frame_buffer *buf = isi->active;
-
-                list_del_init(&buf->list);
-                vbuf->vb2_buf.timestamp = ktime_get_ns();
-                vbuf->sequence = isi->sequence++;
-                vb2_buffer_done(&vbuf->vb2_buf, VB2_BUF_STATE_DONE);
-        }
-
-        if (list_empty(&isi->video_buffer_list)) {
-                isi->active = NULL;
-        } else {
-                /* start next dma frame. */
-                isi->active = list_entry(isi->video_buffer_list.next,
-                                        struct frame_buffer, list);
-                if (!isi->enable_preview_path) {
-                        isi_writel(isi, ISI_DMA_C_DSCR,
-                                (u32)isi->active->p_dma_desc->fbd_phys);
-                        isi_writel(isi, ISI_DMA_C_CTRL,
-                                ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
-                        isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_C_CH);
-                } else {
-                        isi_writel(isi, ISI_DMA_P_DSCR,
-                                (u32)isi->active->p_dma_desc->fbd_phys);
-                        isi_writel(isi, ISI_DMA_P_CTRL,
-                                ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
-                        isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_P_CH);
-                }
-        }
-        return IRQ_HANDLED;
-}
-
-/* ISI interrupt service routine */
-static irqreturn_t isi_interrupt(int irq, void *dev_id)
-{
-        struct atmel_isi *isi = dev_id;
-        u32 status, mask, pending;
-        irqreturn_t ret = IRQ_NONE;
-
-        spin_lock(&isi->lock);
-
-        status = isi_readl(isi, ISI_STATUS);
-        mask = isi_readl(isi, ISI_INTMASK);
-        pending = status & mask;
-
-        if (pending & ISI_CTRL_SRST) {
-                complete(&isi->complete);
-                isi_writel(isi, ISI_INTDIS, ISI_CTRL_SRST);
-                ret = IRQ_HANDLED;
-        } else if (pending & ISI_CTRL_DIS) {
-                complete(&isi->complete);
-                isi_writel(isi, ISI_INTDIS, ISI_CTRL_DIS);
-                ret = IRQ_HANDLED;
-        } else {
-                if (likely(pending & ISI_SR_CXFR_DONE) ||
-                                likely(pending & ISI_SR_PXFR_DONE))
-                        ret = atmel_isi_handle_streaming(isi);
-        }
-
-        spin_unlock(&isi->lock);
-        return ret;
-}
-
-#define WAIT_ISI_RESET          1
-#define WAIT_ISI_DISABLE        0
-static int atmel_isi_wait_status(struct atmel_isi *isi, int wait_reset)
-{
-        unsigned long timeout;
-        /*
-         * The reset or disable will only succeed if we have a
-         * pixel clock from the camera.
-         */
-        init_completion(&isi->complete);
-
-        if (wait_reset) {
-                isi_writel(isi, ISI_INTEN, ISI_CTRL_SRST);
-                isi_writel(isi, ISI_CTRL, ISI_CTRL_SRST);
-        } else {
-                isi_writel(isi, ISI_INTEN, ISI_CTRL_DIS);
-                isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
-        }
-
-        timeout = wait_for_completion_timeout(&isi->complete,
-                        msecs_to_jiffies(500));
-        if (timeout == 0)
-                return -ETIMEDOUT;
-
-        return 0;
-}
-
-/* ------------------------------------------------------------------
-        Videobuf operations
-   ------------------------------------------------------------------*/
-static int queue_setup(struct vb2_queue *vq,
-                                unsigned int *nbuffers, unsigned int *nplanes,
-                                unsigned int sizes[], struct device *alloc_devs[])
-{
-        struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-        struct atmel_isi *isi = ici->priv;
-        unsigned long size;
-
-        size = icd->sizeimage;
-
-        if (!*nbuffers || *nbuffers > MAX_BUFFER_NUM)
-                *nbuffers = MAX_BUFFER_NUM;
-
-        if (size * *nbuffers > VID_LIMIT_BYTES)
-                *nbuffers = VID_LIMIT_BYTES / size;
-
-        *nplanes = 1;
-        sizes[0] = size;
-
-        isi->sequence = 0;
-        isi->active = NULL;
-
-        dev_dbg(icd->parent, "%s, count=%d, size=%ld\n", __func__,
-                *nbuffers, size);
-
-        return 0;
-}
-
-static int buffer_init(struct vb2_buffer *vb)
-{
-        struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
-        struct frame_buffer *buf = container_of(vbuf, struct frame_buffer, vb);
-
-        buf->p_dma_desc = NULL;
-        INIT_LIST_HEAD(&buf->list);
-
-        return 0;
-}
-
-static int buffer_prepare(struct vb2_buffer *vb)
-{
-        struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
-        struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
-        struct frame_buffer *buf = container_of(vbuf, struct frame_buffer, vb);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-        struct atmel_isi *isi = ici->priv;
-        unsigned long size;
-        struct isi_dma_desc *desc;
-
-        size = icd->sizeimage;
-
-        if (vb2_plane_size(vb, 0) < size) {
-                dev_err(icd->parent, "%s data will not fit into plane (%lu < %lu)\n",
-                                __func__, vb2_plane_size(vb, 0), size);
-                return -EINVAL;
-        }
-
-        vb2_set_plane_payload(vb, 0, size);
-
-        if (!buf->p_dma_desc) {
-                if (list_empty(&isi->dma_desc_head)) {
-                        dev_err(icd->parent, "Not enough dma descriptors.\n");
-                        return -EINVAL;
-                } else {
-                        /* Get an available descriptor */
-                        desc = list_entry(isi->dma_desc_head.next,
-                                                struct isi_dma_desc, list);
-                        /* Delete the descriptor since now it is used */
-                        list_del_init(&desc->list);
-
-                        /* Initialize the dma descriptor */
-                        desc->p_fbd->fb_address =
-                                        vb2_dma_contig_plane_dma_addr(vb, 0);
-                        desc->p_fbd->next_fbd_address = 0;
-                        set_dma_ctrl(desc->p_fbd, ISI_DMA_CTRL_WB);
-
-                        buf->p_dma_desc = desc;
-                }
-        }
-        return 0;
-}
-
-static void buffer_cleanup(struct vb2_buffer *vb)
-{
-        struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
-        struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-        struct atmel_isi *isi = ici->priv;
-        struct frame_buffer *buf = container_of(vbuf, struct frame_buffer, vb);
-
-        /* This descriptor is available now and we add to head list */
-        if (buf->p_dma_desc)
-                list_add(&buf->p_dma_desc->list, &isi->dma_desc_head);
-}
-
-static void start_dma(struct atmel_isi *isi, struct frame_buffer *buffer)
-{
-        u32 ctrl, cfg1;
-
-        cfg1 = isi_readl(isi, ISI_CFG1);
-        /* Enable irq: cxfr for the codec path, pxfr for the preview path */
-        isi_writel(isi, ISI_INTEN,
-                        ISI_SR_CXFR_DONE | ISI_SR_PXFR_DONE);
-
-        /* Check if already in a frame */
-        if (!isi->enable_preview_path) {
-                if (isi_readl(isi, ISI_STATUS) & ISI_CTRL_CDC) {
-                        dev_err(isi->soc_host.icd->parent, "Already in frame handling.\n");
-                        return;
-                }
-
-                isi_writel(isi, ISI_DMA_C_DSCR,
-                                (u32)buffer->p_dma_desc->fbd_phys);
-                isi_writel(isi, ISI_DMA_C_CTRL,
-                                ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
-                isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_C_CH);
-        } else {
-                isi_writel(isi, ISI_DMA_P_DSCR,
-                                (u32)buffer->p_dma_desc->fbd_phys);
-                isi_writel(isi, ISI_DMA_P_CTRL,
-                                ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
-                isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_P_CH);
-        }
-
-        cfg1 &= ~ISI_CFG1_FRATE_DIV_MASK;
-        /* Enable linked list */
-        cfg1 |= isi->pdata.frate | ISI_CFG1_DISCR;
-
-        /* Enable ISI */
-        ctrl = ISI_CTRL_EN;
-
-        if (!isi->enable_preview_path)
-                ctrl |= ISI_CTRL_CDC;
-
-        isi_writel(isi, ISI_CTRL, ctrl);
-        isi_writel(isi, ISI_CFG1, cfg1);
-}
-
-static void buffer_queue(struct vb2_buffer *vb)
-{
-        struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
-        struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-        struct atmel_isi *isi = ici->priv;
-        struct frame_buffer *buf = container_of(vbuf, struct frame_buffer, vb);
-        unsigned long flags = 0;
-
-        spin_lock_irqsave(&isi->lock, flags);
-        list_add_tail(&buf->list, &isi->video_buffer_list);
-
-        if (isi->active == NULL) {
-                isi->active = buf;
-                if (vb2_is_streaming(vb->vb2_queue))
-                        start_dma(isi, buf);
-        }
-        spin_unlock_irqrestore(&isi->lock, flags);
-}
-
-static int start_streaming(struct vb2_queue *vq, unsigned int count)
-{
-        struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-        struct atmel_isi *isi = ici->priv;
-        int ret;
-
-        pm_runtime_get_sync(ici->v4l2_dev.dev);
-
-        /* Reset ISI */
-        ret = atmel_isi_wait_status(isi, WAIT_ISI_RESET);
-        if (ret < 0) {
-                dev_err(icd->parent, "Reset ISI timed out\n");
-                pm_runtime_put(ici->v4l2_dev.dev);
-                return ret;
-        }
-        /* Disable all interrupts */
-        isi_writel(isi, ISI_INTDIS, (u32)~0UL);
-
-        configure_geometry(isi, icd->user_width, icd->user_height,
-                                icd->current_fmt);
-
-        spin_lock_irq(&isi->lock);
-        /* Clear any pending interrupt */
-        isi_readl(isi, ISI_STATUS);
-
-        if (count)
-                start_dma(isi, isi->active);
-        spin_unlock_irq(&isi->lock);
-
-        return 0;
-}
-
-/* abort streaming and wait for last buffer */
-static void stop_streaming(struct vb2_queue *vq)
-{
-        struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-        struct atmel_isi *isi = ici->priv;
-        struct frame_buffer *buf, *node;
-        int ret = 0;
-        unsigned long timeout;
-
-        spin_lock_irq(&isi->lock);
-        isi->active = NULL;
-        /* Release all active buffers */
-        list_for_each_entry_safe(buf, node, &isi->video_buffer_list, list) {
-                list_del_init(&buf->list);
-                vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
-        }
-        spin_unlock_irq(&isi->lock);
-
-        if (!isi->enable_preview_path) {
-                timeout = jiffies + FRAME_INTERVAL_MILLI_SEC * HZ;
-                /* Wait until the end of the current frame. */
-                while ((isi_readl(isi, ISI_STATUS) & ISI_CTRL_CDC) &&
-                                time_before(jiffies, timeout))
-                        msleep(1);
-
-                if (time_after(jiffies, timeout))
-                        dev_err(icd->parent,
-                                "Timeout waiting for finishing codec request\n");
-        }
-
-        /* Disable interrupts */
-        isi_writel(isi, ISI_INTDIS,
-                        ISI_SR_CXFR_DONE | ISI_SR_PXFR_DONE);
-
-        /* Disable ISI and wait for it is done */
-        ret = atmel_isi_wait_status(isi, WAIT_ISI_DISABLE);
-        if (ret < 0)
-                dev_err(icd->parent, "Disable ISI timed out\n");
-
-        pm_runtime_put(ici->v4l2_dev.dev);
-}
-
-static const struct vb2_ops isi_video_qops = {
-        .queue_setup            = queue_setup,
-        .buf_init               = buffer_init,
-        .buf_prepare            = buffer_prepare,
-        .buf_cleanup            = buffer_cleanup,
-        .buf_queue              = buffer_queue,
-        .start_streaming        = start_streaming,
-        .stop_streaming         = stop_streaming,
-        .wait_prepare           = vb2_ops_wait_prepare,
-        .wait_finish            = vb2_ops_wait_finish,
-};
-
-/* ------------------------------------------------------------------
-        SOC camera operations for the device
-   ------------------------------------------------------------------*/
-static int isi_camera_init_videobuf(struct vb2_queue *q,
-                                     struct soc_camera_device *icd)
-{
-        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-
-        q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-        q->io_modes = VB2_MMAP;
-        q->drv_priv = icd;
-        q->buf_struct_size = sizeof(struct frame_buffer);
-        q->ops = &isi_video_qops;
-        q->mem_ops = &vb2_dma_contig_memops;
-        q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
-        q->lock = &ici->host_lock;
-        q->dev = ici->v4l2_dev.dev;
-
-        return vb2_queue_init(q);
-}
-
-static int isi_camera_set_fmt(struct soc_camera_device *icd,
-                              struct v4l2_format *f)
-{
-        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        const struct soc_camera_format_xlate *xlate;
-        struct v4l2_pix_format *pix = &f->fmt.pix;
-        struct v4l2_subdev_format format = {
-                .which = V4L2_SUBDEV_FORMAT_ACTIVE,
-        };
-        struct v4l2_mbus_framefmt *mf = &format.format;
-        int ret;
-
-        /* check with atmel-isi support format, if not support use YUYV */
-        if (!is_supported(icd, pix->pixelformat))
-                pix->pixelformat = V4L2_PIX_FMT_YUYV;
-
-        xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
-        if (!xlate) {
-                dev_warn(icd->parent, "Format %x not found\n",
-                         pix->pixelformat);
-                return -EINVAL;
-        }
-
-        dev_dbg(icd->parent, "Plan to set format %dx%d\n",
-                        pix->width, pix->height);
-
-        mf->width       = pix->width;
-        mf->height      = pix->height;
-        mf->field       = pix->field;
-        mf->colorspace  = pix->colorspace;
-        mf->code        = xlate->code;
-
-        ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &format);
-        if (ret < 0)
-                return ret;
-
-        if (mf->code != xlate->code)
-                return -EINVAL;
-
-        pix->width              = mf->width;
-        pix->height             = mf->height;
-        pix->field              = mf->field;
-        pix->colorspace         = mf->colorspace;
-        icd->current_fmt        = xlate;
-
-        dev_dbg(icd->parent, "Finally set format %dx%d\n",
-                pix->width, pix->height);
-
-        return ret;
-}
-
-static int isi_camera_try_fmt(struct soc_camera_device *icd,
-                              struct v4l2_format *f)
-{
-        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        const struct soc_camera_format_xlate *xlate;
-        struct v4l2_pix_format *pix = &f->fmt.pix;
-        struct v4l2_subdev_pad_config pad_cfg;
-        struct v4l2_subdev_format format = {
-                .which = V4L2_SUBDEV_FORMAT_TRY,
-        };
-        struct v4l2_mbus_framefmt *mf = &format.format;
-        u32 pixfmt = pix->pixelformat;
-        int ret;
-
-        /* check with atmel-isi support format, if not support use YUYV */
-        if (!is_supported(icd, pix->pixelformat))
-                pix->pixelformat = V4L2_PIX_FMT_YUYV;
-
-        xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
-        if (pixfmt && !xlate) {
-                dev_warn(icd->parent, "Format %x not found\n", pixfmt);
-                return -EINVAL;
-        }
-
-        /* limit to Atmel ISI hardware capabilities */
-        if (pix->height > MAX_SUPPORT_HEIGHT)
-                pix->height = MAX_SUPPORT_HEIGHT;
-        if (pix->width > MAX_SUPPORT_WIDTH)
-                pix->width = MAX_SUPPORT_WIDTH;
-
-        /* limit to sensor capabilities */
-        mf->width       = pix->width;
-        mf->height      = pix->height;
-        mf->field       = pix->field;
-        mf->colorspace  = pix->colorspace;
-        mf->code        = xlate->code;
-
-        ret = v4l2_subdev_call(sd, pad, set_fmt, &pad_cfg, &format);
-        if (ret < 0)
-                return ret;
-
-        pix->width      = mf->width;
-        pix->height     = mf->height;
-        pix->colorspace = mf->colorspace;
-
-        switch (mf->field) {
-        case V4L2_FIELD_ANY:
-                pix->field = V4L2_FIELD_NONE;
-                break;
-        case V4L2_FIELD_NONE:
-                break;
-        default:
-                dev_err(icd->parent, "Field type %d unsupported.\n",
-                        mf->field);
-                ret = -EINVAL;
-        }
-
-        return ret;
-}
-
-static const struct soc_mbus_pixelfmt isi_camera_formats[] = {
-        {
-                .fourcc                 = V4L2_PIX_FMT_YUYV,
-                .name                   = "Packed YUV422 16 bit",
-                .bits_per_sample        = 8,
-                .packing                = SOC_MBUS_PACKING_2X8_PADHI,
-                .order                  = SOC_MBUS_ORDER_LE,
-                .layout                 = SOC_MBUS_LAYOUT_PACKED,
-        },
-        {
-                .fourcc                 = V4L2_PIX_FMT_RGB565,
-                .name                   = "RGB565",
-                .bits_per_sample        = 8,
-                .packing                = SOC_MBUS_PACKING_2X8_PADHI,
-                .order                  = SOC_MBUS_ORDER_LE,
-                .layout                 = SOC_MBUS_LAYOUT_PACKED,
-        },
-};
-
-/* This will be corrected as we get more formats */
-static bool isi_camera_packing_supported(const struct soc_mbus_pixelfmt *fmt)
-{
-        return  fmt->packing == SOC_MBUS_PACKING_NONE ||
-                (fmt->bits_per_sample == 8 &&
-                 fmt->packing == SOC_MBUS_PACKING_2X8_PADHI) ||
-                (fmt->bits_per_sample > 8 &&
-                 fmt->packing == SOC_MBUS_PACKING_EXTEND16);
-}
-
-#define ISI_BUS_PARAM (V4L2_MBUS_MASTER |       \
-                V4L2_MBUS_HSYNC_ACTIVE_HIGH |   \
-                V4L2_MBUS_HSYNC_ACTIVE_LOW |    \
-                V4L2_MBUS_VSYNC_ACTIVE_HIGH |   \
-                V4L2_MBUS_VSYNC_ACTIVE_LOW |    \
-                V4L2_MBUS_PCLK_SAMPLE_RISING |  \
-                V4L2_MBUS_PCLK_SAMPLE_FALLING | \
-                V4L2_MBUS_DATA_ACTIVE_HIGH)
-
-static int isi_camera_try_bus_param(struct soc_camera_device *icd,
-                                    unsigned char buswidth)
-{
-        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-        struct atmel_isi *isi = ici->priv;
-        struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
-        unsigned long common_flags;
-        int ret;
-
-        ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
-        if (!ret) {
-                common_flags = soc_mbus_config_compatible(&cfg,
-                                                          ISI_BUS_PARAM);
-                if (!common_flags) {
-                        dev_warn(icd->parent,
-                                 "Flags incompatible: camera 0x%x, host 0x%x\n",
-                                 cfg.flags, ISI_BUS_PARAM);
-                        return -EINVAL;
-                }
-        } else if (ret != -ENOIOCTLCMD) {
-                return ret;
-        }
-
-        if ((1 << (buswidth - 1)) & isi->width_flags)
-                return 0;
-        return -EINVAL;
-}
-
-
-static int isi_camera_get_formats(struct soc_camera_device *icd,
-                                  unsigned int idx,
-                                  struct soc_camera_format_xlate *xlate)
-{
-        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        int formats = 0, ret, i, n;
-        /* sensor format */
-        struct v4l2_subdev_mbus_code_enum code = {
-                .which = V4L2_SUBDEV_FORMAT_ACTIVE,
-                .index = idx,
-        };
-        /* soc camera host format */
-        const struct soc_mbus_pixelfmt *fmt;
-
-        ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
-        if (ret < 0)
-                /* No more formats */
-                return 0;
-
-        fmt = soc_mbus_get_fmtdesc(code.code);
-        if (!fmt) {
-                dev_err(icd->parent,
-                        "Invalid format code #%u: %d\n", idx, code.code);
-                return 0;
-        }
-
-        /* This also checks support for the requested bits-per-sample */
-        ret = isi_camera_try_bus_param(icd, fmt->bits_per_sample);
-        if (ret < 0) {
-                dev_err(icd->parent,
-                        "Fail to try the bus parameters.\n");
-                return 0;
-        }
-
-        switch (code.code) {
-        case MEDIA_BUS_FMT_UYVY8_2X8:
-        case MEDIA_BUS_FMT_VYUY8_2X8:
-        case MEDIA_BUS_FMT_YUYV8_2X8:
-        case MEDIA_BUS_FMT_YVYU8_2X8:
-                n = ARRAY_SIZE(isi_camera_formats);
-                formats += n;
-                for (i = 0; xlate && i < n; i++, xlate++) {
-                        xlate->host_fmt = &isi_camera_formats[i];
-                        xlate->code     = code.code;
-                        dev_dbg(icd->parent, "Providing format %s using code %d\n",
-                                xlate->host_fmt->name, xlate->code);
-                }
-                break;
-        default:
-                if (!isi_camera_packing_supported(fmt))
-                        return 0;
-                if (xlate)
-                        dev_dbg(icd->parent,
-                                "Providing format %s in pass-through mode\n",
-                                fmt->name);
-        }
-
-        /* Generic pass-through */
-        formats++;
-        if (xlate) {
-                xlate->host_fmt = fmt;
-                xlate->code     = code.code;
-                xlate++;
-        }
-
-        return formats;
-}
-
-static int isi_camera_add_device(struct soc_camera_device *icd)
-{
-        dev_dbg(icd->parent, "Atmel ISI Camera driver attached to camera %d\n",
-                 icd->devnum);
-
-        return 0;
-}
-
-static void isi_camera_remove_device(struct soc_camera_device *icd)
-{
-        dev_dbg(icd->parent, "Atmel ISI Camera driver detached from camera %d\n",
-                 icd->devnum);
-}
-
-static unsigned int isi_camera_poll(struct file *file, poll_table *pt)
-{
-        struct soc_camera_device *icd = file->private_data;
-
-        return vb2_poll(&icd->vb2_vidq, file, pt);
-}
-
-static int isi_camera_querycap(struct soc_camera_host *ici,
-                               struct v4l2_capability *cap)
-{
-        strcpy(cap->driver, "atmel-isi");
-        strcpy(cap->card, "Atmel Image Sensor Interface");
-        cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
-        cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
-
-        return 0;
-}
-
-static int isi_camera_set_bus_param(struct soc_camera_device *icd)
-{
-        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-        struct atmel_isi *isi = ici->priv;
-        struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
-        unsigned long common_flags;
-        int ret;
-        u32 cfg1 = 0;
-
-        ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
-        if (!ret) {
-                common_flags = soc_mbus_config_compatible(&cfg,
-                                                          ISI_BUS_PARAM);
-                if (!common_flags) {
-                        dev_warn(icd->parent,
-                                 "Flags incompatible: camera 0x%x, host 0x%x\n",
-                                 cfg.flags, ISI_BUS_PARAM);
-                        return -EINVAL;
-                }
-        } else if (ret != -ENOIOCTLCMD) {
-                return ret;
-        } else {
-                common_flags = ISI_BUS_PARAM;
-        }
-        dev_dbg(icd->parent, "Flags cam: 0x%x host: 0x%x common: 0x%lx\n",
-                cfg.flags, ISI_BUS_PARAM, common_flags);
-
-        /* Make choises, based on platform preferences */
-        if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
-            (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
-                if (isi->pdata.hsync_act_low)
-                        common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
-                else
-                        common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
-        }
-
-        if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
-            (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
-                if (isi->pdata.vsync_act_low)
-                        common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
-                else
-                        common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
-        }
-
-        if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
-            (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
-                if (isi->pdata.pclk_act_falling)
-                        common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
-                else
-                        common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
-        }
-
-        cfg.flags = common_flags;
-        ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
-        if (ret < 0 && ret != -ENOIOCTLCMD) {
-                dev_dbg(icd->parent, "camera s_mbus_config(0x%lx) returned %d\n",
-                        common_flags, ret);
-                return ret;
-        }
-
-        /* set bus param for ISI */
-        if (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
-                cfg1 |= ISI_CFG1_HSYNC_POL_ACTIVE_LOW;
-        if (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
-                cfg1 |= ISI_CFG1_VSYNC_POL_ACTIVE_LOW;
-        if (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
-                cfg1 |= ISI_CFG1_PIXCLK_POL_ACTIVE_FALLING;
-
-        dev_dbg(icd->parent, "vsync active %s, hsync active %s, sampling on pix clock %s edge\n",
-                common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW ? "low" : "high",
-                common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW ? "low" : "high",
-                common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING ? "falling" : "rising");
-
-        if (isi->pdata.has_emb_sync)
-                cfg1 |= ISI_CFG1_EMB_SYNC;
-        if (isi->pdata.full_mode)
-                cfg1 |= ISI_CFG1_FULL_MODE;
-
-        cfg1 |= ISI_CFG1_THMASK_BEATS_16;
-
-        /* Enable PM and peripheral clock before operate isi registers */
-        pm_runtime_get_sync(ici->v4l2_dev.dev);
-
-        isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
-        isi_writel(isi, ISI_CFG1, cfg1);
-
-        pm_runtime_put(ici->v4l2_dev.dev);
-
-        return 0;
-}
-
-static struct soc_camera_host_ops isi_soc_camera_host_ops = {
-        .owner          = THIS_MODULE,
-        .add            = isi_camera_add_device,
-        .remove         = isi_camera_remove_device,
-        .set_fmt        = isi_camera_set_fmt,
-        .try_fmt        = isi_camera_try_fmt,
-        .get_formats    = isi_camera_get_formats,
-        .init_videobuf2 = isi_camera_init_videobuf,
-        .poll           = isi_camera_poll,
-        .querycap       = isi_camera_querycap,
-        .set_bus_param  = isi_camera_set_bus_param,
-};
-
-/* -----------------------------------------------------------------------*/
-static int atmel_isi_remove(struct platform_device *pdev)
-{
-        struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
-        struct atmel_isi *isi = container_of(soc_host,
-                                        struct atmel_isi, soc_host);
-
-        soc_camera_host_unregister(soc_host);
-        dma_free_coherent(&pdev->dev,
-                        sizeof(struct fbd) * MAX_BUFFER_NUM,
-                        isi->p_fb_descriptors,
-                        isi->fb_descriptors_phys);
-        pm_runtime_disable(&pdev->dev);
-
-        return 0;
-}
-
-static int atmel_isi_parse_dt(struct atmel_isi *isi,
-                        struct platform_device *pdev)
-{
-        struct device_node *np= pdev->dev.of_node;
-        struct v4l2_of_endpoint ep;
-        int err;
-
-        /* Default settings for ISI */
-        isi->pdata.full_mode = 1;
-        isi->pdata.frate = ISI_CFG1_FRATE_CAPTURE_ALL;
-
-        np = of_graph_get_next_endpoint(np, NULL);
-        if (!np) {
-                dev_err(&pdev->dev, "Could not find the endpoint\n");
-                return -EINVAL;
-        }
-
-        err = v4l2_of_parse_endpoint(np, &ep);
-        of_node_put(np);
-        if (err) {
-                dev_err(&pdev->dev, "Could not parse the endpoint\n");
-                return err;
-        }
-
-        switch (ep.bus.parallel.bus_width) {
-        case 8:
-                isi->pdata.data_width_flags = ISI_DATAWIDTH_8;
-                break;
-        case 10:
-                isi->pdata.data_width_flags =
-                                ISI_DATAWIDTH_8 | ISI_DATAWIDTH_10;
-                break;
-        default:
-                dev_err(&pdev->dev, "Unsupported bus width: %d\n",
-                                ep.bus.parallel.bus_width);
-                return -EINVAL;
-        }
-
-        if (ep.bus.parallel.flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
-                isi->pdata.hsync_act_low = true;
-        if (ep.bus.parallel.flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
-                isi->pdata.vsync_act_low = true;
-        if (ep.bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
-                isi->pdata.pclk_act_falling = true;
-
-        if (ep.bus_type == V4L2_MBUS_BT656)
-                isi->pdata.has_emb_sync = true;
-
-        return 0;
-}
-
-static int atmel_isi_probe(struct platform_device *pdev)
-{
-        int irq;
-        struct atmel_isi *isi;
-        struct resource *regs;
-        int ret, i;
-        struct soc_camera_host *soc_host;
-
-        isi = devm_kzalloc(&pdev->dev, sizeof(struct atmel_isi), GFP_KERNEL);
-        if (!isi) {
-                dev_err(&pdev->dev, "Can't allocate interface!\n");
-                return -ENOMEM;
-        }
-
-        isi->pclk = devm_clk_get(&pdev->dev, "isi_clk");
-        if (IS_ERR(isi->pclk))
-                return PTR_ERR(isi->pclk);
-
-        ret = atmel_isi_parse_dt(isi, pdev);
-        if (ret)
-                return ret;
-
-        isi->active = NULL;
-        spin_lock_init(&isi->lock);
-        INIT_LIST_HEAD(&isi->video_buffer_list);
-        INIT_LIST_HEAD(&isi->dma_desc_head);
-
-        isi->p_fb_descriptors = dma_alloc_coherent(&pdev->dev,
-                                sizeof(struct fbd) * MAX_BUFFER_NUM,
-                                &isi->fb_descriptors_phys,
-                                GFP_KERNEL);
-        if (!isi->p_fb_descriptors) {
-                dev_err(&pdev->dev, "Can't allocate descriptors!\n");
-                return -ENOMEM;
-        }
-
-        for (i = 0; i < MAX_BUFFER_NUM; i++) {
-                isi->dma_desc[i].p_fbd = isi->p_fb_descriptors + i;
-                isi->dma_desc[i].fbd_phys = isi->fb_descriptors_phys +
-                                        i * sizeof(struct fbd);
-                list_add(&isi->dma_desc[i].list, &isi->dma_desc_head);
-        }
-
-        regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-        isi->regs = devm_ioremap_resource(&pdev->dev, regs);
-        if (IS_ERR(isi->regs)) {
-                ret = PTR_ERR(isi->regs);
-                goto err_ioremap;
-        }
-
-        if (isi->pdata.data_width_flags & ISI_DATAWIDTH_8)
-                isi->width_flags = 1 << 7;
-        if (isi->pdata.data_width_flags & ISI_DATAWIDTH_10)
-                isi->width_flags |= 1 << 9;
-
-        irq = platform_get_irq(pdev, 0);
-        if (irq < 0) {
-                ret = irq;
-                goto err_req_irq;
-        }
-
-        ret = devm_request_irq(&pdev->dev, irq, isi_interrupt, 0, "isi", isi);
-        if (ret) {
-                dev_err(&pdev->dev, "Unable to request irq %d\n", irq);
-                goto err_req_irq;
-        }
-        isi->irq = irq;
-
-        soc_host                = &isi->soc_host;
-        soc_host->drv_name      = "isi-camera";
-        soc_host->ops           = &isi_soc_camera_host_ops;
-        soc_host->priv          = isi;
-        soc_host->v4l2_dev.dev  = &pdev->dev;
-        soc_host->nr            = pdev->id;
-
-        pm_suspend_ignore_children(&pdev->dev, true);
-        pm_runtime_enable(&pdev->dev);
-
-        ret = soc_camera_host_register(soc_host);
-        if (ret) {
-                dev_err(&pdev->dev, "Unable to register soc camera host\n");
-                goto err_register_soc_camera_host;
-        }
-        return 0;
-
-err_register_soc_camera_host:
-        pm_runtime_disable(&pdev->dev);
-err_req_irq:
-err_ioremap:
-        dma_free_coherent(&pdev->dev,
-                        sizeof(struct fbd) * MAX_BUFFER_NUM,
-                        isi->p_fb_descriptors,
-                        isi->fb_descriptors_phys);
-
-        return ret;
-}
-
-#ifdef CONFIG_PM
-static int atmel_isi_runtime_suspend(struct device *dev)
-{
-        struct soc_camera_host *soc_host = to_soc_camera_host(dev);
-        struct atmel_isi *isi = container_of(soc_host,
-                                        struct atmel_isi, soc_host);
-
-        clk_disable_unprepare(isi->pclk);
-
-        return 0;
-}
-static int atmel_isi_runtime_resume(struct device *dev)
-{
-        struct soc_camera_host *soc_host = to_soc_camera_host(dev);
-        struct atmel_isi *isi = container_of(soc_host,
-                                        struct atmel_isi, soc_host);
-
-        return clk_prepare_enable(isi->pclk);
-}
-#endif /* CONFIG_PM */
-
-static const struct dev_pm_ops atmel_isi_dev_pm_ops = {
-        SET_RUNTIME_PM_OPS(atmel_isi_runtime_suspend,
-                                atmel_isi_runtime_resume, NULL)
-};
-
-static const struct of_device_id atmel_isi_of_match[] = {
-        { .compatible = "atmel,at91sam9g45-isi" },
-        { }
-};
-MODULE_DEVICE_TABLE(of, atmel_isi_of_match);
-
-static struct platform_driver atmel_isi_driver = {
-        .remove         = atmel_isi_remove,
-        .driver         = {
-                .name = "atmel_isi",
-                .of_match_table = of_match_ptr(atmel_isi_of_match),
-                .pm     = &atmel_isi_dev_pm_ops,
-        },
-};
-
-module_platform_driver_probe(atmel_isi_driver, atmel_isi_probe);
-
-MODULE_AUTHOR("Josh Wu <josh.wu@atmel.com>");
-MODULE_DESCRIPTION("The V4L2 driver for Atmel Linux");
-MODULE_LICENSE("GPL");
-MODULE_SUPPORTED_DEVICE("video");
diff --git a/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c b/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
index a15bfb5aea47..96dc01750bc0 100644
--- a/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
+++ b/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
@@ -1801,18 +1801,7 @@ static struct platform_driver sh_mobile_ceu_driver = {
         .remove         = sh_mobile_ceu_remove,
 };
 
-static int __init sh_mobile_ceu_init(void)
-{
-        return platform_driver_register(&sh_mobile_ceu_driver);
-}
-
-static void __exit sh_mobile_ceu_exit(void)
-{
-        platform_driver_unregister(&sh_mobile_ceu_driver);
-}
-
-module_init(sh_mobile_ceu_init);
-module_exit(sh_mobile_ceu_exit);
+module_platform_driver(sh_mobile_ceu_driver);
 
 MODULE_DESCRIPTION("SuperH Mobile CEU driver");
 MODULE_AUTHOR("Magnus Damm");
diff --git a/drivers/media/platform/soc_camera/soc_camera.c b/drivers/media/platform/soc_camera/soc_camera.c
index edd1c1de4e33..3c9421f4d8e3 100644
--- a/drivers/media/platform/soc_camera/soc_camera.c
+++ b/drivers/media/platform/soc_camera/soc_camera.c
@@ -37,18 +37,12 @@
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-dev.h>
 #include <media/v4l2-of.h>
-#include <media/videobuf-core.h>
 #include <media/videobuf2-v4l2.h>
 
 /* Default to VGA resolution */
 #define DEFAULT_WIDTH   640
 #define DEFAULT_HEIGHT  480
 
-#define is_streaming(ici, icd)                          \
-        (((ici)->ops->init_videobuf) ?                  \
-         (icd)->vb_vidq.streaming :                     \
-         vb2_is_streaming(&(icd)->vb2_vidq))
-
 #define MAP_MAX_NUM 32
 static DECLARE_BITMAP(device_map, MAP_MAX_NUM);
 static LIST_HEAD(hosts);
@@ -367,23 +361,13 @@ static int soc_camera_reqbufs(struct file *file, void *priv,
 {
         int ret;
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
         WARN_ON(priv != file->private_data);
 
         if (icd->streamer && icd->streamer != file)
                 return -EBUSY;
 
-        if (ici->ops->init_videobuf) {
-                ret = videobuf_reqbufs(&icd->vb_vidq, p);
-                if (ret < 0)
-                        return ret;
-
-                ret = ici->ops->reqbufs(icd, p);
-        } else {
-                ret = vb2_reqbufs(&icd->vb2_vidq, p);
-        }
-
+        ret = vb2_reqbufs(&icd->vb2_vidq, p);
         if (!ret)
                 icd->streamer = p->count ? file : NULL;
         return ret;
@@ -393,61 +377,44 @@ static int soc_camera_querybuf(struct file *file, void *priv,
                                struct v4l2_buffer *p)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
         WARN_ON(priv != file->private_data);
 
-        if (ici->ops->init_videobuf)
-                return videobuf_querybuf(&icd->vb_vidq, p);
-        else
-                return vb2_querybuf(&icd->vb2_vidq, p);
+        return vb2_querybuf(&icd->vb2_vidq, p);
 }
 
 static int soc_camera_qbuf(struct file *file, void *priv,
                            struct v4l2_buffer *p)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
         WARN_ON(priv != file->private_data);
 
         if (icd->streamer != file)
                 return -EBUSY;
 
-        if (ici->ops->init_videobuf)
-                return videobuf_qbuf(&icd->vb_vidq, p);
-        else
-                return vb2_qbuf(&icd->vb2_vidq, p);
+        return vb2_qbuf(&icd->vb2_vidq, p);
 }
 
 static int soc_camera_dqbuf(struct file *file, void *priv,
                             struct v4l2_buffer *p)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
         WARN_ON(priv != file->private_data);
 
         if (icd->streamer != file)
                 return -EBUSY;
 
-        if (ici->ops->init_videobuf)
-                return videobuf_dqbuf(&icd->vb_vidq, p, file->f_flags & O_NONBLOCK);
-        else
-                return vb2_dqbuf(&icd->vb2_vidq, p, file->f_flags & O_NONBLOCK);
+        return vb2_dqbuf(&icd->vb2_vidq, p, file->f_flags & O_NONBLOCK);
 }
 
 static int soc_camera_create_bufs(struct file *file, void *priv,
                             struct v4l2_create_buffers *create)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         int ret;
 
-        /* videobuf2 only */
-        if (ici->ops->init_videobuf)
-                return -ENOTTY;
-
         if (icd->streamer && icd->streamer != file)
                 return -EBUSY;
 
@@ -461,24 +428,14 @@ static int soc_camera_prepare_buf(struct file *file, void *priv,
                                   struct v4l2_buffer *b)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
-        /* videobuf2 only */
-        if (ici->ops->init_videobuf)
-                return -EINVAL;
-        else
-                return vb2_prepare_buf(&icd->vb2_vidq, b);
+        return vb2_prepare_buf(&icd->vb2_vidq, b);
 }
 
 static int soc_camera_expbuf(struct file *file, void *priv,
                              struct v4l2_exportbuffer *p)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-
-        /* videobuf2 only */
-        if (ici->ops->init_videobuf)
-                return -ENOTTY;
 
         if (icd->streamer && icd->streamer != file)
                 return -EBUSY;
@@ -602,8 +559,6 @@ static int soc_camera_set_fmt(struct soc_camera_device *icd,
         icd->sizeimage          = pix->sizeimage;
         icd->colorspace         = pix->colorspace;
         icd->field              = pix->field;
-        if (ici->ops->init_videobuf)
-                icd->vb_vidq.field = pix->field;
 
         dev_dbg(icd->pdev, "set width: %d height: %d\n",
                 icd->user_width, icd->user_height);
@@ -745,13 +700,9 @@ static int soc_camera_open(struct file *file)
                 if (ret < 0)
                         goto esfmt;
 
-                if (ici->ops->init_videobuf) {
-                        ici->ops->init_videobuf(&icd->vb_vidq, icd);
-                } else {
-                        ret = ici->ops->init_videobuf2(&icd->vb2_vidq, icd);
-                        if (ret < 0)
-                                goto einitvb;
-                }
+                ret = ici->ops->init_videobuf2(&icd->vb2_vidq, icd);
+                if (ret < 0)
+                        goto einitvb;
                 v4l2_ctrl_handler_setup(&icd->ctrl_handler);
         }
         mutex_unlock(&ici->host_lock);
@@ -842,10 +793,7 @@ static int soc_camera_mmap(struct file *file, struct vm_area_struct *vma)
 
         if (mutex_lock_interruptible(&ici->host_lock))
                 return -ERESTARTSYS;
-        if (ici->ops->init_videobuf)
-                err = videobuf_mmap_mapper(&icd->vb_vidq, vma);
-        else
-                err = vb2_mmap(&icd->vb2_vidq, vma);
+        err = vb2_mmap(&icd->vb2_vidq, vma);
         mutex_unlock(&ici->host_lock);
 
         dev_dbg(icd->pdev, "vma start=0x%08lx, size=%ld, ret=%d\n",
@@ -866,10 +814,7 @@ static unsigned int soc_camera_poll(struct file *file, poll_table *pt)
                 return POLLERR;
 
         mutex_lock(&ici->host_lock);
-        if (ici->ops->init_videobuf && list_empty(&icd->vb_vidq.stream))
-                dev_err(icd->pdev, "Trying to poll with no queued buffers!\n");
-        else
-                res = ici->ops->poll(file, pt);
+        res = ici->ops->poll(file, pt);
         mutex_unlock(&ici->host_lock);
         return res;
 }
@@ -900,7 +845,7 @@ static int soc_camera_s_fmt_vid_cap(struct file *file, void *priv,
         if (icd->streamer && icd->streamer != file)
                 return -EBUSY;
 
-        if (is_streaming(to_soc_camera_host(icd->parent), icd)) {
+        if (vb2_is_streaming(&icd->vb2_vidq)) {
                 dev_err(icd->pdev, "S_FMT denied: queue initialised\n");
                 return -EBUSY;
         }
@@ -971,7 +916,6 @@ static int soc_camera_streamon(struct file *file, void *priv,
                                enum v4l2_buf_type i)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
         int ret;
 
@@ -983,12 +927,8 @@ static int soc_camera_streamon(struct file *file, void *priv,
         if (icd->streamer != file)
                 return -EBUSY;
 
-        /* This calls buf_queue from host driver's videobuf_queue_ops */
-        if (ici->ops->init_videobuf)
-                ret = videobuf_streamon(&icd->vb_vidq);
-        else
-                ret = vb2_streamon(&icd->vb2_vidq, i);
-
+        /* This calls buf_queue from host driver's videobuf2_queue_ops */
+        ret = vb2_streamon(&icd->vb2_vidq, i);
         if (!ret)
                 v4l2_subdev_call(sd, video, s_stream, 1);
 
@@ -1000,7 +940,6 @@ static int soc_camera_streamoff(struct file *file, void *priv,
 {
         struct soc_camera_device *icd = file->private_data;
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         int ret;
 
         WARN_ON(priv != file->private_data);
@@ -1012,13 +951,10 @@ static int soc_camera_streamoff(struct file *file, void *priv,
                 return -EBUSY;
 
         /*
-         * This calls buf_release from host driver's videobuf_queue_ops for all
+         * This calls buf_release from host driver's videobuf2_queue_ops for all
          * remaining buffers. When the last buffer is freed, stop capture
          */
-        if (ici->ops->init_videobuf)
-                ret = videobuf_streamoff(&icd->vb_vidq);
-        else
-                ret = vb2_streamoff(&icd->vb2_vidq, i);
+        ret = vb2_streamoff(&icd->vb2_vidq, i);
 
         v4l2_subdev_call(sd, video, s_stream, 0);
 
@@ -1053,7 +989,7 @@ static int soc_camera_s_selection(struct file *file, void *fh,
 
         if (s->target == V4L2_SEL_TGT_COMPOSE) {
                 /* No output size change during a running capture! */
-                if (is_streaming(ici, icd) &&
+                if (vb2_is_streaming(&icd->vb2_vidq) &&
                     (icd->user_width != s->r.width ||
                      icd->user_height != s->r.height))
                         return -EBUSY;
@@ -1066,7 +1002,8 @@ static int soc_camera_s_selection(struct file *file, void *fh,
                         return -EBUSY;
         }
 
-        if (s->target == V4L2_SEL_TGT_CROP && is_streaming(ici, icd) &&
+        if (s->target == V4L2_SEL_TGT_CROP &&
+            vb2_is_streaming(&icd->vb2_vidq) &&
             ici->ops->set_liveselection)
                 ret = ici->ops->set_liveselection(icd, s);
         else
@@ -1910,9 +1847,7 @@ int soc_camera_host_register(struct soc_camera_host *ici)
             !ici->ops->set_fmt ||
             !ici->ops->set_bus_param ||
             !ici->ops->querycap ||
-            ((!ici->ops->init_videobuf ||
-              !ici->ops->reqbufs) &&
-             !ici->ops->init_videobuf2) ||
+            !ici->ops->init_videobuf2 ||
             !ici->ops->poll ||
             !ici->v4l2_dev.dev)
                 return -EINVAL;
diff --git a/drivers/media/platform/soc_camera/soc_scale_crop.c b/drivers/media/platform/soc_camera/soc_scale_crop.c
index f77252d6ccd3..0116097c0c0f 100644
--- a/drivers/media/platform/soc_camera/soc_scale_crop.c
+++ b/drivers/media/platform/soc_camera/soc_scale_crop.c
@@ -62,7 +62,8 @@ int soc_camera_client_g_rect(struct v4l2_subdev *sd, struct v4l2_rect *rect)
 EXPORT_SYMBOL(soc_camera_client_g_rect);
 
 /* Client crop has changed, update our sub-rectangle to remain within the area */
-static void update_subrect(struct v4l2_rect *rect, struct v4l2_rect *subrect)
+static void move_and_crop_subrect(struct v4l2_rect *rect,
+                                  struct v4l2_rect *subrect)
 {
         if (rect->width < subrect->width)
                 subrect->width = rect->width;
@@ -72,14 +73,14 @@ static void update_subrect(struct v4l2_rect *rect, struct v4l2_rect *subrect)
 
         if (rect->left > subrect->left)
                 subrect->left = rect->left;
-        else if (rect->left + rect->width >
+        else if (rect->left + rect->width <
                  subrect->left + subrect->width)
                 subrect->left = rect->left + rect->width -
                         subrect->width;
 
         if (rect->top > subrect->top)
                 subrect->top = rect->top;
-        else if (rect->top + rect->height >
+        else if (rect->top + rect->height <
                  subrect->top + subrect->height)
                 subrect->top = rect->top + rect->height -
                         subrect->height;
@@ -216,7 +217,7 @@ int soc_camera_client_s_selection(struct v4l2_subdev *sd,
 
         if (!ret) {
                 *target_rect = *cam_rect;
-                update_subrect(target_rect, subrect);
+                move_and_crop_subrect(target_rect, subrect);
         }
 
         return ret;
@@ -299,7 +300,7 @@ update_cache:
         if (host_1to1)
                 *subrect = *rect;
         else
-                update_subrect(rect, subrect);
+                move_and_crop_subrect(rect, subrect);
 
         return 0;
 }
diff --git a/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.c b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.c
index 7652ce2ec1dc..59280ac31937 100644
--- a/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.c
+++ b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.c
@@ -865,9 +865,8 @@ static int c8sectpfe_probe(struct platform_device *pdev)
         }
 
         /* Setup timer interrupt */
-        init_timer(&fei->timer);
-        fei->timer.function = c8sectpfe_timer_interrupt;
-        fei->timer.data = (unsigned long)fei;
+        setup_timer(&fei->timer, c8sectpfe_timer_interrupt,
+                    (unsigned long)fei);
 
         mutex_init(&fei->lock);
 
diff --git a/drivers/media/platform/sti/cec/Makefile b/drivers/media/platform/sti/cec/Makefile
new file mode 100644
index 000000000000..f07905e1448a
--- /dev/null
+++ b/drivers/media/platform/sti/cec/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_VIDEO_STI_HDMI_CEC) += stih-cec.o
diff --git a/drivers/media/platform/sti/cec/stih-cec.c b/drivers/media/platform/sti/cec/stih-cec.c
new file mode 100644
index 000000000000..39ff55145a82
--- /dev/null
+++ b/drivers/media/platform/sti/cec/stih-cec.c
@@ -0,0 +1,404 @@
+/*
+ * STIH4xx CEC driver
+ * Copyright (C) STMicroelectronic SA 2016
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+
+#include <media/cec.h>
+#include <media/cec-notifier.h>
+
+#define CEC_NAME        "stih-cec"
+
+/* CEC registers  */
+#define CEC_CLK_DIV           0x0
+#define CEC_CTRL              0x4
+#define CEC_IRQ_CTRL          0x8
+#define CEC_STATUS            0xC
+#define CEC_EXT_STATUS        0x10
+#define CEC_TX_CTRL           0x14
+#define CEC_FREE_TIME_THRESH  0x18
+#define CEC_BIT_TOUT_THRESH   0x1C
+#define CEC_BIT_PULSE_THRESH  0x20
+#define CEC_DATA              0x24
+#define CEC_TX_ARRAY_CTRL     0x28
+#define CEC_CTRL2             0x2C
+#define CEC_TX_ERROR_STS      0x30
+#define CEC_ADDR_TABLE        0x34
+#define CEC_DATA_ARRAY_CTRL   0x38
+#define CEC_DATA_ARRAY_STATUS 0x3C
+#define CEC_TX_DATA_BASE      0x40
+#define CEC_TX_DATA_TOP       0x50
+#define CEC_TX_DATA_SIZE      0x1
+#define CEC_RX_DATA_BASE      0x54
+#define CEC_RX_DATA_TOP       0x64
+#define CEC_RX_DATA_SIZE      0x1
+
+/* CEC_CTRL2 */
+#define CEC_LINE_INACTIVE_EN   BIT(0)
+#define CEC_AUTO_BUS_ERR_EN    BIT(1)
+#define CEC_STOP_ON_ARB_ERR_EN BIT(2)
+#define CEC_TX_REQ_WAIT_EN     BIT(3)
+
+/* CEC_DATA_ARRAY_CTRL */
+#define CEC_TX_ARRAY_EN          BIT(0)
+#define CEC_RX_ARRAY_EN          BIT(1)
+#define CEC_TX_ARRAY_RESET       BIT(2)
+#define CEC_RX_ARRAY_RESET       BIT(3)
+#define CEC_TX_N_OF_BYTES_IRQ_EN BIT(4)
+#define CEC_TX_STOP_ON_NACK      BIT(7)
+
+/* CEC_TX_ARRAY_CTRL */
+#define CEC_TX_N_OF_BYTES  0x1F
+#define CEC_TX_START       BIT(5)
+#define CEC_TX_AUTO_SOM_EN BIT(6)
+#define CEC_TX_AUTO_EOM_EN BIT(7)
+
+/* CEC_IRQ_CTRL */
+#define CEC_TX_DONE_IRQ_EN   BIT(0)
+#define CEC_ERROR_IRQ_EN     BIT(2)
+#define CEC_RX_DONE_IRQ_EN   BIT(3)
+#define CEC_RX_SOM_IRQ_EN    BIT(4)
+#define CEC_RX_EOM_IRQ_EN    BIT(5)
+#define CEC_FREE_TIME_IRQ_EN BIT(6)
+#define CEC_PIN_STS_IRQ_EN   BIT(7)
+
+/* CEC_CTRL */
+#define CEC_IN_FILTER_EN    BIT(0)
+#define CEC_PWR_SAVE_EN     BIT(1)
+#define CEC_EN              BIT(4)
+#define CEC_ACK_CTRL        BIT(5)
+#define CEC_RX_RESET_EN     BIT(6)
+#define CEC_IGNORE_RX_ERROR BIT(7)
+
+/* CEC_STATUS */
+#define CEC_TX_DONE_STS       BIT(0)
+#define CEC_TX_ACK_GET_STS    BIT(1)
+#define CEC_ERROR_STS         BIT(2)
+#define CEC_RX_DONE_STS       BIT(3)
+#define CEC_RX_SOM_STS        BIT(4)
+#define CEC_RX_EOM_STS        BIT(5)
+#define CEC_FREE_TIME_IRQ_STS BIT(6)
+#define CEC_PIN_STS           BIT(7)
+#define CEC_SBIT_TOUT_STS     BIT(8)
+#define CEC_DBIT_TOUT_STS     BIT(9)
+#define CEC_LPULSE_ERROR_STS  BIT(10)
+#define CEC_HPULSE_ERROR_STS  BIT(11)
+#define CEC_TX_ERROR          BIT(12)
+#define CEC_TX_ARB_ERROR      BIT(13)
+#define CEC_RX_ERROR_MIN      BIT(14)
+#define CEC_RX_ERROR_MAX      BIT(15)
+
+/* Signal free time in bit periods (2.4ms) */
+#define CEC_PRESENT_INIT_SFT 7
+#define CEC_NEW_INIT_SFT     5
+#define CEC_RETRANSMIT_SFT   3
+
+/* Constants for CEC_BIT_TOUT_THRESH register */
+#define CEC_SBIT_TOUT_47MS BIT(1)
+#define CEC_SBIT_TOUT_48MS (BIT(0) | BIT(1))
+#define CEC_SBIT_TOUT_50MS BIT(2)
+#define CEC_DBIT_TOUT_27MS BIT(0)
+#define CEC_DBIT_TOUT_28MS BIT(1)
+#define CEC_DBIT_TOUT_29MS (BIT(0) | BIT(1))
+
+/* Constants for CEC_BIT_PULSE_THRESH register */
+#define CEC_BIT_LPULSE_03MS BIT(1)
+#define CEC_BIT_HPULSE_03MS BIT(3)
+
+/* Constants for CEC_DATA_ARRAY_STATUS register */
+#define CEC_RX_N_OF_BYTES                     0x1F
+#define CEC_TX_N_OF_BYTES_SENT                BIT(5)
+#define CEC_RX_OVERRUN                        BIT(6)
+
+struct stih_cec {
+        struct cec_adapter      *adap;
+        struct device           *dev;
+        struct clk              *clk;
+        void __iomem            *regs;
+        int                     irq;
+        u32                     irq_status;
+        struct cec_notifier     *notifier;
+};
+
+static int stih_cec_adap_enable(struct cec_adapter *adap, bool enable)
+{
+        struct stih_cec *cec = cec_get_drvdata(adap);
+
+        if (enable) {
+                /* The doc says (input TCLK_PERIOD * CEC_CLK_DIV) = 0.1ms */
+                unsigned long clk_freq = clk_get_rate(cec->clk);
+                u32 cec_clk_div = clk_freq / 10000;
+
+                writel(cec_clk_div, cec->regs + CEC_CLK_DIV);
+
+                /* Configuration of the durations activating a timeout */
+                writel(CEC_SBIT_TOUT_47MS | (CEC_DBIT_TOUT_28MS << 4),
+                       cec->regs + CEC_BIT_TOUT_THRESH);
+
+                /* Configuration of the smallest allowed duration for pulses */
+                writel(CEC_BIT_LPULSE_03MS | CEC_BIT_HPULSE_03MS,
+                       cec->regs + CEC_BIT_PULSE_THRESH);
+
+                /* Minimum received bit period threshold */
+                writel(BIT(5) | BIT(7), cec->regs + CEC_TX_CTRL);
+
+                /* Configuration of transceiver data arrays */
+                writel(CEC_TX_ARRAY_EN | CEC_RX_ARRAY_EN | CEC_TX_STOP_ON_NACK,
+                       cec->regs + CEC_DATA_ARRAY_CTRL);
+
+                /* Configuration of the control bits for CEC Transceiver */
+                writel(CEC_IN_FILTER_EN | CEC_EN | CEC_RX_RESET_EN,
+                       cec->regs + CEC_CTRL);
+
+                /* Clear logical addresses */
+                writel(0, cec->regs + CEC_ADDR_TABLE);
+
+                /* Clear the status register */
+                writel(0x0, cec->regs + CEC_STATUS);
+
+                /* Enable the interrupts */
+                writel(CEC_TX_DONE_IRQ_EN | CEC_RX_DONE_IRQ_EN |
+                       CEC_RX_SOM_IRQ_EN | CEC_RX_EOM_IRQ_EN |
+                       CEC_ERROR_IRQ_EN,
+                       cec->regs + CEC_IRQ_CTRL);
+
+        } else {
+                /* Clear logical addresses */
+                writel(0, cec->regs + CEC_ADDR_TABLE);
+
+                /* Clear the status register */
+                writel(0x0, cec->regs + CEC_STATUS);
+
+                /* Disable the interrupts */
+                writel(0, cec->regs + CEC_IRQ_CTRL);
+        }
+
+        return 0;
+}
+
+static int stih_cec_adap_log_addr(struct cec_adapter *adap, u8 logical_addr)
+{
+        struct stih_cec *cec = cec_get_drvdata(adap);
+        u32 reg = readl(cec->regs + CEC_ADDR_TABLE);
+
+        reg |= 1 << logical_addr;
+
+        if (logical_addr == CEC_LOG_ADDR_INVALID)
+                reg = 0;
+
+        writel(reg, cec->regs + CEC_ADDR_TABLE);
+
+        return 0;
+}
+
+static int stih_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
+                                  u32 signal_free_time, struct cec_msg *msg)
+{
+        struct stih_cec *cec = cec_get_drvdata(adap);
+        int i;
+
+        /* Copy message into registers */
+        for (i = 0; i < msg->len; i++)
+                writeb(msg->msg[i], cec->regs + CEC_TX_DATA_BASE + i);
+
+        /* Start transmission, configure hardware to add start and stop bits
+         * Signal free time is handled by the hardware
+         */
+        writel(CEC_TX_AUTO_SOM_EN | CEC_TX_AUTO_EOM_EN | CEC_TX_START |
+               msg->len, cec->regs + CEC_TX_ARRAY_CTRL);
+
+        return 0;
+}
+
+static void stih_tx_done(struct stih_cec *cec, u32 status)
+{
+        if (status & CEC_TX_ERROR) {
+                cec_transmit_done(cec->adap, CEC_TX_STATUS_ERROR, 0, 0, 0, 1);
+                return;
+        }
+
+        if (status & CEC_TX_ARB_ERROR) {
+                cec_transmit_done(cec->adap,
+                                  CEC_TX_STATUS_ARB_LOST, 1, 0, 0, 0);
+                return;
+        }
+
+        if (!(status & CEC_TX_ACK_GET_STS)) {
+                cec_transmit_done(cec->adap, CEC_TX_STATUS_NACK, 0, 1, 0, 0);
+                return;
+        }
+
+        cec_transmit_done(cec->adap, CEC_TX_STATUS_OK, 0, 0, 0, 0);
+}
+
+static void stih_rx_done(struct stih_cec *cec, u32 status)
+{
+        struct cec_msg msg = {};
+        u8 i;
+
+        if (status & CEC_RX_ERROR_MIN)
+                return;
+
+        if (status & CEC_RX_ERROR_MAX)
+                return;
+
+        msg.len = readl(cec->regs + CEC_DATA_ARRAY_STATUS) & 0x1f;
+
+        if (!msg.len)
+                return;
+
+        if (msg.len > 16)
+                msg.len = 16;
+
+        for (i = 0; i < msg.len; i++)
+                msg.msg[i] = readl(cec->regs + CEC_RX_DATA_BASE + i);
+
+        cec_received_msg(cec->adap, &msg);
+}
+
+static irqreturn_t stih_cec_irq_handler_thread(int irq, void *priv)
+{
+        struct stih_cec *cec = priv;
+
+        if (cec->irq_status & CEC_TX_DONE_STS)
+                stih_tx_done(cec, cec->irq_status);
+
+        if (cec->irq_status & CEC_RX_DONE_STS)
+                stih_rx_done(cec, cec->irq_status);
+
+        cec->irq_status = 0;
+
+        return IRQ_HANDLED;
+}
+
+static irqreturn_t stih_cec_irq_handler(int irq, void *priv)
+{
+        struct stih_cec *cec = priv;
+
+        cec->irq_status = readl(cec->regs + CEC_STATUS);
+        writel(cec->irq_status, cec->regs + CEC_STATUS);
+
+        return IRQ_WAKE_THREAD;
+}
+
+static const struct cec_adap_ops sti_cec_adap_ops = {
+        .adap_enable = stih_cec_adap_enable,
+        .adap_log_addr = stih_cec_adap_log_addr,
+        .adap_transmit = stih_cec_adap_transmit,
+};
+
+static int stih_cec_probe(struct platform_device *pdev)
+{
+        struct device *dev = &pdev->dev;
+        struct resource *res;
+        struct stih_cec *cec;
+        struct device_node *np;
+        struct platform_device *hdmi_dev;
+        int ret;
+
+        cec = devm_kzalloc(dev, sizeof(*cec), GFP_KERNEL);
+        if (!cec)
+                return -ENOMEM;
+
+        np = of_parse_phandle(pdev->dev.of_node, "hdmi-phandle", 0);
+
+        if (!np) {
+                dev_err(&pdev->dev, "Failed to find hdmi node in device tree\n");
+                return -ENODEV;
+        }
+
+        hdmi_dev = of_find_device_by_node(np);
+        if (!hdmi_dev)
+                return -EPROBE_DEFER;
+
+        cec->notifier = cec_notifier_get(&hdmi_dev->dev);
+        if (!cec->notifier)
+                return -ENOMEM;
+
+        cec->dev = dev;
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        cec->regs = devm_ioremap_resource(dev, res);
+        if (IS_ERR(cec->regs))
+                return PTR_ERR(cec->regs);
+
+        cec->irq = platform_get_irq(pdev, 0);
+        if (cec->irq < 0)
+                return cec->irq;
+
+        ret = devm_request_threaded_irq(dev, cec->irq, stih_cec_irq_handler,
+                                        stih_cec_irq_handler_thread, 0,
+                                        pdev->name, cec);
+        if (ret)
+                return ret;
+
+        cec->clk = devm_clk_get(dev, "cec-clk");
+        if (IS_ERR(cec->clk)) {
+                dev_err(dev, "Cannot get cec clock\n");
+                return PTR_ERR(cec->clk);
+        }
+
+        cec->adap = cec_allocate_adapter(&sti_cec_adap_ops, cec,
+                        CEC_NAME,
+                        CEC_CAP_LOG_ADDRS | CEC_CAP_PASSTHROUGH |
+                        CEC_CAP_TRANSMIT, 1);
+        ret = PTR_ERR_OR_ZERO(cec->adap);
+        if (ret)
+                return ret;
+
+        ret = cec_register_adapter(cec->adap, &pdev->dev);
+        if (ret) {
+                cec_delete_adapter(cec->adap);
+                return ret;
+        }
+
+        cec_register_cec_notifier(cec->adap, cec->notifier);
+
+        platform_set_drvdata(pdev, cec);
+        return 0;
+}
+
+static int stih_cec_remove(struct platform_device *pdev)
+{
+        struct stih_cec *cec = platform_get_drvdata(pdev);
+
+        cec_unregister_adapter(cec->adap);
+        cec_notifier_put(cec->notifier);
+
+        return 0;
+}
+
+static const struct of_device_id stih_cec_match[] = {
+        {
+                .compatible     = "st,stih-cec",
+        },
+        {},
+};
+MODULE_DEVICE_TABLE(of, stih_cec_match);
+
+static struct platform_driver stih_cec_pdrv = {
+        .probe  = stih_cec_probe,
+        .remove = stih_cec_remove,
+        .driver = {
+                .name           = CEC_NAME,
+                .of_match_table = stih_cec_match,
+        },
+};
+
+module_platform_driver(stih_cec_pdrv);
+
+MODULE_AUTHOR("Benjamin Gaignard <benjamin.gaignard@linaro.org>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("STIH4xx CEC driver");
diff --git a/drivers/media/platform/sti/delta/delta-mjpeg-dec.c b/drivers/media/platform/sti/delta/delta-mjpeg-dec.c
index e79bdc611432..84ea43c0eb46 100644
--- a/drivers/media/platform/sti/delta/delta-mjpeg-dec.c
+++ b/drivers/media/platform/sti/delta/delta-mjpeg-dec.c
@@ -375,7 +375,7 @@ static int delta_mjpeg_decode(struct delta_ctx *pctx, struct delta_au *pau)
         struct delta_mjpeg_ctx *ctx = to_ctx(pctx);
         int ret;
         struct delta_au au = *pau;
-        unsigned int data_offset;
+        unsigned int data_offset = 0;
         struct mjpeg_header *header = &ctx->header_struct;
 
         if (!ctx->header) {
diff --git a/drivers/media/platform/ti-vpe/vpdma.c b/drivers/media/platform/ti-vpe/vpdma.c
index 23472e3784ff..e2cf2b90e500 100644
--- a/drivers/media/platform/ti-vpe/vpdma.c
+++ b/drivers/media/platform/ti-vpe/vpdma.c
@@ -801,17 +801,17 @@ static void dump_dtd(struct vpdma_dtd *dtd)
  * flags: VPDMA flags to configure some descriptor fileds
  */
 void vpdma_add_out_dtd(struct vpdma_desc_list *list, int width,
-                const struct v4l2_rect *c_rect,
+                int stride, const struct v4l2_rect *c_rect,
                 const struct vpdma_data_format *fmt, dma_addr_t dma_addr,
                 int max_w, int max_h, enum vpdma_channel chan, u32 flags)
 {
-        vpdma_rawchan_add_out_dtd(list, width, c_rect, fmt, dma_addr,
+        vpdma_rawchan_add_out_dtd(list, width, stride, c_rect, fmt, dma_addr,
                                   max_w, max_h, chan_info[chan].num, flags);
 }
 EXPORT_SYMBOL(vpdma_add_out_dtd);
 
 void vpdma_rawchan_add_out_dtd(struct vpdma_desc_list *list, int width,
-                const struct v4l2_rect *c_rect,
+                int stride, const struct v4l2_rect *c_rect,
                 const struct vpdma_data_format *fmt, dma_addr_t dma_addr,
                 int max_w, int max_h, int raw_vpdma_chan, u32 flags)
 {
@@ -821,7 +821,6 @@ void vpdma_rawchan_add_out_dtd(struct vpdma_desc_list *list, int width,
         int channel, next_chan;
         struct v4l2_rect rect = *c_rect;
         int depth = fmt->depth;
-        int stride;
         struct vpdma_dtd *dtd;
 
         channel = next_chan = raw_vpdma_chan;
@@ -833,8 +832,6 @@ void vpdma_rawchan_add_out_dtd(struct vpdma_desc_list *list, int width,
                 depth = 8;
         }
 
-        stride = ALIGN((depth * width) >> 3, VPDMA_STRIDE_ALIGN);
-
         dma_addr += rect.top * stride + (rect.left * depth >> 3);
 
         dtd = list->next;
@@ -882,7 +879,7 @@ EXPORT_SYMBOL(vpdma_rawchan_add_out_dtd);
  *                      contribute to the client)
  */
 void vpdma_add_in_dtd(struct vpdma_desc_list *list, int width,
-                const struct v4l2_rect *c_rect,
+                int stride, const struct v4l2_rect *c_rect,
                 const struct vpdma_data_format *fmt, dma_addr_t dma_addr,
                 enum vpdma_channel chan, int field, u32 flags, int frame_width,
                 int frame_height, int start_h, int start_v)
@@ -892,7 +889,6 @@ void vpdma_add_in_dtd(struct vpdma_desc_list *list, int width,
         int depth = fmt->depth;
         int channel, next_chan;
         struct v4l2_rect rect = *c_rect;
-        int stride;
         struct vpdma_dtd *dtd;
 
         channel = next_chan = chan_info[chan].num;
@@ -904,8 +900,6 @@ void vpdma_add_in_dtd(struct vpdma_desc_list *list, int width,
                 depth = 8;
         }
 
-        stride = ALIGN((depth * width) >> 3, VPDMA_STRIDE_ALIGN);
-
         dma_addr += rect.top * stride + (rect.left * depth >> 3);
 
         dtd = list->next;
diff --git a/drivers/media/platform/ti-vpe/vpdma.h b/drivers/media/platform/ti-vpe/vpdma.h
index 131700c112b2..7e611501c291 100644
--- a/drivers/media/platform/ti-vpe/vpdma.h
+++ b/drivers/media/platform/ti-vpe/vpdma.h
@@ -242,16 +242,16 @@ void vpdma_add_sync_on_channel_ctd(struct vpdma_desc_list *list,
 void vpdma_add_abort_channel_ctd(struct vpdma_desc_list *list,
                 int chan_num);
 void vpdma_add_out_dtd(struct vpdma_desc_list *list, int width,
-                const struct v4l2_rect *c_rect,
+                int stride, const struct v4l2_rect *c_rect,
                 const struct vpdma_data_format *fmt, dma_addr_t dma_addr,
                 int max_w, int max_h, enum vpdma_channel chan, u32 flags);
 void vpdma_rawchan_add_out_dtd(struct vpdma_desc_list *list, int width,
-                const struct v4l2_rect *c_rect,
+                int stride, const struct v4l2_rect *c_rect,
                 const struct vpdma_data_format *fmt, dma_addr_t dma_addr,
                 int max_w, int max_h, int raw_vpdma_chan, u32 flags);
 
 void vpdma_add_in_dtd(struct vpdma_desc_list *list, int width,
-                const struct v4l2_rect *c_rect,
+                int stride, const struct v4l2_rect *c_rect,
                 const struct vpdma_data_format *fmt, dma_addr_t dma_addr,
                 enum vpdma_channel chan, int field, u32 flags, int frame_width,
                 int frame_height, int start_h, int start_v);
diff --git a/drivers/media/platform/ti-vpe/vpe.c b/drivers/media/platform/ti-vpe/vpe.c
index f0156b7759e9..c47151495b6f 100644
--- a/drivers/media/platform/ti-vpe/vpe.c
+++ b/drivers/media/platform/ti-vpe/vpe.c
@@ -1085,7 +1085,8 @@ static void add_out_dtd(struct vpe_ctx *ctx, int port)
         vpdma_set_max_size(ctx->dev->vpdma, VPDMA_MAX_SIZE1,
                            MAX_W, MAX_H);
 
-        vpdma_add_out_dtd(&ctx->desc_list, q_data->width, &q_data->c_rect,
+        vpdma_add_out_dtd(&ctx->desc_list, q_data->width,
+                          q_data->bytesperline[VPE_LUMA], &q_data->c_rect,
                           vpdma_fmt, dma_addr, MAX_OUT_WIDTH_REG1,
                           MAX_OUT_HEIGHT_REG1, p_data->channel, flags);
 }
@@ -1169,7 +1170,8 @@ static void add_in_dtd(struct vpe_ctx *ctx, int port)
         if (p_data->vb_part && fmt->fourcc == V4L2_PIX_FMT_NV12)
                 frame_height /= 2;
 
-        vpdma_add_in_dtd(&ctx->desc_list, q_data->width, &q_data->c_rect,
+        vpdma_add_in_dtd(&ctx->desc_list, q_data->width,
+                         q_data->bytesperline[VPE_LUMA], &q_data->c_rect,
                 vpdma_fmt, dma_addr, p_data->channel, field, flags, frame_width,
                 frame_height, 0, 0);
 }
@@ -1595,6 +1597,7 @@ static int __vpe_try_fmt(struct vpe_ctx *ctx, struct v4l2_format *f,
         struct v4l2_plane_pix_format *plane_fmt;
         unsigned int w_align;
         int i, depth, depth_bytes, height;
+        unsigned int stride = 0;
 
         if (!fmt || !(fmt->types & type)) {
                 vpe_err(ctx->dev, "Fourcc format (0x%08x) invalid.\n",
@@ -1681,16 +1684,27 @@ static int __vpe_try_fmt(struct vpe_ctx *ctx, struct v4l2_format *f,
                 plane_fmt = &pix->plane_fmt[i];
                 depth = fmt->vpdma_fmt[i]->depth;
 
-                if (i == VPE_LUMA)
-                        plane_fmt->bytesperline = (pix->width * depth) >> 3;
-                else
-                        plane_fmt->bytesperline = pix->width;
+                stride = (pix->width * fmt->vpdma_fmt[VPE_LUMA]->depth) >> 3;
+                if (stride > plane_fmt->bytesperline)
+                        plane_fmt->bytesperline = stride;
+
+                plane_fmt->bytesperline = ALIGN(plane_fmt->bytesperline,
+                                                VPDMA_STRIDE_ALIGN);
 
-                if (pix->num_planes == 1 && fmt->coplanar)
-                        depth += fmt->vpdma_fmt[VPE_CHROMA]->depth;
-                plane_fmt->sizeimage =
-                                (pix->height * pix->width * depth) >> 3;
+                if (i == VPE_LUMA) {
+                        plane_fmt->sizeimage = pix->height *
+                                               plane_fmt->bytesperline;
 
+                        if (pix->num_planes == 1 && fmt->coplanar)
+                                plane_fmt->sizeimage += pix->height *
+                                        plane_fmt->bytesperline *
+                                        fmt->vpdma_fmt[VPE_CHROMA]->depth >> 3;
+
+                } else { /* i == VIP_CHROMA */
+                        plane_fmt->sizeimage = (pix->height *
+                                               plane_fmt->bytesperline *
+                                               depth) >> 3;
+                }
                 memset(plane_fmt->reserved, 0, sizeof(plane_fmt->reserved));
         }
 
diff --git a/drivers/media/platform/vimc/Kconfig b/drivers/media/platform/vimc/Kconfig
new file mode 100644
index 000000000000..a18f6352c422
--- /dev/null
+++ b/drivers/media/platform/vimc/Kconfig
@@ -0,0 +1,14 @@
+config VIDEO_VIMC
+        tristate "Virtual Media Controller Driver (VIMC)"
+        depends on VIDEO_DEV && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+        select VIDEOBUF2_VMALLOC
+        default n
+        ---help---
+          Skeleton driver for Virtual Media Controller
+
+          This driver can be compared to the vivid driver for emulating
+          a media node that exposes a complex media topology. The topology
+          is hard coded for now but is meant to be highly configurable in
+          the future.
+
+          When in doubt, say N.
diff --git a/drivers/media/platform/vimc/Makefile b/drivers/media/platform/vimc/Makefile
new file mode 100644
index 000000000000..c45195e5e05c
--- /dev/null
+++ b/drivers/media/platform/vimc/Makefile
@@ -0,0 +1,3 @@
+vimc-objs := vimc-core.o vimc-capture.o vimc-sensor.o
+
+obj-$(CONFIG_VIDEO_VIMC) += vimc.o
diff --git a/drivers/media/platform/vimc/vimc-capture.c b/drivers/media/platform/vimc/vimc-capture.c
new file mode 100644
index 000000000000..9adb06d7e13d
--- /dev/null
+++ b/drivers/media/platform/vimc/vimc-capture.c
@@ -0,0 +1,498 @@
+/*
+ * vimc-capture.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-vmalloc.h>
+
+#include "vimc-capture.h"
+
+struct vimc_cap_device {
+        struct vimc_ent_device ved;
+        struct video_device vdev;
+        struct v4l2_pix_format format;
+        struct vb2_queue queue;
+        struct list_head buf_list;
+        /*
+         * NOTE: in a real driver, a spin lock must be used to access the
+         * queue because the frames are generated from a hardware interruption
+         * and the isr is not allowed to sleep.
+         * Even if it is not necessary a spinlock in the vimc driver, we
+         * use it here as a code reference
+         */
+        spinlock_t qlock;
+        struct mutex lock;
+        u32 sequence;
+        struct media_pipeline pipe;
+};
+
+struct vimc_cap_buffer {
+        /*
+         * struct vb2_v4l2_buffer must be the first element
+         * the videobuf2 framework will allocate this struct based on
+         * buf_struct_size and use the first sizeof(struct vb2_buffer) bytes of
+         * memory as a vb2_buffer
+         */
+        struct vb2_v4l2_buffer vb2;
+        struct list_head list;
+};
+
+static int vimc_cap_querycap(struct file *file, void *priv,
+                             struct v4l2_capability *cap)
+{
+        struct vimc_cap_device *vcap = video_drvdata(file);
+
+        strlcpy(cap->driver, KBUILD_MODNAME, sizeof(cap->driver));
+        strlcpy(cap->card, KBUILD_MODNAME, sizeof(cap->card));
+        snprintf(cap->bus_info, sizeof(cap->bus_info),
+                 "platform:%s", vcap->vdev.v4l2_dev->name);
+
+        return 0;
+}
+
+static int vimc_cap_fmt_vid_cap(struct file *file, void *priv,
+                                  struct v4l2_format *f)
+{
+        struct vimc_cap_device *vcap = video_drvdata(file);
+
+        f->fmt.pix = vcap->format;
+
+        return 0;
+}
+
+static int vimc_cap_enum_fmt_vid_cap(struct file *file, void *priv,
+                                     struct v4l2_fmtdesc *f)
+{
+        struct vimc_cap_device *vcap = video_drvdata(file);
+
+        if (f->index > 0)
+                return -EINVAL;
+
+        /* We only support one format for now */
+        f->pixelformat = vcap->format.pixelformat;
+
+        return 0;
+}
+
+static const struct v4l2_file_operations vimc_cap_fops = {
+        .owner          = THIS_MODULE,
+        .open           = v4l2_fh_open,
+        .release        = vb2_fop_release,
+        .read           = vb2_fop_read,
+        .poll           = vb2_fop_poll,
+        .unlocked_ioctl = video_ioctl2,
+        .mmap           = vb2_fop_mmap,
+};
+
+static const struct v4l2_ioctl_ops vimc_cap_ioctl_ops = {
+        .vidioc_querycap = vimc_cap_querycap,
+
+        .vidioc_g_fmt_vid_cap = vimc_cap_fmt_vid_cap,
+        .vidioc_s_fmt_vid_cap = vimc_cap_fmt_vid_cap,
+        .vidioc_try_fmt_vid_cap = vimc_cap_fmt_vid_cap,
+        .vidioc_enum_fmt_vid_cap = vimc_cap_enum_fmt_vid_cap,
+
+        .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,
+};
+
+static void vimc_cap_return_all_buffers(struct vimc_cap_device *vcap,
+                                        enum vb2_buffer_state state)
+{
+        struct vimc_cap_buffer *vbuf, *node;
+
+        spin_lock(&vcap->qlock);
+
+        list_for_each_entry_safe(vbuf, node, &vcap->buf_list, list) {
+                list_del(&vbuf->list);
+                vb2_buffer_done(&vbuf->vb2.vb2_buf, state);
+        }
+
+        spin_unlock(&vcap->qlock);
+}
+
+static int vimc_cap_pipeline_s_stream(struct vimc_cap_device *vcap, int enable)
+{
+        struct v4l2_subdev *sd;
+        struct media_pad *pad;
+        int ret;
+
+        /* Start the stream in the subdevice direct connected */
+        pad = media_entity_remote_pad(&vcap->vdev.entity.pads[0]);
+
+        /*
+         * if it is a raw node from vimc-core, there is nothing to activate
+         * TODO: remove this when there are no more raw nodes in the
+         * core and return error instead
+         */
+        if (pad->entity->obj_type == MEDIA_ENTITY_TYPE_BASE)
+                return 0;
+
+        sd = media_entity_to_v4l2_subdev(pad->entity);
+        ret = v4l2_subdev_call(sd, video, s_stream, enable);
+        if (ret && ret != -ENOIOCTLCMD)
+                return ret;
+
+        return 0;
+}
+
+static int vimc_cap_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+        struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);
+        struct media_entity *entity = &vcap->vdev.entity;
+        int ret;
+
+        vcap->sequence = 0;
+
+        /* Start the media pipeline */
+        ret = media_pipeline_start(entity, &vcap->pipe);
+        if (ret) {
+                vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_QUEUED);
+                return ret;
+        }
+
+        /* Enable streaming from the pipe */
+        ret = vimc_cap_pipeline_s_stream(vcap, 1);
+        if (ret) {
+                media_pipeline_stop(entity);
+                vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_QUEUED);
+                return ret;
+        }
+
+        return 0;
+}
+
+/*
+ * Stop the stream engine. Any remaining buffers in the stream queue are
+ * dequeued and passed on to the vb2 framework marked as STATE_ERROR.
+ */
+static void vimc_cap_stop_streaming(struct vb2_queue *vq)
+{
+        struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);
+
+        /* Disable streaming from the pipe */
+        vimc_cap_pipeline_s_stream(vcap, 0);
+
+        /* Stop the media pipeline */
+        media_pipeline_stop(&vcap->vdev.entity);
+
+        /* Release all active buffers */
+        vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_ERROR);
+}
+
+static void vimc_cap_buf_queue(struct vb2_buffer *vb2_buf)
+{
+        struct vimc_cap_device *vcap = vb2_get_drv_priv(vb2_buf->vb2_queue);
+        struct vimc_cap_buffer *buf = container_of(vb2_buf,
+                                                   struct vimc_cap_buffer,
+                                                   vb2.vb2_buf);
+
+        spin_lock(&vcap->qlock);
+        list_add_tail(&buf->list, &vcap->buf_list);
+        spin_unlock(&vcap->qlock);
+}
+
+static int vimc_cap_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+                                unsigned int *nplanes, unsigned int sizes[],
+                                struct device *alloc_devs[])
+{
+        struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);
+
+        if (*nplanes)
+                return sizes[0] < vcap->format.sizeimage ? -EINVAL : 0;
+        /* We don't support multiplanes for now */
+        *nplanes = 1;
+        sizes[0] = vcap->format.sizeimage;
+
+        return 0;
+}
+
+static int vimc_cap_buffer_prepare(struct vb2_buffer *vb)
+{
+        struct vimc_cap_device *vcap = vb2_get_drv_priv(vb->vb2_queue);
+        unsigned long size = vcap->format.sizeimage;
+
+        if (vb2_plane_size(vb, 0) < size) {
+                dev_err(vcap->vdev.v4l2_dev->dev,
+                        "%s: buffer too small (%lu < %lu)\n",
+                        vcap->vdev.name, vb2_plane_size(vb, 0), size);
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static const struct vb2_ops vimc_cap_qops = {
+        .start_streaming        = vimc_cap_start_streaming,
+        .stop_streaming         = vimc_cap_stop_streaming,
+        .buf_queue              = vimc_cap_buf_queue,
+        .queue_setup            = vimc_cap_queue_setup,
+        .buf_prepare            = vimc_cap_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,
+};
+
+/*
+ * NOTE: this function is a copy of v4l2_subdev_link_validate_get_format
+ * maybe the v4l2 function should be public
+ */
+static int vimc_cap_v4l2_subdev_link_validate_get_format(struct media_pad *pad,
+                                                struct v4l2_subdev_format *fmt)
+{
+        struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(pad->entity);
+
+        fmt->which = V4L2_SUBDEV_FORMAT_ACTIVE;
+        fmt->pad = pad->index;
+
+        return v4l2_subdev_call(sd, pad, get_fmt, NULL, fmt);
+}
+
+static int vimc_cap_link_validate(struct media_link *link)
+{
+        struct v4l2_subdev_format source_fmt;
+        const struct vimc_pix_map *vpix;
+        struct vimc_cap_device *vcap = container_of(link->sink->entity,
+                                                    struct vimc_cap_device,
+                                                    vdev.entity);
+        struct v4l2_pix_format *sink_fmt = &vcap->format;
+        int ret;
+
+        /*
+         * if it is a raw node from vimc-core, ignore the link for now
+         * TODO: remove this when there are no more raw nodes in the
+         * core and return error instead
+         */
+        if (link->source->entity->obj_type == MEDIA_ENTITY_TYPE_BASE)
+                return 0;
+
+        /* Get the the format of the subdev */
+        ret = vimc_cap_v4l2_subdev_link_validate_get_format(link->source,
+                                                            &source_fmt);
+        if (ret)
+                return ret;
+
+        dev_dbg(vcap->vdev.v4l2_dev->dev,
+                "%s: link validate formats src:%dx%d %d sink:%dx%d %d\n",
+                vcap->vdev.name,
+                source_fmt.format.width, source_fmt.format.height,
+                source_fmt.format.code,
+                sink_fmt->width, sink_fmt->height,
+                sink_fmt->pixelformat);
+
+        /* The width, height and code must match. */
+        vpix = vimc_pix_map_by_pixelformat(sink_fmt->pixelformat);
+        if (source_fmt.format.width != sink_fmt->width
+            || source_fmt.format.height != sink_fmt->height
+            || vpix->code != source_fmt.format.code)
+                return -EPIPE;
+
+        /*
+         * The field order must match, or the sink field order must be NONE
+         * to support interlaced hardware connected to bridges that support
+         * progressive formats only.
+         */
+        if (source_fmt.format.field != sink_fmt->field &&
+            sink_fmt->field != V4L2_FIELD_NONE)
+                return -EPIPE;
+
+        return 0;
+}
+
+static const struct media_entity_operations vimc_cap_mops = {
+        .link_validate          = vimc_cap_link_validate,
+};
+
+static void vimc_cap_destroy(struct vimc_ent_device *ved)
+{
+        struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device,
+                                                    ved);
+
+        vb2_queue_release(&vcap->queue);
+        media_entity_cleanup(ved->ent);
+        video_unregister_device(&vcap->vdev);
+        vimc_pads_cleanup(vcap->ved.pads);
+        kfree(vcap);
+}
+
+static void vimc_cap_process_frame(struct vimc_ent_device *ved,
+                                   struct media_pad *sink, const void *frame)
+{
+        struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device,
+                                                    ved);
+        struct vimc_cap_buffer *vimc_buf;
+        void *vbuf;
+
+        spin_lock(&vcap->qlock);
+
+        /* Get the first entry of the list */
+        vimc_buf = list_first_entry_or_null(&vcap->buf_list,
+                                            typeof(*vimc_buf), list);
+        if (!vimc_buf) {
+                spin_unlock(&vcap->qlock);
+                return;
+        }
+
+        /* Remove this entry from the list */
+        list_del(&vimc_buf->list);
+
+        spin_unlock(&vcap->qlock);
+
+        /* Fill the buffer */
+        vimc_buf->vb2.vb2_buf.timestamp = ktime_get_ns();
+        vimc_buf->vb2.sequence = vcap->sequence++;
+        vimc_buf->vb2.field = vcap->format.field;
+
+        vbuf = vb2_plane_vaddr(&vimc_buf->vb2.vb2_buf, 0);
+
+        memcpy(vbuf, frame, vcap->format.sizeimage);
+
+        /* Set it as ready */
+        vb2_set_plane_payload(&vimc_buf->vb2.vb2_buf, 0,
+                              vcap->format.sizeimage);
+        vb2_buffer_done(&vimc_buf->vb2.vb2_buf, VB2_BUF_STATE_DONE);
+}
+
+struct vimc_ent_device *vimc_cap_create(struct v4l2_device *v4l2_dev,
+                                        const char *const name,
+                                        u16 num_pads,
+                                        const unsigned long *pads_flag)
+{
+        const struct vimc_pix_map *vpix;
+        struct vimc_cap_device *vcap;
+        struct video_device *vdev;
+        struct vb2_queue *q;
+        int ret;
+
+        /*
+         * Check entity configuration params
+         * NOTE: we only support a single sink pad
+         */
+        if (!name || num_pads != 1 || !pads_flag ||
+            !(pads_flag[0] & MEDIA_PAD_FL_SINK))
+                return ERR_PTR(-EINVAL);
+
+        /* Allocate the vimc_cap_device struct */
+        vcap = kzalloc(sizeof(*vcap), GFP_KERNEL);
+        if (!vcap)
+                return ERR_PTR(-ENOMEM);
+
+        /* Allocate the pads */
+        vcap->ved.pads = vimc_pads_init(num_pads, pads_flag);
+        if (IS_ERR(vcap->ved.pads)) {
+                ret = PTR_ERR(vcap->ved.pads);
+                goto err_free_vcap;
+        }
+
+        /* Initialize the media entity */
+        vcap->vdev.entity.name = name;
+        vcap->vdev.entity.function = MEDIA_ENT_F_IO_V4L;
+        ret = media_entity_pads_init(&vcap->vdev.entity,
+                                     num_pads, vcap->ved.pads);
+        if (ret)
+                goto err_clean_pads;
+
+        /* Initialize the lock */
+        mutex_init(&vcap->lock);
+
+        /* Initialize the vb2 queue */
+        q = &vcap->queue;
+        q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        q->io_modes = VB2_MMAP | VB2_DMABUF;
+        q->drv_priv = vcap;
+        q->buf_struct_size = sizeof(struct vimc_cap_buffer);
+        q->ops = &vimc_cap_qops;
+        q->mem_ops = &vb2_vmalloc_memops;
+        q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+        q->min_buffers_needed = 2;
+        q->lock = &vcap->lock;
+
+        ret = vb2_queue_init(q);
+        if (ret) {
+                dev_err(vcap->vdev.v4l2_dev->dev,
+                        "%s: vb2 queue init failed (err=%d)\n",
+                        vcap->vdev.name, ret);
+                goto err_clean_m_ent;
+        }
+
+        /* Initialize buffer list and its lock */
+        INIT_LIST_HEAD(&vcap->buf_list);
+        spin_lock_init(&vcap->qlock);
+
+        /* Set the frame format (this is hardcoded for now) */
+        vcap->format.width = 640;
+        vcap->format.height = 480;
+        vcap->format.pixelformat = V4L2_PIX_FMT_RGB24;
+        vcap->format.field = V4L2_FIELD_NONE;
+        vcap->format.colorspace = V4L2_COLORSPACE_SRGB;
+
+        vpix = vimc_pix_map_by_pixelformat(vcap->format.pixelformat);
+
+        vcap->format.bytesperline = vcap->format.width * vpix->bpp;
+        vcap->format.sizeimage = vcap->format.bytesperline *
+                                 vcap->format.height;
+
+        /* Fill the vimc_ent_device struct */
+        vcap->ved.destroy = vimc_cap_destroy;
+        vcap->ved.ent = &vcap->vdev.entity;
+        vcap->ved.process_frame = vimc_cap_process_frame;
+
+        /* Initialize the video_device struct */
+        vdev = &vcap->vdev;
+        vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+        vdev->entity.ops = &vimc_cap_mops;
+        vdev->release = video_device_release_empty;
+        vdev->fops = &vimc_cap_fops;
+        vdev->ioctl_ops = &vimc_cap_ioctl_ops;
+        vdev->lock = &vcap->lock;
+        vdev->queue = q;
+        vdev->v4l2_dev = v4l2_dev;
+        vdev->vfl_dir = VFL_DIR_RX;
+        strlcpy(vdev->name, name, sizeof(vdev->name));
+        video_set_drvdata(vdev, &vcap->ved);
+
+        /* Register the video_device with the v4l2 and the media framework */
+        ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
+        if (ret) {
+                dev_err(vcap->vdev.v4l2_dev->dev,
+                        "%s: video register failed (err=%d)\n",
+                        vcap->vdev.name, ret);
+                goto err_release_queue;
+        }
+
+        return &vcap->ved;
+
+err_release_queue:
+        vb2_queue_release(q);
+err_clean_m_ent:
+        media_entity_cleanup(&vcap->vdev.entity);
+err_clean_pads:
+        vimc_pads_cleanup(vcap->ved.pads);
+err_free_vcap:
+        kfree(vcap);
+
+        return ERR_PTR(ret);
+}
diff --git a/drivers/media/platform/vimc/vimc-capture.h b/drivers/media/platform/vimc/vimc-capture.h
new file mode 100644
index 000000000000..581a813abdf1
--- /dev/null
+++ b/drivers/media/platform/vimc/vimc-capture.h
@@ -0,0 +1,28 @@
+/*
+ * vimc-capture.h Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _VIMC_CAPTURE_H_
+#define _VIMC_CAPTURE_H_
+
+#include "vimc-core.h"
+
+struct vimc_ent_device *vimc_cap_create(struct v4l2_device *v4l2_dev,
+                                        const char *const name,
+                                        u16 num_pads,
+                                        const unsigned long *pads_flag);
+
+#endif
diff --git a/drivers/media/platform/vimc/vimc-core.c b/drivers/media/platform/vimc/vimc-core.c
new file mode 100644
index 000000000000..bc107da8fbd5
--- /dev/null
+++ b/drivers/media/platform/vimc/vimc-core.c
@@ -0,0 +1,695 @@
+/*
+ * vimc-core.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <media/media-device.h>
+#include <media/v4l2-device.h>
+
+#include "vimc-capture.h"
+#include "vimc-core.h"
+#include "vimc-sensor.h"
+
+#define VIMC_PDEV_NAME "vimc"
+#define VIMC_MDEV_MODEL_NAME "VIMC MDEV"
+
+#define VIMC_ENT_LINK(src, srcpad, sink, sinkpad, link_flags) { \
+        .src_ent = src,                                         \
+        .src_pad = srcpad,                                      \
+        .sink_ent = sink,                                       \
+        .sink_pad = sinkpad,                                    \
+        .flags = link_flags,                                    \
+}
+
+struct vimc_device {
+        /*
+         * The pipeline configuration
+         * (filled before calling vimc_device_register)
+         */
+        const struct vimc_pipeline_config *pipe_cfg;
+
+        /* The Associated media_device parent */
+        struct media_device mdev;
+
+        /* Internal v4l2 parent device*/
+        struct v4l2_device v4l2_dev;
+
+        /* Internal topology */
+        struct vimc_ent_device **ved;
+};
+
+/**
+ * enum vimc_ent_node - Select the functionality of a node in the topology
+ * @VIMC_ENT_NODE_SENSOR:       A node of type SENSOR simulates a camera sensor
+ *                              generating internal images in bayer format and
+ *                              propagating those images through the pipeline
+ * @VIMC_ENT_NODE_CAPTURE:      A node of type CAPTURE is a v4l2 video_device
+ *                              that exposes the received image from the
+ *                              pipeline to the user space
+ * @VIMC_ENT_NODE_INPUT:        A node of type INPUT is a v4l2 video_device that
+ *                              receives images from the user space and
+ *                              propagates them through the pipeline
+ * @VIMC_ENT_NODE_DEBAYER:      A node type DEBAYER expects to receive a frame
+ *                              in bayer format converts it to RGB
+ * @VIMC_ENT_NODE_SCALER:       A node of type SCALER scales the received image
+ *                              by a given multiplier
+ *
+ * This enum is used in the entity configuration struct to allow the definition
+ * of a custom topology specifying the role of each node on it.
+ */
+enum vimc_ent_node {
+        VIMC_ENT_NODE_SENSOR,
+        VIMC_ENT_NODE_CAPTURE,
+        VIMC_ENT_NODE_INPUT,
+        VIMC_ENT_NODE_DEBAYER,
+        VIMC_ENT_NODE_SCALER,
+};
+
+/* Structure which describes individual configuration for each entity */
+struct vimc_ent_config {
+        const char *name;
+        size_t pads_qty;
+        const unsigned long *pads_flag;
+        enum vimc_ent_node node;
+};
+
+/* Structure which describes links between entities */
+struct vimc_ent_link {
+        unsigned int src_ent;
+        u16 src_pad;
+        unsigned int sink_ent;
+        u16 sink_pad;
+        u32 flags;
+};
+
+/* Structure which describes the whole topology */
+struct vimc_pipeline_config {
+        const struct vimc_ent_config *ents;
+        size_t num_ents;
+        const struct vimc_ent_link *links;
+        size_t num_links;
+};
+
+/* --------------------------------------------------------------------------
+ * Topology Configuration
+ */
+
+static const struct vimc_ent_config ent_config[] = {
+        {
+                .name = "Sensor A",
+                .pads_qty = 1,
+                .pads_flag = (const unsigned long[]){MEDIA_PAD_FL_SOURCE},
+                .node = VIMC_ENT_NODE_SENSOR,
+        },
+        {
+                .name = "Sensor B",
+                .pads_qty = 1,
+                .pads_flag = (const unsigned long[]){MEDIA_PAD_FL_SOURCE},
+                .node = VIMC_ENT_NODE_SENSOR,
+        },
+        {
+                .name = "Debayer A",
+                .pads_qty = 2,
+                .pads_flag = (const unsigned long[]){MEDIA_PAD_FL_SINK,
+                                                     MEDIA_PAD_FL_SOURCE},
+                .node = VIMC_ENT_NODE_DEBAYER,
+        },
+        {
+                .name = "Debayer B",
+                .pads_qty = 2,
+                .pads_flag = (const unsigned long[]){MEDIA_PAD_FL_SINK,
+                                                     MEDIA_PAD_FL_SOURCE},
+                .node = VIMC_ENT_NODE_DEBAYER,
+        },
+        {
+                .name = "Raw Capture 0",
+                .pads_qty = 1,
+                .pads_flag = (const unsigned long[]){MEDIA_PAD_FL_SINK},
+                .node = VIMC_ENT_NODE_CAPTURE,
+        },
+        {
+                .name = "Raw Capture 1",
+                .pads_qty = 1,
+                .pads_flag = (const unsigned long[]){MEDIA_PAD_FL_SINK},
+                .node = VIMC_ENT_NODE_CAPTURE,
+        },
+        {
+                .name = "RGB/YUV Input",
+                .pads_qty = 1,
+                .pads_flag = (const unsigned long[]){MEDIA_PAD_FL_SOURCE},
+                .node = VIMC_ENT_NODE_INPUT,
+        },
+        {
+                .name = "Scaler",
+                .pads_qty = 2,
+                .pads_flag = (const unsigned long[]){MEDIA_PAD_FL_SINK,
+                                                     MEDIA_PAD_FL_SOURCE},
+                .node = VIMC_ENT_NODE_SCALER,
+        },
+        {
+                .name = "RGB/YUV Capture",
+                .pads_qty = 1,
+                .pads_flag = (const unsigned long[]){MEDIA_PAD_FL_SINK},
+                .node = VIMC_ENT_NODE_CAPTURE,
+        },
+};
+
+static const struct vimc_ent_link ent_links[] = {
+        /* Link: Sensor A (Pad 0)->(Pad 0) Debayer A */
+        VIMC_ENT_LINK(0, 0, 2, 0, MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
+        /* Link: Sensor A (Pad 0)->(Pad 0) Raw Capture 0 */
+        VIMC_ENT_LINK(0, 0, 4, 0, MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
+        /* Link: Sensor B (Pad 0)->(Pad 0) Debayer B */
+        VIMC_ENT_LINK(1, 0, 3, 0, MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
+        /* Link: Sensor B (Pad 0)->(Pad 0) Raw Capture 1 */
+        VIMC_ENT_LINK(1, 0, 5, 0, MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
+        /* Link: Debayer A (Pad 1)->(Pad 0) Scaler */
+        VIMC_ENT_LINK(2, 1, 7, 0, MEDIA_LNK_FL_ENABLED),
+        /* Link: Debayer B (Pad 1)->(Pad 0) Scaler */
+        VIMC_ENT_LINK(3, 1, 7, 0, 0),
+        /* Link: RGB/YUV Input (Pad 0)->(Pad 0) Scaler */
+        VIMC_ENT_LINK(6, 0, 7, 0, 0),
+        /* Link: Scaler (Pad 1)->(Pad 0) RGB/YUV Capture */
+        VIMC_ENT_LINK(7, 1, 8, 0, MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
+};
+
+static const struct vimc_pipeline_config pipe_cfg = {
+        .ents           = ent_config,
+        .num_ents       = ARRAY_SIZE(ent_config),
+        .links          = ent_links,
+        .num_links      = ARRAY_SIZE(ent_links)
+};
+
+/* -------------------------------------------------------------------------- */
+
+static const struct vimc_pix_map vimc_pix_map_list[] = {
+        /* TODO: add all missing formats */
+
+        /* RGB formats */
+        {
+                .code = MEDIA_BUS_FMT_BGR888_1X24,
+                .pixelformat = V4L2_PIX_FMT_BGR24,
+                .bpp = 3,
+        },
+        {
+                .code = MEDIA_BUS_FMT_RGB888_1X24,
+                .pixelformat = V4L2_PIX_FMT_RGB24,
+                .bpp = 3,
+        },
+        {
+                .code = MEDIA_BUS_FMT_ARGB8888_1X32,
+                .pixelformat = V4L2_PIX_FMT_ARGB32,
+                .bpp = 4,
+        },
+
+        /* Bayer formats */
+        {
+                .code = MEDIA_BUS_FMT_SBGGR8_1X8,
+                .pixelformat = V4L2_PIX_FMT_SBGGR8,
+                .bpp = 1,
+        },
+        {
+                .code = MEDIA_BUS_FMT_SGBRG8_1X8,
+                .pixelformat = V4L2_PIX_FMT_SGBRG8,
+                .bpp = 1,
+        },
+        {
+                .code = MEDIA_BUS_FMT_SGRBG8_1X8,
+                .pixelformat = V4L2_PIX_FMT_SGRBG8,
+                .bpp = 1,
+        },
+        {
+                .code = MEDIA_BUS_FMT_SRGGB8_1X8,
+                .pixelformat = V4L2_PIX_FMT_SRGGB8,
+                .bpp = 1,
+        },
+        {
+                .code = MEDIA_BUS_FMT_SBGGR10_1X10,
+                .pixelformat = V4L2_PIX_FMT_SBGGR10,
+                .bpp = 2,
+        },
+        {
+                .code = MEDIA_BUS_FMT_SGBRG10_1X10,
+                .pixelformat = V4L2_PIX_FMT_SGBRG10,
+                .bpp = 2,
+        },
+        {
+                .code = MEDIA_BUS_FMT_SGRBG10_1X10,
+                .pixelformat = V4L2_PIX_FMT_SGRBG10,
+                .bpp = 2,
+        },
+        {
+                .code = MEDIA_BUS_FMT_SRGGB10_1X10,
+                .pixelformat = V4L2_PIX_FMT_SRGGB10,
+                .bpp = 2,
+        },
+
+        /* 10bit raw bayer a-law compressed to 8 bits */
+        {
+                .code = MEDIA_BUS_FMT_SBGGR10_ALAW8_1X8,
+                .pixelformat = V4L2_PIX_FMT_SBGGR10ALAW8,
+                .bpp = 1,
+        },
+        {
+                .code = MEDIA_BUS_FMT_SGBRG10_ALAW8_1X8,
+                .pixelformat = V4L2_PIX_FMT_SGBRG10ALAW8,
+                .bpp = 1,
+        },
+        {
+                .code = MEDIA_BUS_FMT_SGRBG10_ALAW8_1X8,
+                .pixelformat = V4L2_PIX_FMT_SGRBG10ALAW8,
+                .bpp = 1,
+        },
+        {
+                .code = MEDIA_BUS_FMT_SRGGB10_ALAW8_1X8,
+                .pixelformat = V4L2_PIX_FMT_SRGGB10ALAW8,
+                .bpp = 1,
+        },
+
+        /* 10bit raw bayer DPCM compressed to 8 bits */
+        {
+                .code = MEDIA_BUS_FMT_SBGGR10_DPCM8_1X8,
+                .pixelformat = V4L2_PIX_FMT_SBGGR10DPCM8,
+                .bpp = 1,
+        },
+        {
+                .code = MEDIA_BUS_FMT_SGBRG10_DPCM8_1X8,
+                .pixelformat = V4L2_PIX_FMT_SGBRG10DPCM8,
+                .bpp = 1,
+        },
+        {
+                .code = MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8,
+                .pixelformat = V4L2_PIX_FMT_SGRBG10DPCM8,
+                .bpp = 1,
+        },
+        {
+                .code = MEDIA_BUS_FMT_SRGGB10_DPCM8_1X8,
+                .pixelformat = V4L2_PIX_FMT_SRGGB10DPCM8,
+                .bpp = 1,
+        },
+        {
+                .code = MEDIA_BUS_FMT_SBGGR12_1X12,
+                .pixelformat = V4L2_PIX_FMT_SBGGR12,
+                .bpp = 2,
+        },
+        {
+                .code = MEDIA_BUS_FMT_SGBRG12_1X12,
+                .pixelformat = V4L2_PIX_FMT_SGBRG12,
+                .bpp = 2,
+        },
+        {
+                .code = MEDIA_BUS_FMT_SGRBG12_1X12,
+                .pixelformat = V4L2_PIX_FMT_SGRBG12,
+                .bpp = 2,
+        },
+        {
+                .code = MEDIA_BUS_FMT_SRGGB12_1X12,
+                .pixelformat = V4L2_PIX_FMT_SRGGB12,
+                .bpp = 2,
+        },
+};
+
+const struct vimc_pix_map *vimc_pix_map_by_code(u32 code)
+{
+        unsigned int i;
+
+        for (i = 0; i < ARRAY_SIZE(vimc_pix_map_list); i++) {
+                if (vimc_pix_map_list[i].code == code)
+                        return &vimc_pix_map_list[i];
+        }
+        return NULL;
+}
+
+const struct vimc_pix_map *vimc_pix_map_by_pixelformat(u32 pixelformat)
+{
+        unsigned int i;
+
+        for (i = 0; i < ARRAY_SIZE(vimc_pix_map_list); i++) {
+                if (vimc_pix_map_list[i].pixelformat == pixelformat)
+                        return &vimc_pix_map_list[i];
+        }
+        return NULL;
+}
+
+int vimc_propagate_frame(struct media_pad *src, const void *frame)
+{
+        struct media_link *link;
+
+        if (!(src->flags & MEDIA_PAD_FL_SOURCE))
+                return -EINVAL;
+
+        /* Send this frame to all sink pads that are direct linked */
+        list_for_each_entry(link, &src->entity->links, list) {
+                if (link->source == src &&
+                    (link->flags & MEDIA_LNK_FL_ENABLED)) {
+                        struct vimc_ent_device *ved = NULL;
+                        struct media_entity *entity = link->sink->entity;
+
+                        if (is_media_entity_v4l2_subdev(entity)) {
+                                struct v4l2_subdev *sd =
+                                        container_of(entity, struct v4l2_subdev,
+                                                     entity);
+                                ved = v4l2_get_subdevdata(sd);
+                        } else if (is_media_entity_v4l2_video_device(entity)) {
+                                struct video_device *vdev =
+                                        container_of(entity,
+                                                     struct video_device,
+                                                     entity);
+                                ved = video_get_drvdata(vdev);
+                        }
+                        if (ved && ved->process_frame)
+                                ved->process_frame(ved, link->sink, frame);
+                }
+        }
+
+        return 0;
+}
+
+static void vimc_device_unregister(struct vimc_device *vimc)
+{
+        unsigned int i;
+
+        media_device_unregister(&vimc->mdev);
+        /* Cleanup (only initialized) entities */
+        for (i = 0; i < vimc->pipe_cfg->num_ents; i++) {
+                if (vimc->ved[i] && vimc->ved[i]->destroy)
+                        vimc->ved[i]->destroy(vimc->ved[i]);
+
+                vimc->ved[i] = NULL;
+        }
+        v4l2_device_unregister(&vimc->v4l2_dev);
+        media_device_cleanup(&vimc->mdev);
+}
+
+/* Helper function to allocate and initialize pads */
+struct media_pad *vimc_pads_init(u16 num_pads, const unsigned long *pads_flag)
+{
+        struct media_pad *pads;
+        unsigned int i;
+
+        /* Allocate memory for the pads */
+        pads = kcalloc(num_pads, sizeof(*pads), GFP_KERNEL);
+        if (!pads)
+                return ERR_PTR(-ENOMEM);
+
+        /* Initialize the pads */
+        for (i = 0; i < num_pads; i++) {
+                pads[i].index = i;
+                pads[i].flags = pads_flag[i];
+        }
+
+        return pads;
+}
+
+/*
+ * TODO: remove this function when all the
+ * entities specific code are implemented
+ */
+static void vimc_raw_destroy(struct vimc_ent_device *ved)
+{
+        media_device_unregister_entity(ved->ent);
+
+        media_entity_cleanup(ved->ent);
+
+        vimc_pads_cleanup(ved->pads);
+
+        kfree(ved->ent);
+
+        kfree(ved);
+}
+
+/*
+ * TODO: remove this function when all the
+ * entities specific code are implemented
+ */
+static struct vimc_ent_device *vimc_raw_create(struct v4l2_device *v4l2_dev,
+                                               const char *const name,
+                                               u16 num_pads,
+                                               const unsigned long *pads_flag)
+{
+        struct vimc_ent_device *ved;
+        int ret;
+
+        /* Allocate the main ved struct */
+        ved = kzalloc(sizeof(*ved), GFP_KERNEL);
+        if (!ved)
+                return ERR_PTR(-ENOMEM);
+
+        /* Allocate the media entity */
+        ved->ent = kzalloc(sizeof(*ved->ent), GFP_KERNEL);
+        if (!ved->ent) {
+                ret = -ENOMEM;
+                goto err_free_ved;
+        }
+
+        /* Allocate the pads */
+        ved->pads = vimc_pads_init(num_pads, pads_flag);
+        if (IS_ERR(ved->pads)) {
+                ret = PTR_ERR(ved->pads);
+                goto err_free_ent;
+        }
+
+        /* Initialize the media entity */
+        ved->ent->name = name;
+        ved->ent->function = MEDIA_ENT_F_V4L2_SUBDEV_UNKNOWN;
+        ret = media_entity_pads_init(ved->ent, num_pads, ved->pads);
+        if (ret)
+                goto err_cleanup_pads;
+
+        /* Register the media entity */
+        ret = media_device_register_entity(v4l2_dev->mdev, ved->ent);
+        if (ret)
+                goto err_cleanup_entity;
+
+        /* Fill out the destroy function and return */
+        ved->destroy = vimc_raw_destroy;
+        return ved;
+
+err_cleanup_entity:
+        media_entity_cleanup(ved->ent);
+err_cleanup_pads:
+        vimc_pads_cleanup(ved->pads);
+err_free_ent:
+        kfree(ved->ent);
+err_free_ved:
+        kfree(ved);
+
+        return ERR_PTR(ret);
+}
+
+static int vimc_device_register(struct vimc_device *vimc)
+{
+        unsigned int i;
+        int ret;
+
+        /* Allocate memory for the vimc_ent_devices pointers */
+        vimc->ved = devm_kcalloc(vimc->mdev.dev, vimc->pipe_cfg->num_ents,
+                                 sizeof(*vimc->ved), GFP_KERNEL);
+        if (!vimc->ved)
+                return -ENOMEM;
+
+        /* Link the media device within the v4l2_device */
+        vimc->v4l2_dev.mdev = &vimc->mdev;
+
+        /* Register the v4l2 struct */
+        ret = v4l2_device_register(vimc->mdev.dev, &vimc->v4l2_dev);
+        if (ret) {
+                dev_err(vimc->mdev.dev,
+                        "v4l2 device register failed (err=%d)\n", ret);
+                return ret;
+        }
+
+        /* Initialize entities */
+        for (i = 0; i < vimc->pipe_cfg->num_ents; i++) {
+                struct vimc_ent_device *(*create_func)(struct v4l2_device *,
+                                                       const char *const,
+                                                       u16,
+                                                       const unsigned long *);
+
+                /* Register the specific node */
+                switch (vimc->pipe_cfg->ents[i].node) {
+                case VIMC_ENT_NODE_SENSOR:
+                        create_func = vimc_sen_create;
+                        break;
+
+                case VIMC_ENT_NODE_CAPTURE:
+                        create_func = vimc_cap_create;
+                        break;
+
+                /* TODO: Instantiate the specific topology node */
+                case VIMC_ENT_NODE_INPUT:
+                case VIMC_ENT_NODE_DEBAYER:
+                case VIMC_ENT_NODE_SCALER:
+                default:
+                        /*
+                         * TODO: remove this when all the entities specific
+                         * code are implemented
+                         */
+                        create_func = vimc_raw_create;
+                        break;
+                }
+
+                vimc->ved[i] = create_func(&vimc->v4l2_dev,
+                                           vimc->pipe_cfg->ents[i].name,
+                                           vimc->pipe_cfg->ents[i].pads_qty,
+                                           vimc->pipe_cfg->ents[i].pads_flag);
+                if (IS_ERR(vimc->ved[i])) {
+                        ret = PTR_ERR(vimc->ved[i]);
+                        vimc->ved[i] = NULL;
+                        goto err;
+                }
+        }
+
+        /* Initialize the links between entities */
+        for (i = 0; i < vimc->pipe_cfg->num_links; i++) {
+                const struct vimc_ent_link *link = &vimc->pipe_cfg->links[i];
+
+                ret = media_create_pad_link(vimc->ved[link->src_ent]->ent,
+                                            link->src_pad,
+                                            vimc->ved[link->sink_ent]->ent,
+                                            link->sink_pad,
+                                            link->flags);
+                if (ret)
+                        goto err;
+        }
+
+        /* Register the media device */
+        ret = media_device_register(&vimc->mdev);
+        if (ret) {
+                dev_err(vimc->mdev.dev,
+                        "media device register failed (err=%d)\n", ret);
+                return ret;
+        }
+
+        /* Expose all subdev's nodes*/
+        ret = v4l2_device_register_subdev_nodes(&vimc->v4l2_dev);
+        if (ret) {
+                dev_err(vimc->mdev.dev,
+                        "vimc subdev nodes registration failed (err=%d)\n",
+                        ret);
+                goto err;
+        }
+
+        return 0;
+
+err:
+        /* Destroy the so far created topology */
+        vimc_device_unregister(vimc);
+
+        return ret;
+}
+
+static int vimc_probe(struct platform_device *pdev)
+{
+        struct vimc_device *vimc;
+        int ret;
+
+        /* Prepare the vimc topology structure */
+
+        /* Allocate memory for the vimc structure */
+        vimc = kzalloc(sizeof(*vimc), GFP_KERNEL);
+        if (!vimc)
+                return -ENOMEM;
+
+        /* Set the pipeline configuration struct */
+        vimc->pipe_cfg = &pipe_cfg;
+
+        /* Initialize media device */
+        strlcpy(vimc->mdev.model, VIMC_MDEV_MODEL_NAME,
+                sizeof(vimc->mdev.model));
+        vimc->mdev.dev = &pdev->dev;
+        media_device_init(&vimc->mdev);
+
+        /* Create vimc topology */
+        ret = vimc_device_register(vimc);
+        if (ret) {
+                dev_err(vimc->mdev.dev,
+                        "vimc device registration failed (err=%d)\n", ret);
+                kfree(vimc);
+                return ret;
+        }
+
+        /* Link the topology object with the platform device object */
+        platform_set_drvdata(pdev, vimc);
+
+        return 0;
+}
+
+static int vimc_remove(struct platform_device *pdev)
+{
+        struct vimc_device *vimc = platform_get_drvdata(pdev);
+
+        /* Destroy all the topology */
+        vimc_device_unregister(vimc);
+        kfree(vimc);
+
+        return 0;
+}
+
+static void vimc_dev_release(struct device *dev)
+{
+}
+
+static struct platform_device vimc_pdev = {
+        .name           = VIMC_PDEV_NAME,
+        .dev.release    = vimc_dev_release,
+};
+
+static struct platform_driver vimc_pdrv = {
+        .probe          = vimc_probe,
+        .remove         = vimc_remove,
+        .driver         = {
+                .name   = VIMC_PDEV_NAME,
+        },
+};
+
+static int __init vimc_init(void)
+{
+        int ret;
+
+        ret = platform_device_register(&vimc_pdev);
+        if (ret) {
+                dev_err(&vimc_pdev.dev,
+                        "platform device registration failed (err=%d)\n", ret);
+                return ret;
+        }
+
+        ret = platform_driver_register(&vimc_pdrv);
+        if (ret) {
+                dev_err(&vimc_pdev.dev,
+                        "platform driver registration failed (err=%d)\n", ret);
+
+                platform_device_unregister(&vimc_pdev);
+        }
+
+        return ret;
+}
+
+static void __exit vimc_exit(void)
+{
+        platform_driver_unregister(&vimc_pdrv);
+
+        platform_device_unregister(&vimc_pdev);
+}
+
+module_init(vimc_init);
+module_exit(vimc_exit);
+
+MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC)");
+MODULE_AUTHOR("Helen Fornazier <helen.fornazier@gmail.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/vimc/vimc-core.h b/drivers/media/platform/vimc/vimc-core.h
new file mode 100644
index 000000000000..4525d23211ca
--- /dev/null
+++ b/drivers/media/platform/vimc/vimc-core.h
@@ -0,0 +1,112 @@
+/*
+ * vimc-core.h Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _VIMC_CORE_H_
+#define _VIMC_CORE_H_
+
+#include <linux/slab.h>
+#include <media/v4l2-device.h>
+
+/**
+ * struct vimc_pix_map - maps media bus code with v4l2 pixel format
+ *
+ * @code:               media bus format code defined by MEDIA_BUS_FMT_* macros
+ * @bbp:                number of bytes each pixel occupies
+ * @pixelformat:        pixel format devined by V4L2_PIX_FMT_* macros
+ *
+ * Struct which matches the MEDIA_BUS_FMT_* codes with the corresponding
+ * V4L2_PIX_FMT_* fourcc pixelformat and its bytes per pixel (bpp)
+ */
+struct vimc_pix_map {
+        unsigned int code;
+        unsigned int bpp;
+        u32 pixelformat;
+};
+
+/**
+ * struct vimc_ent_device - core struct that represents a node in the topology
+ *
+ * @ent:                the pointer to struct media_entity for the node
+ * @pads:               the list of pads of the node
+ * @destroy:            callback to destroy the node
+ * @process_frame:      callback send a frame to that node
+ *
+ * Each node of the topology must create a vimc_ent_device struct. Depending on
+ * the node it will be of an instance of v4l2_subdev or video_device struct
+ * where both contains a struct media_entity.
+ * Those structures should embedded the vimc_ent_device struct through
+ * v4l2_set_subdevdata() and video_set_drvdata() respectivaly, allowing the
+ * vimc_ent_device struct to be retrieved from the corresponding struct
+ * media_entity
+ */
+struct vimc_ent_device {
+        struct media_entity *ent;
+        struct media_pad *pads;
+        void (*destroy)(struct vimc_ent_device *);
+        void (*process_frame)(struct vimc_ent_device *ved,
+                              struct media_pad *sink, const void *frame);
+};
+
+/**
+ * vimc_propagate_frame - propagate a frame through the topology
+ *
+ * @src:        the source pad where the frame is being originated
+ * @frame:      the frame to be propagated
+ *
+ * This function will call the process_frame callback from the vimc_ent_device
+ * struct of the nodes directly connected to the @src pad
+ */
+int vimc_propagate_frame(struct media_pad *src, const void *frame);
+
+/**
+ * vimc_pads_init - initialize pads
+ *
+ * @num_pads:   number of pads to initialize
+ * @pads_flags: flags to use in each pad
+ *
+ * Helper functions to allocate/initialize pads
+ */
+struct media_pad *vimc_pads_init(u16 num_pads,
+                                 const unsigned long *pads_flag);
+
+/**
+ * vimc_pads_cleanup - free pads
+ *
+ * @pads: pointer to the pads
+ *
+ * Helper function to free the pads initialized with vimc_pads_init
+ */
+static inline void vimc_pads_cleanup(struct media_pad *pads)
+{
+        kfree(pads);
+}
+
+/**
+ * vimc_pix_map_by_code - get vimc_pix_map struct by media bus code
+ *
+ * @code:               media bus format code defined by MEDIA_BUS_FMT_* macros
+ */
+const struct vimc_pix_map *vimc_pix_map_by_code(u32 code);
+
+/**
+ * vimc_pix_map_by_pixelformat - get vimc_pix_map struct by v4l2 pixel format
+ *
+ * @pixelformat:        pixel format devined by V4L2_PIX_FMT_* macros
+ */
+const struct vimc_pix_map *vimc_pix_map_by_pixelformat(u32 pixelformat);
+
+#endif
diff --git a/drivers/media/platform/vimc/vimc-sensor.c b/drivers/media/platform/vimc/vimc-sensor.c
new file mode 100644
index 000000000000..591f6a4f8bd3
--- /dev/null
+++ b/drivers/media/platform/vimc/vimc-sensor.c
@@ -0,0 +1,276 @@
+/*
+ * vimc-sensor.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/freezer.h>
+#include <linux/kthread.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/vmalloc.h>
+#include <media/v4l2-subdev.h>
+
+#include "vimc-sensor.h"
+
+struct vimc_sen_device {
+        struct vimc_ent_device ved;
+        struct v4l2_subdev sd;
+        struct task_struct *kthread_sen;
+        u8 *frame;
+        /* The active format */
+        struct v4l2_mbus_framefmt mbus_format;
+        int frame_size;
+};
+
+static int vimc_sen_enum_mbus_code(struct v4l2_subdev *sd,
+                                   struct v4l2_subdev_pad_config *cfg,
+                                   struct v4l2_subdev_mbus_code_enum *code)
+{
+        struct vimc_sen_device *vsen =
+                                container_of(sd, struct vimc_sen_device, sd);
+
+        /* TODO: Add support for other codes */
+        if (code->index)
+                return -EINVAL;
+
+        code->code = vsen->mbus_format.code;
+
+        return 0;
+}
+
+static int vimc_sen_enum_frame_size(struct v4l2_subdev *sd,
+                                    struct v4l2_subdev_pad_config *cfg,
+                                    struct v4l2_subdev_frame_size_enum *fse)
+{
+        struct vimc_sen_device *vsen =
+                                container_of(sd, struct vimc_sen_device, sd);
+
+        /* TODO: Add support to other formats */
+        if (fse->index)
+                return -EINVAL;
+
+        /* TODO: Add support for other codes */
+        if (fse->code != vsen->mbus_format.code)
+                return -EINVAL;
+
+        fse->min_width = vsen->mbus_format.width;
+        fse->max_width = vsen->mbus_format.width;
+        fse->min_height = vsen->mbus_format.height;
+        fse->max_height = vsen->mbus_format.height;
+
+        return 0;
+}
+
+static int vimc_sen_get_fmt(struct v4l2_subdev *sd,
+                            struct v4l2_subdev_pad_config *cfg,
+                            struct v4l2_subdev_format *format)
+{
+        struct vimc_sen_device *vsen =
+                                container_of(sd, struct vimc_sen_device, sd);
+
+        format->format = vsen->mbus_format;
+
+        return 0;
+}
+
+static const struct v4l2_subdev_pad_ops vimc_sen_pad_ops = {
+        .enum_mbus_code         = vimc_sen_enum_mbus_code,
+        .enum_frame_size        = vimc_sen_enum_frame_size,
+        .get_fmt                = vimc_sen_get_fmt,
+        /* TODO: Add support to other formats */
+        .set_fmt                = vimc_sen_get_fmt,
+};
+
+/* media operations */
+static const struct media_entity_operations vimc_sen_mops = {
+        .link_validate = v4l2_subdev_link_validate,
+};
+
+static int vimc_thread_sen(void *data)
+{
+        struct vimc_sen_device *vsen = data;
+        unsigned int i;
+
+        set_freezable();
+        set_current_state(TASK_UNINTERRUPTIBLE);
+
+        for (;;) {
+                try_to_freeze();
+                if (kthread_should_stop())
+                        break;
+
+                memset(vsen->frame, 100, vsen->frame_size);
+
+                /* Send the frame to all source pads */
+                for (i = 0; i < vsen->sd.entity.num_pads; i++)
+                        vimc_propagate_frame(&vsen->sd.entity.pads[i],
+                                             vsen->frame);
+
+                /* 60 frames per second */
+                schedule_timeout(HZ/60);
+        }
+
+        return 0;
+}
+
+static int vimc_sen_s_stream(struct v4l2_subdev *sd, int enable)
+{
+        struct vimc_sen_device *vsen =
+                                container_of(sd, struct vimc_sen_device, sd);
+        int ret;
+
+        if (enable) {
+                const struct vimc_pix_map *vpix;
+
+                if (vsen->kthread_sen)
+                        return -EINVAL;
+
+                /* Calculate the frame size */
+                vpix = vimc_pix_map_by_code(vsen->mbus_format.code);
+                vsen->frame_size = vsen->mbus_format.width * vpix->bpp *
+                                   vsen->mbus_format.height;
+
+                /*
+                 * Allocate the frame buffer. Use vmalloc to be able to
+                 * allocate a large amount of memory
+                 */
+                vsen->frame = vmalloc(vsen->frame_size);
+                if (!vsen->frame)
+                        return -ENOMEM;
+
+                /* Initialize the image generator thread */
+                vsen->kthread_sen = kthread_run(vimc_thread_sen, vsen, "%s-sen",
+                                                vsen->sd.v4l2_dev->name);
+                if (IS_ERR(vsen->kthread_sen)) {
+                        dev_err(vsen->sd.v4l2_dev->dev,
+                                "%s: kernel_thread() failed\n", vsen->sd.name);
+                        vfree(vsen->frame);
+                        vsen->frame = NULL;
+                        return PTR_ERR(vsen->kthread_sen);
+                }
+        } else {
+                if (!vsen->kthread_sen)
+                        return -EINVAL;
+
+                /* Stop image generator */
+                ret = kthread_stop(vsen->kthread_sen);
+                vsen->kthread_sen = NULL;
+
+                vfree(vsen->frame);
+                vsen->frame = NULL;
+                return ret;
+        }
+
+        return 0;
+}
+
+struct v4l2_subdev_video_ops vimc_sen_video_ops = {
+        .s_stream = vimc_sen_s_stream,
+};
+
+static const struct v4l2_subdev_ops vimc_sen_ops = {
+        .pad = &vimc_sen_pad_ops,
+        .video = &vimc_sen_video_ops,
+};
+
+static void vimc_sen_destroy(struct vimc_ent_device *ved)
+{
+        struct vimc_sen_device *vsen =
+                                container_of(ved, struct vimc_sen_device, ved);
+
+        v4l2_device_unregister_subdev(&vsen->sd);
+        media_entity_cleanup(ved->ent);
+        kfree(vsen);
+}
+
+struct vimc_ent_device *vimc_sen_create(struct v4l2_device *v4l2_dev,
+                                        const char *const name,
+                                        u16 num_pads,
+                                        const unsigned long *pads_flag)
+{
+        struct vimc_sen_device *vsen;
+        unsigned int i;
+        int ret;
+
+        /* NOTE: a sensor node may be created with more then one pad */
+        if (!name || !num_pads || !pads_flag)
+                return ERR_PTR(-EINVAL);
+
+        /* check if all pads are sources */
+        for (i = 0; i < num_pads; i++)
+                if (!(pads_flag[i] & MEDIA_PAD_FL_SOURCE))
+                        return ERR_PTR(-EINVAL);
+
+        /* Allocate the vsen struct */
+        vsen = kzalloc(sizeof(*vsen), GFP_KERNEL);
+        if (!vsen)
+                return ERR_PTR(-ENOMEM);
+
+        /* Allocate the pads */
+        vsen->ved.pads = vimc_pads_init(num_pads, pads_flag);
+        if (IS_ERR(vsen->ved.pads)) {
+                ret = PTR_ERR(vsen->ved.pads);
+                goto err_free_vsen;
+        }
+
+        /* Fill the vimc_ent_device struct */
+        vsen->ved.destroy = vimc_sen_destroy;
+        vsen->ved.ent = &vsen->sd.entity;
+
+        /* Initialize the subdev */
+        v4l2_subdev_init(&vsen->sd, &vimc_sen_ops);
+        vsen->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+        vsen->sd.entity.ops = &vimc_sen_mops;
+        vsen->sd.owner = THIS_MODULE;
+        strlcpy(vsen->sd.name, name, sizeof(vsen->sd.name));
+        v4l2_set_subdevdata(&vsen->sd, &vsen->ved);
+
+        /* Expose this subdev to user space */
+        vsen->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+        /* Initialize the media entity */
+        ret = media_entity_pads_init(&vsen->sd.entity,
+                                     num_pads, vsen->ved.pads);
+        if (ret)
+                goto err_clean_pads;
+
+        /* Set the active frame format (this is hardcoded for now) */
+        vsen->mbus_format.width = 640;
+        vsen->mbus_format.height = 480;
+        vsen->mbus_format.code = MEDIA_BUS_FMT_RGB888_1X24;
+        vsen->mbus_format.field = V4L2_FIELD_NONE;
+        vsen->mbus_format.colorspace = V4L2_COLORSPACE_SRGB;
+        vsen->mbus_format.quantization = V4L2_QUANTIZATION_FULL_RANGE;
+        vsen->mbus_format.xfer_func = V4L2_XFER_FUNC_SRGB;
+
+        /* Register the subdev with the v4l2 and the media framework */
+        ret = v4l2_device_register_subdev(v4l2_dev, &vsen->sd);
+        if (ret) {
+                dev_err(vsen->sd.v4l2_dev->dev,
+                        "%s: subdev register failed (err=%d)\n",
+                        vsen->sd.name, ret);
+                goto err_clean_m_ent;
+        }
+
+        return &vsen->ved;
+
+err_clean_m_ent:
+        media_entity_cleanup(&vsen->sd.entity);
+err_clean_pads:
+        vimc_pads_cleanup(vsen->ved.pads);
+err_free_vsen:
+        kfree(vsen);
+
+        return ERR_PTR(ret);
+}
diff --git a/drivers/media/platform/vimc/vimc-sensor.h b/drivers/media/platform/vimc/vimc-sensor.h
new file mode 100644
index 000000000000..505310e8aeb7
--- /dev/null
+++ b/drivers/media/platform/vimc/vimc-sensor.h
@@ -0,0 +1,28 @@
+/*
+ * vimc-sensor.h Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _VIMC_SENSOR_H_
+#define _VIMC_SENSOR_H_
+
+#include "vimc-core.h"
+
+struct vimc_ent_device *vimc_sen_create(struct v4l2_device *v4l2_dev,
+                                        const char *const name,
+                                        u16 num_pads,
+                                        const unsigned long *pads_flag);
+
+#endif
diff --git a/drivers/media/platform/vivid/Kconfig b/drivers/media/platform/vivid/Kconfig
index db0dd19d227a..b36ac19dc6e4 100644
--- a/drivers/media/platform/vivid/Kconfig
+++ b/drivers/media/platform/vivid/Kconfig
@@ -1,13 +1,14 @@
 config VIDEO_VIVID
         tristate "Virtual Video Test Driver"
         depends on VIDEO_DEV && VIDEO_V4L2 && !SPARC32 && !SPARC64 && FB
+        depends on HAS_DMA
         select FONT_SUPPORT
         select FONT_8x16
         select FB_CFB_FILLRECT
         select FB_CFB_COPYAREA
         select FB_CFB_IMAGEBLIT
-        select MEDIA_CEC_EDID
         select VIDEOBUF2_VMALLOC
+        select VIDEOBUF2_DMA_CONTIG
         select VIDEO_V4L2_TPG
         default n
         ---help---
@@ -25,7 +26,7 @@ config VIDEO_VIVID
 
 config VIDEO_VIVID_CEC
         bool "Enable CEC emulation support"
-        depends on VIDEO_VIVID && MEDIA_CEC_SUPPORT
+        depends on VIDEO_VIVID && CEC_CORE
         ---help---
           When selected the vivid module will emulate the optional
           HDMI CEC feature.
diff --git a/drivers/media/platform/vivid/vivid-cec.c b/drivers/media/platform/vivid/vivid-cec.c
index cb4933592a3c..653f4099f737 100644
--- a/drivers/media/platform/vivid/vivid-cec.c
+++ b/drivers/media/platform/vivid/vivid-cec.c
@@ -135,7 +135,7 @@ static int vivid_cec_adap_log_addr(struct cec_adapter *adap, u8 log_addr)
 static int vivid_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
                                    u32 signal_free_time, struct cec_msg *msg)
 {
-        struct vivid_dev *dev = adap->priv;
+        struct vivid_dev *dev = cec_get_drvdata(adap);
         struct vivid_cec_work *cw = kzalloc(sizeof(*cw), GFP_KERNEL);
         long delta_jiffies = 0;
 
@@ -166,7 +166,7 @@ static int vivid_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
 
 static int vivid_received(struct cec_adapter *adap, struct cec_msg *msg)
 {
-        struct vivid_dev *dev = adap->priv;
+        struct vivid_dev *dev = cec_get_drvdata(adap);
         struct cec_msg reply;
         u8 dest = cec_msg_destination(msg);
         u8 disp_ctl;
diff --git a/drivers/media/platform/vivid/vivid-core.c b/drivers/media/platform/vivid/vivid-core.c
index 51e37812ec98..ef344b9a48af 100644
--- a/drivers/media/platform/vivid/vivid-core.c
+++ b/drivers/media/platform/vivid/vivid-core.c
@@ -30,6 +30,7 @@
 #include <linux/videodev2.h>
 #include <linux/v4l2-dv-timings.h>
 #include <media/videobuf2-vmalloc.h>
+#include <media/videobuf2-dma-contig.h>
 #include <media/v4l2-dv-timings.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-fh.h>
@@ -151,6 +152,12 @@ static bool no_error_inj;
 module_param(no_error_inj, bool, 0444);
 MODULE_PARM_DESC(no_error_inj, " if set disable the error injecting controls");
 
+static unsigned int allocators[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = 0 };
+module_param_array(allocators, uint, NULL, 0444);
+MODULE_PARM_DESC(allocators, " memory allocator selection, default is 0.\n"
+                             "\t\t    0 == vmalloc\n"
+                             "\t\t    1 == dma-contig");
+
 static struct vivid_dev *vivid_devs[VIVID_MAX_DEVS];
 
 const struct v4l2_rect vivid_min_rect = {
@@ -636,6 +643,10 @@ static int vivid_create_instance(struct platform_device *pdev, int inst)
 {
         static const struct v4l2_dv_timings def_dv_timings =
                                         V4L2_DV_BT_CEA_1280X720P60;
+        static const struct vb2_mem_ops * const vivid_mem_ops[2] = {
+                &vb2_vmalloc_memops,
+                &vb2_dma_contig_memops,
+        };
         unsigned in_type_counter[4] = { 0, 0, 0, 0 };
         unsigned out_type_counter[4] = { 0, 0, 0, 0 };
         int ccs_cap = ccs_cap_mode[inst];
@@ -646,6 +657,7 @@ static int vivid_create_instance(struct platform_device *pdev, int inst)
         struct video_device *vfd;
         struct vb2_queue *q;
         unsigned node_type = node_types[inst];
+        unsigned int allocator = allocators[inst];
         v4l2_std_id tvnorms_cap = 0, tvnorms_out = 0;
         int ret;
         int i;
@@ -1039,6 +1051,11 @@ static int vivid_create_instance(struct platform_device *pdev, int inst)
                 goto unreg_dev;
         }
 
+        if (allocator == 1)
+                dma_coerce_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
+        else if (allocator >= ARRAY_SIZE(vivid_mem_ops))
+                allocator = 0;
+
         /* start creating the vb2 queues */
         if (dev->has_vid_cap) {
                 /* initialize vid_cap queue */
@@ -1049,10 +1066,11 @@ static int vivid_create_instance(struct platform_device *pdev, int inst)
                 q->drv_priv = dev;
                 q->buf_struct_size = sizeof(struct vivid_buffer);
                 q->ops = &vivid_vid_cap_qops;
-                q->mem_ops = &vb2_vmalloc_memops;
+                q->mem_ops = vivid_mem_ops[allocator];
                 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
                 q->min_buffers_needed = 2;
                 q->lock = &dev->mutex;
+                q->dev = dev->v4l2_dev.dev;
 
                 ret = vb2_queue_init(q);
                 if (ret)
@@ -1068,10 +1086,11 @@ static int vivid_create_instance(struct platform_device *pdev, int inst)
                 q->drv_priv = dev;
                 q->buf_struct_size = sizeof(struct vivid_buffer);
                 q->ops = &vivid_vid_out_qops;
-                q->mem_ops = &vb2_vmalloc_memops;
+                q->mem_ops = vivid_mem_ops[allocator];
                 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
                 q->min_buffers_needed = 2;
                 q->lock = &dev->mutex;
+                q->dev = dev->v4l2_dev.dev;
 
                 ret = vb2_queue_init(q);
                 if (ret)
@@ -1087,10 +1106,11 @@ static int vivid_create_instance(struct platform_device *pdev, int inst)
                 q->drv_priv = dev;
                 q->buf_struct_size = sizeof(struct vivid_buffer);
                 q->ops = &vivid_vbi_cap_qops;
-                q->mem_ops = &vb2_vmalloc_memops;
+                q->mem_ops = vivid_mem_ops[allocator];
                 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
                 q->min_buffers_needed = 2;
                 q->lock = &dev->mutex;
+                q->dev = dev->v4l2_dev.dev;
 
                 ret = vb2_queue_init(q);
                 if (ret)
@@ -1106,10 +1126,11 @@ static int vivid_create_instance(struct platform_device *pdev, int inst)
                 q->drv_priv = dev;
                 q->buf_struct_size = sizeof(struct vivid_buffer);
                 q->ops = &vivid_vbi_out_qops;
-                q->mem_ops = &vb2_vmalloc_memops;
+                q->mem_ops = vivid_mem_ops[allocator];
                 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
                 q->min_buffers_needed = 2;
                 q->lock = &dev->mutex;
+                q->dev = dev->v4l2_dev.dev;
 
                 ret = vb2_queue_init(q);
                 if (ret)
@@ -1124,10 +1145,11 @@ static int vivid_create_instance(struct platform_device *pdev, int inst)
                 q->drv_priv = dev;
                 q->buf_struct_size = sizeof(struct vivid_buffer);
                 q->ops = &vivid_sdr_cap_qops;
-                q->mem_ops = &vb2_vmalloc_memops;
+                q->mem_ops = vivid_mem_ops[allocator];
                 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
                 q->min_buffers_needed = 8;
                 q->lock = &dev->mutex;
+                q->dev = dev->v4l2_dev.dev;
 
                 ret = vb2_queue_init(q);
                 if (ret)
diff --git a/drivers/media/platform/vivid/vivid-vid-cap.c b/drivers/media/platform/vivid/vivid-vid-cap.c
index a18e6fec219b..01419455e545 100644
--- a/drivers/media/platform/vivid/vivid-vid-cap.c
+++ b/drivers/media/platform/vivid/vivid-vid-cap.c
@@ -616,7 +616,7 @@ int vivid_try_fmt_vid_cap(struct file *file, void *priv,
         /* This driver supports custom bytesperline values */
 
         mp->num_planes = fmt->buffers;
-        for (p = 0; p < mp->num_planes; p++) {
+        for (p = 0; p < fmt->buffers; p++) {
                 /* Calculate the minimum supported bytesperline value */
                 bytesperline = (mp->width * fmt->bit_depth[p]) >> 3;
                 /* Calculate the maximum supported bytesperline value */
@@ -626,10 +626,17 @@ int vivid_try_fmt_vid_cap(struct file *file, void *priv,
                         pfmt[p].bytesperline = max_bpl;
                 if (pfmt[p].bytesperline < bytesperline)
                         pfmt[p].bytesperline = bytesperline;
-                pfmt[p].sizeimage = tpg_calc_line_width(&dev->tpg, p, pfmt[p].bytesperline) *
-                        mp->height + fmt->data_offset[p];
+
+                pfmt[p].sizeimage = (pfmt[p].bytesperline * mp->height) /
+                                fmt->vdownsampling[p] + fmt->data_offset[p];
+
                 memset(pfmt[p].reserved, 0, sizeof(pfmt[p].reserved));
         }
+        for (p = fmt->buffers; p < fmt->planes; p++)
+                pfmt[0].sizeimage += (pfmt[0].bytesperline * mp->height *
+                        (fmt->bit_depth[p] / fmt->vdownsampling[p])) /
+                        (fmt->bit_depth[0] / fmt->vdownsampling[0]);
+
         mp->colorspace = vivid_colorspace_cap(dev);
         if (fmt->color_enc == TGP_COLOR_ENC_HSV)
                 mp->hsv_enc = vivid_hsv_enc_cap(dev);
diff --git a/drivers/media/platform/vivid/vivid-vid-common.c b/drivers/media/platform/vivid/vivid-vid-common.c
index 5fc010f6ce67..f0f423c7ca41 100644
--- a/drivers/media/platform/vivid/vivid-vid-common.c
+++ b/drivers/media/platform/vivid/vivid-vid-common.c
@@ -858,7 +858,7 @@ int vidioc_g_edid(struct file *file, void *_fh,
                 return -EINVAL;
         if (edid->start_block + edid->blocks > dev->edid_blocks)
                 edid->blocks = dev->edid_blocks - edid->start_block;
-        memcpy(edid->edid, dev->edid, edid->blocks * 128);
-        cec_set_edid_phys_addr(edid->edid, edid->blocks * 128, adap->phys_addr);
+        cec_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);
         return 0;
 }
diff --git a/drivers/media/platform/vivid/vivid-vid-out.c b/drivers/media/platform/vivid/vivid-vid-out.c
index 7ba52ee98371..0b1b6218ede8 100644
--- a/drivers/media/platform/vivid/vivid-vid-out.c
+++ b/drivers/media/platform/vivid/vivid-vid-out.c
@@ -390,22 +390,28 @@ int vivid_try_fmt_vid_out(struct file *file, void *priv,
 
         /* This driver supports custom bytesperline values */
 
-        /* Calculate the minimum supported bytesperline value */
-        bytesperline = (mp->width * fmt->bit_depth[0]) >> 3;
-        /* Calculate the maximum supported bytesperline value */
-        max_bpl = (MAX_ZOOM * MAX_WIDTH * fmt->bit_depth[0]) >> 3;
         mp->num_planes = fmt->buffers;
-        for (p = 0; p < mp->num_planes; p++) {
+        for (p = 0; p < fmt->buffers; p++) {
+                /* Calculate the minimum supported bytesperline value */
+                bytesperline = (mp->width * fmt->bit_depth[p]) >> 3;
+                /* Calculate the maximum supported bytesperline value */
+                max_bpl = (MAX_ZOOM * MAX_WIDTH * fmt->bit_depth[p]) >> 3;
+
                 if (pfmt[p].bytesperline > max_bpl)
                         pfmt[p].bytesperline = max_bpl;
                 if (pfmt[p].bytesperline < bytesperline)
                         pfmt[p].bytesperline = bytesperline;
-                pfmt[p].sizeimage = pfmt[p].bytesperline * mp->height;
+
+                pfmt[p].sizeimage = (pfmt[p].bytesperline * mp->height) /
+                                        fmt->vdownsampling[p];
+
                 memset(pfmt[p].reserved, 0, sizeof(pfmt[p].reserved));
         }
         for (p = fmt->buffers; p < fmt->planes; p++)
-                pfmt[0].sizeimage += (pfmt[0].bytesperline * fmt->bit_depth[p]) /
-                                     (fmt->bit_depth[0] * fmt->vdownsampling[p]);
+                pfmt[0].sizeimage += (pfmt[0].bytesperline * mp->height *
+                        (fmt->bit_depth[p] / fmt->vdownsampling[p])) /
+                        (fmt->bit_depth[0] / fmt->vdownsampling[0]);
+
         mp->xfer_func = V4L2_XFER_FUNC_DEFAULT;
         mp->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
         mp->quantization = V4L2_QUANTIZATION_DEFAULT;
@@ -1172,14 +1178,12 @@ int vidioc_subscribe_event(struct v4l2_fh *fh,
                         const struct v4l2_event_subscription *sub)
 {
         switch (sub->type) {
-        case V4L2_EVENT_CTRL:
-                return v4l2_ctrl_subscribe_event(fh, sub);
         case V4L2_EVENT_SOURCE_CHANGE:
                 if (fh->vdev->vfl_dir == VFL_DIR_RX)
                         return v4l2_src_change_event_subscribe(fh, sub);
                 break;
         default:
-                break;
+                return v4l2_ctrl_subscribe_event(fh, sub);
         }
         return -EINVAL;
 }
diff --git a/drivers/media/platform/vsp1/Makefile b/drivers/media/platform/vsp1/Makefile
index 1328e1bd2143..a33afc385a48 100644
--- a/drivers/media/platform/vsp1/Makefile
+++ b/drivers/media/platform/vsp1/Makefile
@@ -3,6 +3,7 @@ vsp1-y					+= vsp1_dl.o vsp1_drm.o vsp1_video.o
 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_bru.o vsp1_sru.o vsp1_uds.o
+vsp1-y                                  += vsp1_hgo.o vsp1_hgt.o vsp1_histo.o
 vsp1-y                                  += vsp1_lif.o
 
 obj-$(CONFIG_VIDEO_RENESAS_VSP1)        += vsp1.o
diff --git a/drivers/media/platform/vsp1/vsp1.h b/drivers/media/platform/vsp1/vsp1.h
index b23fa879a9aa..85387a64179a 100644
--- a/drivers/media/platform/vsp1/vsp1.h
+++ b/drivers/media/platform/vsp1/vsp1.h
@@ -32,6 +32,8 @@ struct vsp1_entity;
 struct vsp1_platform_data;
 struct vsp1_bru;
 struct vsp1_clu;
+struct vsp1_hgo;
+struct vsp1_hgt;
 struct vsp1_hsit;
 struct vsp1_lif;
 struct vsp1_lut;
@@ -50,6 +52,8 @@ struct vsp1_uds;
 #define VSP1_HAS_CLU            (1 << 4)
 #define VSP1_HAS_WPF_VFLIP      (1 << 5)
 #define VSP1_HAS_WPF_HFLIP      (1 << 6)
+#define VSP1_HAS_HGO            (1 << 7)
+#define VSP1_HAS_HGT            (1 << 8)
 
 struct vsp1_device_info {
         u32 version;
@@ -73,6 +77,8 @@ struct vsp1_device {
 
         struct vsp1_bru *bru;
         struct vsp1_clu *clu;
+        struct vsp1_hgo *hgo;
+        struct vsp1_hgt *hgt;
         struct vsp1_hsit *hsi;
         struct vsp1_hsit *hst;
         struct vsp1_lif *lif;
diff --git a/drivers/media/platform/vsp1/vsp1_bru.c b/drivers/media/platform/vsp1/vsp1_bru.c
index ee8355c28f94..85362c5ef57a 100644
--- a/drivers/media/platform/vsp1/vsp1_bru.c
+++ b/drivers/media/platform/vsp1/vsp1_bru.c
@@ -251,7 +251,8 @@ static int bru_set_selection(struct v4l2_subdev *subdev,
         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);
 
-        /* Scaling isn't supported, the compose rectangle size must be identical
+        /*
+         * Scaling isn't supported, the compose rectangle size must be identical
          * to the sink format size.
          */
         format = vsp1_entity_get_pad_format(&bru->entity, config, sel->pad);
@@ -300,13 +301,15 @@ static void bru_configure(struct vsp1_entity *entity,
         format = vsp1_entity_get_pad_format(&bru->entity, bru->entity.config,
                                             bru->entity.source_pad);
 
-        /* The hardware is extremely flexible but we have no userspace API to
+        /*
+         * The hardware is extremely flexible but we have no userspace API to
          * expose all the parameters, nor is it clear whether we would have use
          * cases for all the supported modes. Let's just harcode the parameters
          * to sane default values for now.
          */
 
-        /* Disable dithering and enable color data normalization unless the
+        /*
+         * Disable dithering and enable color data normalization unless the
          * format at the pipeline output is premultiplied.
          */
         flags = pipe->output ? pipe->output->format.flags : 0;
@@ -314,7 +317,8 @@ static void bru_configure(struct vsp1_entity *entity,
                        flags & V4L2_PIX_FMT_FLAG_PREMUL_ALPHA ?
                        0 : VI6_BRU_INCTRL_NRM);
 
-        /* Set the background position to cover the whole output image and
+        /*
+         * Set the background position to cover the whole output image and
          * configure its color.
          */
         vsp1_bru_write(bru, dl, VI6_BRU_VIRRPF_SIZE,
@@ -325,7 +329,8 @@ static void bru_configure(struct vsp1_entity *entity,
         vsp1_bru_write(bru, dl, VI6_BRU_VIRRPF_COL, bru->bgcolor |
                        (0xff << VI6_BRU_VIRRPF_COL_A_SHIFT));
 
-        /* Route BRU input 1 as SRC input to the ROP unit and configure the ROP
+        /*
+         * Route BRU input 1 as SRC input to the ROP unit and configure the ROP
          * unit with a NOP operation to make BRU input 1 available as the
          * Blend/ROP unit B SRC input.
          */
@@ -337,7 +342,8 @@ static void bru_configure(struct vsp1_entity *entity,
                 bool premultiplied = false;
                 u32 ctrl = 0;
 
-                /* Configure all Blend/ROP units corresponding to an enabled BRU
+                /*
+                 * Configure all Blend/ROP units corresponding to an enabled BRU
                  * input for alpha blending. Blend/ROP units corresponding to
                  * disabled BRU inputs are used in ROP NOP mode to ignore the
                  * SRC input.
@@ -352,13 +358,15 @@ static void bru_configure(struct vsp1_entity *entity,
                              |  VI6_BRU_CTRL_AROP(VI6_ROP_NOP);
                 }
 
-                /* Select the virtual RPF as the Blend/ROP unit A DST input to
+                /*
+                 * Select the virtual RPF as the Blend/ROP unit A DST input to
                  * serve as a background color.
                  */
                 if (i == 0)
                         ctrl |= VI6_BRU_CTRL_DSTSEL_VRPF;
 
-                /* Route BRU inputs 0 to 3 as SRC inputs to Blend/ROP units A to
+                /*
+                 * Route BRU inputs 0 to 3 as SRC inputs to Blend/ROP units A to
                  * D in that order. The Blend/ROP unit B SRC is hardwired to the
                  * ROP unit output, the corresponding register bits must be set
                  * to 0.
@@ -368,7 +376,8 @@ static void bru_configure(struct vsp1_entity *entity,
 
                 vsp1_bru_write(bru, dl, VI6_BRU_CTRL(i), ctrl);
 
-                /* Harcode the blending formula to
+                /*
+                 * Harcode the blending formula to
                  *
                  *      DSTc = DSTc * (1 - SRCa) + SRCc * SRCa
                  *      DSTa = DSTa * (1 - SRCa) + SRCa
diff --git a/drivers/media/platform/vsp1/vsp1_dl.c b/drivers/media/platform/vsp1/vsp1_dl.c
index ad545aff4e35..7d8f37772b56 100644
--- a/drivers/media/platform/vsp1/vsp1_dl.c
+++ b/drivers/media/platform/vsp1/vsp1_dl.c
@@ -240,7 +240,8 @@ static struct vsp1_dl_list *vsp1_dl_list_alloc(struct vsp1_dl_manager *dlm)
         INIT_LIST_HEAD(&dl->fragments);
         dl->dlm = dlm;
 
-        /* Initialize the display list body and allocate DMA memory for the body
+        /*
+         * Initialize the display list body and allocate DMA memory for the body
          * and the optional header. Both are allocated together to avoid memory
          * fragmentation, with the header located right after the body in
          * memory.
@@ -511,7 +512,8 @@ void vsp1_dl_list_commit(struct vsp1_dl_list *dl)
                 goto done;
         }
 
-        /* Once the UPD bit has been set the hardware can start processing the
+        /*
+         * Once the UPD bit has been set the hardware can start processing the
          * display list at any time and we can't touch the address and size
          * registers. In that case mark the update as pending, it will be
          * queued up to the hardware by the frame end interrupt handler.
@@ -523,7 +525,8 @@ void vsp1_dl_list_commit(struct vsp1_dl_list *dl)
                 goto done;
         }
 
-        /* Program the hardware with the display list body address and size.
+        /*
+         * Program the hardware with the display list body address and size.
          * The UPD bit will be cleared by the device when the display list is
          * processed.
          */
@@ -547,7 +550,8 @@ void vsp1_dlm_irq_display_start(struct vsp1_dl_manager *dlm)
 {
         spin_lock(&dlm->lock);
 
-        /* The display start interrupt signals the end of the display list
+        /*
+         * The display start interrupt signals the end of the display list
          * processing by the device. The active display list, if any, won't be
          * accessed anymore and can be reused.
          */
@@ -566,14 +570,16 @@ void vsp1_dlm_irq_frame_end(struct vsp1_dl_manager *dlm)
         __vsp1_dl_list_put(dlm->active);
         dlm->active = NULL;
 
-        /* Header mode is used for mem-to-mem pipelines only. We don't need to
+        /*
+         * Header mode is used for mem-to-mem pipelines only. We don't need to
          * perform any operation as there can't be any new display list queued
          * in that case.
          */
         if (dlm->mode == VSP1_DL_MODE_HEADER)
                 goto done;
 
-        /* The UPD bit set indicates that the commit operation raced with the
+        /*
+         * The UPD bit set indicates that the commit operation raced with the
          * interrupt and occurred after the frame end event and UPD clear but
          * before interrupt processing. The hardware hasn't taken the update
          * into account yet, we'll thus skip one frame and retry.
@@ -581,7 +587,8 @@ void vsp1_dlm_irq_frame_end(struct vsp1_dl_manager *dlm)
         if (vsp1_read(vsp1, VI6_DL_BODY_SIZE) & VI6_DL_BODY_SIZE_UPD)
                 goto done;
 
-        /* The device starts processing the queued display list right after the
+        /*
+         * The device starts processing the queued display list right after the
          * frame end interrupt. The display list thus becomes active.
          */
         if (dlm->queued) {
@@ -589,7 +596,8 @@ void vsp1_dlm_irq_frame_end(struct vsp1_dl_manager *dlm)
                 dlm->queued = NULL;
         }
 
-        /* Now that the UPD bit has been cleared we can queue the next display
+        /*
+         * Now that the UPD bit has been cleared we can queue the next display
          * list to the hardware if one has been prepared.
          */
         if (dlm->pending) {
@@ -615,7 +623,8 @@ void vsp1_dlm_setup(struct vsp1_device *vsp1)
                  | VI6_DL_CTRL_DC2 | VI6_DL_CTRL_DC1 | VI6_DL_CTRL_DC0
                  | VI6_DL_CTRL_DLE;
 
-        /* The DRM pipeline operates with display lists in Continuous Frame
+        /*
+         * The DRM pipeline operates with display lists in Continuous Frame
          * Mode, all other pipelines use manual start.
          */
         if (vsp1->drm)
diff --git a/drivers/media/platform/vsp1/vsp1_drm.c b/drivers/media/platform/vsp1/vsp1_drm.c
index b4c0f10fc3b0..9d235e830f5a 100644
--- a/drivers/media/platform/vsp1/vsp1_drm.c
+++ b/drivers/media/platform/vsp1/vsp1_drm.c
@@ -78,7 +78,8 @@ int vsp1_du_setup_lif(struct device *dev, const struct vsp1_du_lif_config *cfg)
         int ret;
 
         if (!cfg) {
-                /* NULL configuration means the CRTC is being disabled, stop
+                /*
+                 * NULL configuration means the CRTC is being disabled, stop
                  * the pipeline and turn the light off.
                  */
                 ret = vsp1_pipeline_stop(pipe);
@@ -106,7 +107,8 @@ int vsp1_du_setup_lif(struct device *dev, const struct vsp1_du_lif_config *cfg)
         dev_dbg(vsp1->dev, "%s: configuring LIF with format %ux%u\n",
                 __func__, cfg->width, cfg->height);
 
-        /* Configure the format at the BRU sinks and propagate it through the
+        /*
+         * Configure the format at the BRU sinks and propagate it through the
          * pipeline.
          */
         memset(&format, 0, sizeof(format));
@@ -175,7 +177,8 @@ int vsp1_du_setup_lif(struct device *dev, const struct vsp1_du_lif_config *cfg)
                 __func__, format.format.width, format.format.height,
                 format.format.code);
 
-        /* Verify that the format at the output of the pipeline matches the
+        /*
+         * Verify that the format at the output of the pipeline matches the
          * requested frame size and media bus code.
          */
         if (format.format.width != cfg->width ||
@@ -185,7 +188,8 @@ int vsp1_du_setup_lif(struct device *dev, const struct vsp1_du_lif_config *cfg)
                 return -EPIPE;
         }
 
-        /* Mark the pipeline as streaming and enable the VSP1. This will store
+        /*
+         * Mark the pipeline as streaming and enable the VSP1. This will store
          * the pipeline pointer in all entities, which the s_stream handlers
          * will need. We don't start the entities themselves right at this point
          * as there's no plane configured yet, so we can't start processing
@@ -219,9 +223,6 @@ void vsp1_du_atomic_begin(struct device *dev)
         struct vsp1_pipeline *pipe = &vsp1->drm->pipe;
 
         vsp1->drm->num_inputs = pipe->num_inputs;
-
-        /* Prepare the display list. */
-        pipe->dl = vsp1_dl_list_get(pipe->output->dlm);
 }
 EXPORT_SYMBOL_GPL(vsp1_du_atomic_begin);
 
@@ -320,7 +321,8 @@ static int vsp1_du_setup_rpf_pipe(struct vsp1_device *vsp1,
         const struct v4l2_rect *crop;
         int ret;
 
-        /* Configure the format on the RPF sink pad and propagate it up to the
+        /*
+         * Configure the format on the RPF sink pad and propagate it up to the
          * BRU sink pad.
          */
         crop = &vsp1->drm->inputs[rpf->entity.index].crop;
@@ -359,7 +361,8 @@ static int vsp1_du_setup_rpf_pipe(struct vsp1_device *vsp1,
                 __func__, sel.r.left, sel.r.top, sel.r.width, sel.r.height,
                 rpf->entity.index);
 
-        /* RPF source, hardcode the format to ARGB8888 to turn on format
+        /*
+         * RPF source, hardcode the format to ARGB8888 to turn on format
          * conversion if needed.
          */
         format.pad = RWPF_PAD_SOURCE;
@@ -425,10 +428,14 @@ void vsp1_du_atomic_flush(struct device *dev)
         struct vsp1_pipeline *pipe = &vsp1->drm->pipe;
         struct vsp1_rwpf *inputs[VSP1_MAX_RPF] = { NULL, };
         struct vsp1_entity *entity;
+        struct vsp1_dl_list *dl;
         unsigned long flags;
         unsigned int i;
         int ret;
 
+        /* Prepare the display list. */
+        dl = vsp1_dl_list_get(pipe->output->dlm);
+
         /* Count the number of enabled inputs and sort them by Z-order. */
         pipe->num_inputs = 0;
 
@@ -483,26 +490,25 @@ void vsp1_du_atomic_flush(struct device *dev)
                         struct vsp1_rwpf *rpf = to_rwpf(&entity->subdev);
 
                         if (!pipe->inputs[rpf->entity.index]) {
-                                vsp1_dl_list_write(pipe->dl, entity->route->reg,
+                                vsp1_dl_list_write(dl, entity->route->reg,
                                                    VI6_DPR_NODE_UNUSED);
                                 continue;
                         }
                 }
 
-                vsp1_entity_route_setup(entity, pipe->dl);
+                vsp1_entity_route_setup(entity, pipe, dl);
 
                 if (entity->ops->configure) {
-                        entity->ops->configure(entity, pipe, pipe->dl,
+                        entity->ops->configure(entity, pipe, dl,
                                                VSP1_ENTITY_PARAMS_INIT);
-                        entity->ops->configure(entity, pipe, pipe->dl,
+                        entity->ops->configure(entity, pipe, dl,
                                                VSP1_ENTITY_PARAMS_RUNTIME);
-                        entity->ops->configure(entity, pipe, pipe->dl,
+                        entity->ops->configure(entity, pipe, dl,
                                                VSP1_ENTITY_PARAMS_PARTITION);
                 }
         }
 
-        vsp1_dl_list_commit(pipe->dl);
-        pipe->dl = NULL;
+        vsp1_dl_list_commit(dl);
 
         /* Start or stop the pipeline if needed. */
         if (!vsp1->drm->num_inputs && pipe->num_inputs) {
@@ -528,7 +534,8 @@ int vsp1_drm_create_links(struct vsp1_device *vsp1)
         unsigned int i;
         int ret;
 
-        /* VSPD instances require a BRU to perform composition and a LIF to
+        /*
+         * VSPD instances require a BRU to perform composition and a LIF to
          * output to the DU.
          */
         if (!vsp1->bru || !vsp1->lif)
@@ -595,6 +602,7 @@ int vsp1_drm_init(struct vsp1_device *vsp1)
         pipe->bru = &vsp1->bru->entity;
         pipe->lif = &vsp1->lif->entity;
         pipe->output = vsp1->wpf[0];
+        pipe->output->pipe = pipe;
 
         return 0;
 }
diff --git a/drivers/media/platform/vsp1/vsp1_drm.h b/drivers/media/platform/vsp1/vsp1_drm.h
index 9e28ab9254ba..c8d2f88fc483 100644
--- a/drivers/media/platform/vsp1/vsp1_drm.h
+++ b/drivers/media/platform/vsp1/vsp1_drm.h
@@ -21,7 +21,7 @@
  * vsp1_drm - State for the API exposed to the DRM driver
  * @pipe: the VSP1 pipeline used for display
  * @num_inputs: number of active pipeline inputs at the beginning of an update
- * @planes: source crop rectangle, destination compose rectangle and z-order
+ * @inputs: source crop rectangle, destination compose rectangle and z-order
  *      position for every input
  */
 struct vsp1_drm {
diff --git a/drivers/media/platform/vsp1/vsp1_drv.c b/drivers/media/platform/vsp1/vsp1_drv.c
index aa237b48ad55..048446af5ae7 100644
--- a/drivers/media/platform/vsp1/vsp1_drv.c
+++ b/drivers/media/platform/vsp1/vsp1_drv.c
@@ -30,6 +30,8 @@
 #include "vsp1_clu.h"
 #include "vsp1_dl.h"
 #include "vsp1_drm.h"
+#include "vsp1_hgo.h"
+#include "vsp1_hgt.h"
 #include "vsp1_hsit.h"
 #include "vsp1_lif.h"
 #include "vsp1_lut.h"
@@ -105,7 +107,9 @@ static int vsp1_create_sink_links(struct vsp1_device *vsp1,
                 if (source->type == sink->type)
                         continue;
 
-                if (source->type == VSP1_ENTITY_LIF ||
+                if (source->type == VSP1_ENTITY_HGO ||
+                    source->type == VSP1_ENTITY_HGT ||
+                    source->type == VSP1_ENTITY_LIF ||
                     source->type == VSP1_ENTITY_WPF)
                         continue;
 
@@ -148,6 +152,26 @@ static int vsp1_uapi_create_links(struct vsp1_device *vsp1)
                         return ret;
         }
 
+        if (vsp1->hgo) {
+                ret = media_create_pad_link(&vsp1->hgo->histo.entity.subdev.entity,
+                                            HISTO_PAD_SOURCE,
+                                            &vsp1->hgo->histo.video.entity, 0,
+                                            MEDIA_LNK_FL_ENABLED |
+                                            MEDIA_LNK_FL_IMMUTABLE);
+                if (ret < 0)
+                        return ret;
+        }
+
+        if (vsp1->hgt) {
+                ret = media_create_pad_link(&vsp1->hgt->histo.entity.subdev.entity,
+                                            HISTO_PAD_SOURCE,
+                                            &vsp1->hgt->histo.video.entity, 0,
+                                            MEDIA_LNK_FL_ENABLED |
+                                            MEDIA_LNK_FL_IMMUTABLE);
+                if (ret < 0)
+                        return ret;
+        }
+
         if (vsp1->lif) {
                 ret = media_create_pad_link(&vsp1->wpf[0]->entity.subdev.entity,
                                             RWPF_PAD_SOURCE,
@@ -170,7 +194,8 @@ static int vsp1_uapi_create_links(struct vsp1_device *vsp1)
         }
 
         for (i = 0; i < vsp1->info->wpf_count; ++i) {
-                /* Connect the video device to the WPF. All connections are
+                /*
+                 * Connect the video device to the WPF. All connections are
                  * immutable.
                  */
                 struct vsp1_rwpf *wpf = vsp1->wpf[i];
@@ -227,7 +252,8 @@ static int vsp1_create_entities(struct vsp1_device *vsp1)
         media_device_init(mdev);
 
         vsp1->media_ops.link_setup = vsp1_entity_link_setup;
-        /* Don't perform link validation when the userspace API is disabled as
+        /*
+         * Don't perform link validation when the userspace API is disabled as
          * the pipeline is configured internally by the driver in that case, and
          * its configuration can thus be trusted.
          */
@@ -279,7 +305,30 @@ static int vsp1_create_entities(struct vsp1_device *vsp1)
 
         list_add_tail(&vsp1->hst->entity.list_dev, &vsp1->entities);
 
-        /* The LIF is only supported when used in conjunction with the DU, in
+        if (vsp1->info->features & VSP1_HAS_HGO && vsp1->info->uapi) {
+                vsp1->hgo = vsp1_hgo_create(vsp1);
+                if (IS_ERR(vsp1->hgo)) {
+                        ret = PTR_ERR(vsp1->hgo);
+                        goto done;
+                }
+
+                list_add_tail(&vsp1->hgo->histo.entity.list_dev,
+                              &vsp1->entities);
+        }
+
+        if (vsp1->info->features & VSP1_HAS_HGT && vsp1->info->uapi) {
+                vsp1->hgt = vsp1_hgt_create(vsp1);
+                if (IS_ERR(vsp1->hgt)) {
+                        ret = PTR_ERR(vsp1->hgt);
+                        goto done;
+                }
+
+                list_add_tail(&vsp1->hgt->histo.entity.list_dev,
+                              &vsp1->entities);
+        }
+
+        /*
+         * The LIF is only supported when used in conjunction with the DU, in
          * which case the userspace API is disabled. If the userspace API is
          * enabled skip the LIF, even when present.
          */
@@ -391,7 +440,8 @@ static int vsp1_create_entities(struct vsp1_device *vsp1)
         if (ret < 0)
                 goto done;
 
-        /* Register subdev nodes if the userspace API is enabled or initialize
+        /*
+         * Register subdev nodes if the userspace API is enabled or initialize
          * the DRM pipeline otherwise.
          */
         if (vsp1->info->uapi) {
@@ -562,8 +612,9 @@ static const struct vsp1_device_info vsp1_device_infos[] = {
                 .version = VI6_IP_VERSION_MODEL_VSPS_H2,
                 .model = "VSP1-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_HGO
+                          | VSP1_HAS_HGT | VSP1_HAS_LUT | VSP1_HAS_SRU
+                          | VSP1_HAS_WPF_VFLIP,
                 .rpf_count = 5,
                 .uds_count = 3,
                 .wpf_count = 4,
@@ -583,7 +634,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_LIF | VSP1_HAS_LUT,
+                .features = VSP1_HAS_BRU | VSP1_HAS_HGO | VSP1_HAS_LIF
+                          | VSP1_HAS_LUT,
                 .rpf_count = 4,
                 .uds_count = 1,
                 .wpf_count = 1,
@@ -593,8 +645,9 @@ static const struct vsp1_device_info vsp1_device_infos[] = {
                 .version = VI6_IP_VERSION_MODEL_VSPS_M2,
                 .model = "VSP1-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_HGO
+                          | VSP1_HAS_HGT | VSP1_HAS_LUT | VSP1_HAS_SRU
+                          | VSP1_HAS_WPF_VFLIP,
                 .rpf_count = 5,
                 .uds_count = 1,
                 .wpf_count = 4,
@@ -626,8 +679,9 @@ static const struct vsp1_device_info vsp1_device_infos[] = {
                 .version = VI6_IP_VERSION_MODEL_VSPI_GEN3,
                 .model = "VSP2-I",
                 .gen = 3,
-                .features = VSP1_HAS_CLU | VSP1_HAS_LUT | VSP1_HAS_SRU
-                          | VSP1_HAS_WPF_HFLIP | VSP1_HAS_WPF_VFLIP,
+                .features = VSP1_HAS_CLU | VSP1_HAS_HGO | VSP1_HAS_HGT
+                          | VSP1_HAS_LUT | VSP1_HAS_SRU | VSP1_HAS_WPF_HFLIP
+                          | VSP1_HAS_WPF_VFLIP,
                 .rpf_count = 1,
                 .uds_count = 1,
                 .wpf_count = 1,
@@ -645,8 +699,8 @@ static const struct vsp1_device_info vsp1_device_infos[] = {
                 .version = VI6_IP_VERSION_MODEL_VSPBC_GEN3,
                 .model = "VSP2-BC",
                 .gen = 3,
-                .features = VSP1_HAS_BRU | VSP1_HAS_CLU | VSP1_HAS_LUT
-                          | VSP1_HAS_WPF_VFLIP,
+                .features = VSP1_HAS_BRU | VSP1_HAS_CLU | VSP1_HAS_HGO
+                          | VSP1_HAS_LUT | VSP1_HAS_WPF_VFLIP,
                 .rpf_count = 5,
                 .wpf_count = 1,
                 .num_bru_inputs = 5,
diff --git a/drivers/media/platform/vsp1/vsp1_entity.c b/drivers/media/platform/vsp1/vsp1_entity.c
index da673495c222..4bdb3b141611 100644
--- a/drivers/media/platform/vsp1/vsp1_entity.c
+++ b/drivers/media/platform/vsp1/vsp1_entity.c
@@ -21,6 +21,8 @@
 #include "vsp1.h"
 #include "vsp1_dl.h"
 #include "vsp1_entity.h"
+#include "vsp1_pipe.h"
+#include "vsp1_rwpf.h"
 
 static inline struct vsp1_entity *
 media_entity_to_vsp1_entity(struct media_entity *entity)
@@ -28,11 +30,42 @@ media_entity_to_vsp1_entity(struct media_entity *entity)
         return container_of(entity, struct vsp1_entity, subdev.entity);
 }
 
-void vsp1_entity_route_setup(struct vsp1_entity *source,
+void vsp1_entity_route_setup(struct vsp1_entity *entity,
+                             struct vsp1_pipeline *pipe,
                              struct vsp1_dl_list *dl)
 {
+        struct vsp1_entity *source;
         struct vsp1_entity *sink;
 
+        if (entity->type == VSP1_ENTITY_HGO) {
+                u32 smppt;
+
+                /*
+                 * The HGO is a special case, its routing is configured on the
+                 * sink pad.
+                 */
+                source = media_entity_to_vsp1_entity(entity->sources[0]);
+                smppt = (pipe->output->entity.index << VI6_DPR_SMPPT_TGW_SHIFT)
+                      | (source->route->output << VI6_DPR_SMPPT_PT_SHIFT);
+
+                vsp1_dl_list_write(dl, VI6_DPR_HGO_SMPPT, smppt);
+                return;
+        } else if (entity->type == VSP1_ENTITY_HGT) {
+                u32 smppt;
+
+                /*
+                 * The HGT is a special case, its routing is configured on the
+                 * sink pad.
+                 */
+                source = media_entity_to_vsp1_entity(entity->sources[0]);
+                smppt = (pipe->output->entity.index << VI6_DPR_SMPPT_TGW_SHIFT)
+                      | (source->route->output << VI6_DPR_SMPPT_PT_SHIFT);
+
+                vsp1_dl_list_write(dl, VI6_DPR_HGT_SMPPT, smppt);
+                return;
+        }
+
+        source = entity;
         if (source->route->reg == 0)
                 return;
 
@@ -199,7 +232,8 @@ int vsp1_subdev_enum_mbus_code(struct v4l2_subdev *subdev,
                 struct v4l2_subdev_pad_config *config;
                 struct v4l2_mbus_framefmt *format;
 
-                /* The entity can't perform format conversion, the sink format
+                /*
+                 * The entity can't perform format conversion, the sink format
                  * is always identical to the source format.
                  */
                 if (code->index)
@@ -263,7 +297,8 @@ int vsp1_subdev_enum_frame_size(struct v4l2_subdev *subdev,
                 fse->min_height = min_height;
                 fse->max_height = max_height;
         } else {
-                /* The size on the source pad are fixed and always identical to
+                /*
+                 * The size on the source pad are fixed and always identical to
                  * the size on the sink pad.
                  */
                 fse->min_width = format->width;
@@ -281,25 +316,32 @@ done:
  * Media Operations
  */
 
-int vsp1_entity_link_setup(struct media_entity *entity,
-                           const struct media_pad *local,
-                           const struct media_pad *remote, u32 flags)
+static int vsp1_entity_link_setup_source(const struct media_pad *source_pad,
+                                         const struct media_pad *sink_pad,
+                                         u32 flags)
 {
         struct vsp1_entity *source;
 
-        if (!(local->flags & MEDIA_PAD_FL_SOURCE))
-                return 0;
-
-        source = media_entity_to_vsp1_entity(local->entity);
+        source = media_entity_to_vsp1_entity(source_pad->entity);
 
         if (!source->route)
                 return 0;
 
         if (flags & MEDIA_LNK_FL_ENABLED) {
-                if (source->sink)
-                        return -EBUSY;
-                source->sink = remote->entity;
-                source->sink_pad = remote->index;
+                struct vsp1_entity *sink
+                        = media_entity_to_vsp1_entity(sink_pad->entity);
+
+                /*
+                 * Fan-out is limited to one for the normal data path plus
+                 * optional HGO and HGT. We ignore the HGO and HGT here.
+                 */
+                if (sink->type != VSP1_ENTITY_HGO &&
+                    sink->type != VSP1_ENTITY_HGT) {
+                        if (source->sink)
+                                return -EBUSY;
+                        source->sink = sink_pad->entity;
+                        source->sink_pad = sink_pad->index;
+                }
         } else {
                 source->sink = NULL;
                 source->sink_pad = 0;
@@ -308,6 +350,85 @@ int vsp1_entity_link_setup(struct media_entity *entity,
         return 0;
 }
 
+static int vsp1_entity_link_setup_sink(const struct media_pad *source_pad,
+                                       const struct media_pad *sink_pad,
+                                       u32 flags)
+{
+        struct vsp1_entity *sink;
+
+        sink = media_entity_to_vsp1_entity(sink_pad->entity);
+
+        if (flags & MEDIA_LNK_FL_ENABLED) {
+                /* Fan-in is limited to one. */
+                if (sink->sources[sink_pad->index])
+                        return -EBUSY;
+
+                sink->sources[sink_pad->index] = source_pad->entity;
+        } else {
+                sink->sources[sink_pad->index] = NULL;
+        }
+
+        return 0;
+}
+
+int vsp1_entity_link_setup(struct media_entity *entity,
+                           const struct media_pad *local,
+                           const struct media_pad *remote, u32 flags)
+{
+        if (local->flags & MEDIA_PAD_FL_SOURCE)
+                return vsp1_entity_link_setup_source(local, remote, flags);
+        else
+                return vsp1_entity_link_setup_sink(remote, local, flags);
+}
+
+/**
+ * vsp1_entity_remote_pad - Find the pad at the remote end of a link
+ * @pad: Pad at the local end of the link
+ *
+ * Search for a remote pad connected to the given pad by iterating over all
+ * links originating or terminating at that pad until an enabled link is found.
+ *
+ * Our link setup implementation guarantees that the output fan-out will not be
+ * higher than one for the data pipelines, except for the links to the HGO and
+ * HGT that can be enabled in addition to a regular data link. When traversing
+ * outgoing links this function ignores HGO and HGT entities and should thus be
+ * used in place of the generic media_entity_remote_pad() function to traverse
+ * data pipelines.
+ *
+ * Return a pointer to the pad at the remote end of the first found enabled
+ * link, or NULL if no enabled link has been found.
+ */
+struct media_pad *vsp1_entity_remote_pad(struct media_pad *pad)
+{
+        struct media_link *link;
+
+        list_for_each_entry(link, &pad->entity->links, list) {
+                struct vsp1_entity *entity;
+
+                if (!(link->flags & MEDIA_LNK_FL_ENABLED))
+                        continue;
+
+                /* If we're the sink the source will never be an HGO or HGT. */
+                if (link->sink == pad)
+                        return link->source;
+
+                if (link->source != pad)
+                        continue;
+
+                /* If the sink isn't a subdevice it can't be an HGO or HGT. */
+                if (!is_media_entity_v4l2_subdev(link->sink->entity))
+                        return link->sink;
+
+                entity = media_entity_to_vsp1_entity(link->sink->entity);
+                if (entity->type != VSP1_ENTITY_HGO &&
+                    entity->type != VSP1_ENTITY_HGT)
+                        return link->sink;
+        }
+
+        return NULL;
+
+}
+
 /* -----------------------------------------------------------------------------
  * Initialization
  */
@@ -334,6 +455,8 @@ static const struct vsp1_route vsp1_routes[] = {
             VI6_DPR_NODE_BRU_IN(2), VI6_DPR_NODE_BRU_IN(3),
             VI6_DPR_NODE_BRU_IN(4) }, VI6_DPR_NODE_BRU_OUT },
         VSP1_ENTITY_ROUTE(CLU),
+        { VSP1_ENTITY_HGO, 0, 0, { 0, }, 0 },
+        { VSP1_ENTITY_HGT, 0, 0, { 0, }, 0 },
         VSP1_ENTITY_ROUTE(HSI),
         VSP1_ENTITY_ROUTE(HST),
         { VSP1_ENTITY_LIF, 0, 0, { VI6_DPR_NODE_LIF, }, VI6_DPR_NODE_LIF },
@@ -386,7 +509,14 @@ int vsp1_entity_init(struct vsp1_device *vsp1, struct vsp1_entity *entity,
         for (i = 0; i < num_pads - 1; ++i)
                 entity->pads[i].flags = MEDIA_PAD_FL_SINK;
 
-        entity->pads[num_pads - 1].flags = MEDIA_PAD_FL_SOURCE;
+        entity->sources = devm_kcalloc(vsp1->dev, max(num_pads - 1, 1U),
+                                       sizeof(*entity->sources), GFP_KERNEL);
+        if (entity->sources == NULL)
+                return -ENOMEM;
+
+        /* Single-pad entities only have a sink. */
+        entity->pads[num_pads - 1].flags = num_pads > 1 ? MEDIA_PAD_FL_SOURCE
+                                         : MEDIA_PAD_FL_SINK;
 
         /* Initialize the media entity. */
         ret = media_entity_pads_init(&entity->subdev.entity, num_pads,
@@ -407,7 +537,8 @@ int vsp1_entity_init(struct vsp1_device *vsp1, struct vsp1_entity *entity,
 
         vsp1_entity_init_cfg(subdev, NULL);
 
-        /* Allocate the pad configuration to store formats and selection
+        /*
+         * Allocate the pad configuration to store formats and selection
          * rectangles.
          */
         entity->config = v4l2_subdev_alloc_pad_config(&entity->subdev);
diff --git a/drivers/media/platform/vsp1/vsp1_entity.h b/drivers/media/platform/vsp1/vsp1_entity.h
index 901146f807b9..c169a060b6d2 100644
--- a/drivers/media/platform/vsp1/vsp1_entity.h
+++ b/drivers/media/platform/vsp1/vsp1_entity.h
@@ -25,6 +25,8 @@ struct vsp1_pipeline;
 enum vsp1_entity_type {
         VSP1_ENTITY_BRU,
         VSP1_ENTITY_CLU,
+        VSP1_ENTITY_HGO,
+        VSP1_ENTITY_HGT,
         VSP1_ENTITY_HSI,
         VSP1_ENTITY_HST,
         VSP1_ENTITY_LIF,
@@ -102,6 +104,7 @@ struct vsp1_entity {
         struct media_pad *pads;
         unsigned int source_pad;
 
+        struct media_entity **sources;
         struct media_entity *sink;
         unsigned int sink_pad;
 
@@ -142,9 +145,12 @@ vsp1_entity_get_pad_selection(struct vsp1_entity *entity,
 int vsp1_entity_init_cfg(struct v4l2_subdev *subdev,
                          struct v4l2_subdev_pad_config *cfg);
 
-void vsp1_entity_route_setup(struct vsp1_entity *source,
+void vsp1_entity_route_setup(struct vsp1_entity *entity,
+                             struct vsp1_pipeline *pipe,
                              struct vsp1_dl_list *dl);
 
+struct media_pad *vsp1_entity_remote_pad(struct media_pad *pad);
+
 int vsp1_subdev_get_pad_format(struct v4l2_subdev *subdev,
                                struct v4l2_subdev_pad_config *cfg,
                                struct v4l2_subdev_format *fmt);
diff --git a/drivers/media/platform/vsp1/vsp1_hgo.c b/drivers/media/platform/vsp1/vsp1_hgo.c
new file mode 100644
index 000000000000..50309c053b78
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_hgo.c
@@ -0,0 +1,230 @@
+/*
+ * vsp1_hgo.c  --  R-Car VSP1 Histogram Generator 1D
+ *
+ * Copyright (C) 2016 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/device.h>
+#include <linux/gfp.h>
+
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-vmalloc.h>
+
+#include "vsp1.h"
+#include "vsp1_dl.h"
+#include "vsp1_hgo.h"
+
+#define HGO_DATA_SIZE                           ((2 + 256) * 4)
+
+/* -----------------------------------------------------------------------------
+ * Device Access
+ */
+
+static inline u32 vsp1_hgo_read(struct vsp1_hgo *hgo, u32 reg)
+{
+        return vsp1_read(hgo->histo.entity.vsp1, reg);
+}
+
+static inline void vsp1_hgo_write(struct vsp1_hgo *hgo, struct vsp1_dl_list *dl,
+                                  u32 reg, u32 data)
+{
+        vsp1_dl_list_write(dl, reg, data);
+}
+
+/* -----------------------------------------------------------------------------
+ * Frame End Handler
+ */
+
+void vsp1_hgo_frame_end(struct vsp1_entity *entity)
+{
+        struct vsp1_hgo *hgo = to_hgo(&entity->subdev);
+        struct vsp1_histogram_buffer *buf;
+        unsigned int i;
+        size_t size;
+        u32 *data;
+
+        buf = vsp1_histogram_buffer_get(&hgo->histo);
+        if (!buf)
+                return;
+
+        data = buf->addr;
+
+        if (hgo->num_bins == 256) {
+                *data++ = vsp1_hgo_read(hgo, VI6_HGO_G_MAXMIN);
+                *data++ = vsp1_hgo_read(hgo, VI6_HGO_G_SUM);
+
+                for (i = 0; i < 256; ++i) {
+                        vsp1_write(hgo->histo.entity.vsp1,
+                                   VI6_HGO_EXT_HIST_ADDR, i);
+                        *data++ = vsp1_hgo_read(hgo, VI6_HGO_EXT_HIST_DATA);
+                }
+
+                size = (2 + 256) * sizeof(u32);
+        } else if (hgo->max_rgb) {
+                *data++ = vsp1_hgo_read(hgo, VI6_HGO_G_MAXMIN);
+                *data++ = vsp1_hgo_read(hgo, VI6_HGO_G_SUM);
+
+                for (i = 0; i < 64; ++i)
+                        *data++ = vsp1_hgo_read(hgo, VI6_HGO_G_HISTO(i));
+
+                size = (2 + 64) * sizeof(u32);
+        } else {
+                *data++ = vsp1_hgo_read(hgo, VI6_HGO_R_MAXMIN);
+                *data++ = vsp1_hgo_read(hgo, VI6_HGO_G_MAXMIN);
+                *data++ = vsp1_hgo_read(hgo, VI6_HGO_B_MAXMIN);
+
+                *data++ = vsp1_hgo_read(hgo, VI6_HGO_R_SUM);
+                *data++ = vsp1_hgo_read(hgo, VI6_HGO_G_SUM);
+                *data++ = vsp1_hgo_read(hgo, VI6_HGO_B_SUM);
+
+                for (i = 0; i < 64; ++i) {
+                        data[i] = vsp1_hgo_read(hgo, VI6_HGO_R_HISTO(i));
+                        data[i+64] = vsp1_hgo_read(hgo, VI6_HGO_G_HISTO(i));
+                        data[i+128] = vsp1_hgo_read(hgo, VI6_HGO_B_HISTO(i));
+                }
+
+                size = (6 + 64 * 3) * sizeof(u32);
+        }
+
+        vsp1_histogram_buffer_complete(&hgo->histo, buf, size);
+}
+
+/* -----------------------------------------------------------------------------
+ * Controls
+ */
+
+#define V4L2_CID_VSP1_HGO_MAX_RGB               (V4L2_CID_USER_BASE | 0x1001)
+#define V4L2_CID_VSP1_HGO_NUM_BINS              (V4L2_CID_USER_BASE | 0x1002)
+
+static const struct v4l2_ctrl_config hgo_max_rgb_control = {
+        .id = V4L2_CID_VSP1_HGO_MAX_RGB,
+        .name = "Maximum RGB Mode",
+        .type = V4L2_CTRL_TYPE_BOOLEAN,
+        .min = 0,
+        .max = 1,
+        .def = 0,
+        .step = 1,
+        .flags = V4L2_CTRL_FLAG_MODIFY_LAYOUT,
+};
+
+static const s64 hgo_num_bins[] = {
+        64, 256,
+};
+
+static const struct v4l2_ctrl_config hgo_num_bins_control = {
+        .id = V4L2_CID_VSP1_HGO_NUM_BINS,
+        .name = "Number of Bins",
+        .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+        .min = 0,
+        .max = 1,
+        .def = 0,
+        .qmenu_int = hgo_num_bins,
+        .flags = V4L2_CTRL_FLAG_MODIFY_LAYOUT,
+};
+
+/* -----------------------------------------------------------------------------
+ * VSP1 Entity Operations
+ */
+
+static void hgo_configure(struct vsp1_entity *entity,
+                          struct vsp1_pipeline *pipe,
+                          struct vsp1_dl_list *dl,
+                          enum vsp1_entity_params params)
+{
+        struct vsp1_hgo *hgo = to_hgo(&entity->subdev);
+        struct v4l2_rect *compose;
+        struct v4l2_rect *crop;
+        unsigned int hratio;
+        unsigned int vratio;
+
+        if (params != VSP1_ENTITY_PARAMS_INIT)
+                return;
+
+        crop = vsp1_entity_get_pad_selection(entity, entity->config,
+                                             HISTO_PAD_SINK, V4L2_SEL_TGT_CROP);
+        compose = vsp1_entity_get_pad_selection(entity, entity->config,
+                                                HISTO_PAD_SINK,
+                                                V4L2_SEL_TGT_COMPOSE);
+
+        vsp1_hgo_write(hgo, dl, VI6_HGO_REGRST, VI6_HGO_REGRST_RCLEA);
+
+        vsp1_hgo_write(hgo, dl, VI6_HGO_OFFSET,
+                       (crop->left << VI6_HGO_OFFSET_HOFFSET_SHIFT) |
+                       (crop->top << VI6_HGO_OFFSET_VOFFSET_SHIFT));
+        vsp1_hgo_write(hgo, dl, VI6_HGO_SIZE,
+                       (crop->width << VI6_HGO_SIZE_HSIZE_SHIFT) |
+                       (crop->height << VI6_HGO_SIZE_VSIZE_SHIFT));
+
+        mutex_lock(hgo->ctrls.handler.lock);
+        hgo->max_rgb = hgo->ctrls.max_rgb->cur.val;
+        if (hgo->ctrls.num_bins)
+                hgo->num_bins = hgo_num_bins[hgo->ctrls.num_bins->cur.val];
+        mutex_unlock(hgo->ctrls.handler.lock);
+
+        hratio = crop->width * 2 / compose->width / 3;
+        vratio = crop->height * 2 / compose->height / 3;
+        vsp1_hgo_write(hgo, dl, VI6_HGO_MODE,
+                       (hgo->num_bins == 256 ? VI6_HGO_MODE_STEP : 0) |
+                       (hgo->max_rgb ? VI6_HGO_MODE_MAXRGB : 0) |
+                       (hratio << VI6_HGO_MODE_HRATIO_SHIFT) |
+                       (vratio << VI6_HGO_MODE_VRATIO_SHIFT));
+}
+
+static const struct vsp1_entity_operations hgo_entity_ops = {
+        .configure = hgo_configure,
+        .destroy = vsp1_histogram_destroy,
+};
+
+/* -----------------------------------------------------------------------------
+ * Initialization and Cleanup
+ */
+
+static const unsigned int hgo_mbus_formats[] = {
+        MEDIA_BUS_FMT_AYUV8_1X32,
+        MEDIA_BUS_FMT_ARGB8888_1X32,
+        MEDIA_BUS_FMT_AHSV8888_1X32,
+};
+
+struct vsp1_hgo *vsp1_hgo_create(struct vsp1_device *vsp1)
+{
+        struct vsp1_hgo *hgo;
+        int ret;
+
+        hgo = devm_kzalloc(vsp1->dev, sizeof(*hgo), GFP_KERNEL);
+        if (hgo == NULL)
+                return ERR_PTR(-ENOMEM);
+
+        /* Initialize the control handler. */
+        v4l2_ctrl_handler_init(&hgo->ctrls.handler,
+                               vsp1->info->gen == 3 ? 2 : 1);
+        hgo->ctrls.max_rgb = v4l2_ctrl_new_custom(&hgo->ctrls.handler,
+                                                  &hgo_max_rgb_control, NULL);
+        if (vsp1->info->gen == 3)
+                hgo->ctrls.num_bins =
+                        v4l2_ctrl_new_custom(&hgo->ctrls.handler,
+                                             &hgo_num_bins_control, NULL);
+
+        hgo->max_rgb = false;
+        hgo->num_bins = 64;
+
+        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/vsp1/vsp1_hgo.h b/drivers/media/platform/vsp1/vsp1_hgo.h
new file mode 100644
index 000000000000..c6c0b7a80e0c
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_hgo.h
@@ -0,0 +1,45 @@
+/*
+ * vsp1_hgo.h  --  R-Car VSP1 Histogram Generator 1D
+ *
+ * Copyright (C) 2016 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#ifndef __VSP1_HGO_H__
+#define __VSP1_HGO_H__
+
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+
+#include "vsp1_histo.h"
+
+struct vsp1_device;
+
+struct vsp1_hgo {
+        struct vsp1_histogram histo;
+
+        struct {
+                struct v4l2_ctrl_handler handler;
+                struct v4l2_ctrl *max_rgb;
+                struct v4l2_ctrl *num_bins;
+        } ctrls;
+
+        bool max_rgb;
+        unsigned int num_bins;
+};
+
+static inline struct vsp1_hgo *to_hgo(struct v4l2_subdev *subdev)
+{
+        return container_of(subdev, struct vsp1_hgo, histo.entity.subdev);
+}
+
+struct vsp1_hgo *vsp1_hgo_create(struct vsp1_device *vsp1);
+void vsp1_hgo_frame_end(struct vsp1_entity *hgo);
+
+#endif /* __VSP1_HGO_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_hgt.c b/drivers/media/platform/vsp1/vsp1_hgt.c
new file mode 100644
index 000000000000..b5ce305e3e6f
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_hgt.c
@@ -0,0 +1,222 @@
+/*
+ * vsp1_hgt.c  --  R-Car VSP1 Histogram Generator 2D
+ *
+ * Copyright (C) 2016 Renesas Electronics Corporation
+ *
+ * Contact: Niklas Söderlund (niklas.soderlund@ragnatech.se)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/device.h>
+#include <linux/gfp.h>
+
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-vmalloc.h>
+
+#include "vsp1.h"
+#include "vsp1_dl.h"
+#include "vsp1_hgt.h"
+
+#define HGT_DATA_SIZE                           ((2 +  6 * 32) * 4)
+
+/* -----------------------------------------------------------------------------
+ * Device Access
+ */
+
+static inline u32 vsp1_hgt_read(struct vsp1_hgt *hgt, u32 reg)
+{
+        return vsp1_read(hgt->histo.entity.vsp1, reg);
+}
+
+static inline void vsp1_hgt_write(struct vsp1_hgt *hgt, struct vsp1_dl_list *dl,
+                                  u32 reg, u32 data)
+{
+        vsp1_dl_list_write(dl, reg, data);
+}
+
+/* -----------------------------------------------------------------------------
+ * Frame End Handler
+ */
+
+void vsp1_hgt_frame_end(struct vsp1_entity *entity)
+{
+        struct vsp1_hgt *hgt = to_hgt(&entity->subdev);
+        struct vsp1_histogram_buffer *buf;
+        unsigned int m;
+        unsigned int n;
+        u32 *data;
+
+        buf = vsp1_histogram_buffer_get(&hgt->histo);
+        if (!buf)
+                return;
+
+        data = buf->addr;
+
+        *data++ = vsp1_hgt_read(hgt, VI6_HGT_MAXMIN);
+        *data++ = vsp1_hgt_read(hgt, VI6_HGT_SUM);
+
+        for (m = 0; m < 6; ++m)
+                for (n = 0; n < 32; ++n)
+                        *data++ = vsp1_hgt_read(hgt, VI6_HGT_HISTO(m, n));
+
+        vsp1_histogram_buffer_complete(&hgt->histo, buf, HGT_DATA_SIZE);
+}
+
+/* -----------------------------------------------------------------------------
+ * Controls
+ */
+
+#define V4L2_CID_VSP1_HGT_HUE_AREAS     (V4L2_CID_USER_BASE | 0x1001)
+
+static int hgt_hue_areas_try_ctrl(struct v4l2_ctrl *ctrl)
+{
+        const u8 *values = ctrl->p_new.p_u8;
+        unsigned int i;
+
+        /*
+         * The hardware has constraints on the hue area boundaries beyond the
+         * control min, max and step. The values must match one of the following
+         * expressions.
+         *
+         * 0L <= 0U <= 1L <= 1U <= 2L <= 2U <= 3L <= 3U <= 4L <= 4U <= 5L <= 5U
+         * 0U <= 1L <= 1U <= 2L <= 2U <= 3L <= 3U <= 4L <= 4U <= 5L <= 5U <= 0L
+         *
+         * Start by verifying the common part...
+         */
+        for (i = 1; i < (HGT_NUM_HUE_AREAS * 2) - 1; ++i) {
+                if (values[i] > values[i+1])
+                        return -EINVAL;
+        }
+
+        /* ... and handle 0L separately. */
+        if (values[0] > values[1] && values[11] > values[0])
+                return -EINVAL;
+
+        return 0;
+}
+
+static int hgt_hue_areas_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct vsp1_hgt *hgt = container_of(ctrl->handler, struct vsp1_hgt,
+                                            ctrls);
+
+        memcpy(hgt->hue_areas, ctrl->p_new.p_u8, sizeof(hgt->hue_areas));
+        return 0;
+}
+
+static const struct v4l2_ctrl_ops hgt_hue_areas_ctrl_ops = {
+        .try_ctrl = hgt_hue_areas_try_ctrl,
+        .s_ctrl = hgt_hue_areas_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config hgt_hue_areas = {
+        .ops = &hgt_hue_areas_ctrl_ops,
+        .id = V4L2_CID_VSP1_HGT_HUE_AREAS,
+        .name = "Boundary Values for Hue Area",
+        .type = V4L2_CTRL_TYPE_U8,
+        .min = 0,
+        .max = 255,
+        .def = 0,
+        .step = 1,
+        .dims = { 12 },
+};
+
+/* -----------------------------------------------------------------------------
+ * VSP1 Entity Operations
+ */
+
+static void hgt_configure(struct vsp1_entity *entity,
+                          struct vsp1_pipeline *pipe,
+                          struct vsp1_dl_list *dl,
+                          enum vsp1_entity_params params)
+{
+        struct vsp1_hgt *hgt = to_hgt(&entity->subdev);
+        struct v4l2_rect *compose;
+        struct v4l2_rect *crop;
+        unsigned int hratio;
+        unsigned int vratio;
+        u8 lower;
+        u8 upper;
+        unsigned int i;
+
+        if (params != VSP1_ENTITY_PARAMS_INIT)
+                return;
+
+        crop = vsp1_entity_get_pad_selection(entity, entity->config,
+                                             HISTO_PAD_SINK, V4L2_SEL_TGT_CROP);
+        compose = vsp1_entity_get_pad_selection(entity, entity->config,
+                                                HISTO_PAD_SINK,
+                                                V4L2_SEL_TGT_COMPOSE);
+
+        vsp1_hgt_write(hgt, dl, VI6_HGT_REGRST, VI6_HGT_REGRST_RCLEA);
+
+        vsp1_hgt_write(hgt, dl, VI6_HGT_OFFSET,
+                       (crop->left << VI6_HGT_OFFSET_HOFFSET_SHIFT) |
+                       (crop->top << VI6_HGT_OFFSET_VOFFSET_SHIFT));
+        vsp1_hgt_write(hgt, dl, VI6_HGT_SIZE,
+                       (crop->width << VI6_HGT_SIZE_HSIZE_SHIFT) |
+                       (crop->height << VI6_HGT_SIZE_VSIZE_SHIFT));
+
+        mutex_lock(hgt->ctrls.lock);
+        for (i = 0; i < HGT_NUM_HUE_AREAS; ++i) {
+                lower = hgt->hue_areas[i*2 + 0];
+                upper = hgt->hue_areas[i*2 + 1];
+                vsp1_hgt_write(hgt, dl, VI6_HGT_HUE_AREA(i),
+                               (lower << VI6_HGT_HUE_AREA_LOWER_SHIFT) |
+                               (upper << VI6_HGT_HUE_AREA_UPPER_SHIFT));
+        }
+        mutex_unlock(hgt->ctrls.lock);
+
+        hratio = crop->width * 2 / compose->width / 3;
+        vratio = crop->height * 2 / compose->height / 3;
+        vsp1_hgt_write(hgt, dl, VI6_HGT_MODE,
+                       (hratio << VI6_HGT_MODE_HRATIO_SHIFT) |
+                       (vratio << VI6_HGT_MODE_VRATIO_SHIFT));
+}
+
+static const struct vsp1_entity_operations hgt_entity_ops = {
+        .configure = hgt_configure,
+        .destroy = vsp1_histogram_destroy,
+};
+
+/* -----------------------------------------------------------------------------
+ * Initialization and Cleanup
+ */
+
+static const unsigned int hgt_mbus_formats[] = {
+        MEDIA_BUS_FMT_AHSV8888_1X32,
+};
+
+struct vsp1_hgt *vsp1_hgt_create(struct vsp1_device *vsp1)
+{
+        struct vsp1_hgt *hgt;
+        int ret;
+
+        hgt = devm_kzalloc(vsp1->dev, sizeof(*hgt), GFP_KERNEL);
+        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,
+                                  ARRAY_SIZE(hgt_mbus_formats),
+                                  HGT_DATA_SIZE, V4L2_META_FMT_VSP1_HGT);
+        if (ret < 0) {
+                vsp1_entity_destroy(&hgt->histo.entity);
+                return ERR_PTR(ret);
+        }
+
+        v4l2_ctrl_handler_setup(&hgt->ctrls);
+
+        return hgt;
+}
diff --git a/drivers/media/platform/vsp1/vsp1_hgt.h b/drivers/media/platform/vsp1/vsp1_hgt.h
new file mode 100644
index 000000000000..83f2e130942a
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_hgt.h
@@ -0,0 +1,42 @@
+/*
+ * vsp1_hgt.h  --  R-Car VSP1 Histogram Generator 2D
+ *
+ * Copyright (C) 2016 Renesas Electronics Corporation
+ *
+ * Contact: Niklas Söderlund (niklas.soderlund@ragnatech.se)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#ifndef __VSP1_HGT_H__
+#define __VSP1_HGT_H__
+
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+
+#include "vsp1_histo.h"
+
+struct vsp1_device;
+
+#define HGT_NUM_HUE_AREAS                       6
+
+struct vsp1_hgt {
+        struct vsp1_histogram histo;
+
+        struct v4l2_ctrl_handler ctrls;
+
+        u8 hue_areas[HGT_NUM_HUE_AREAS * 2];
+};
+
+static inline struct vsp1_hgt *to_hgt(struct v4l2_subdev *subdev)
+{
+        return container_of(subdev, struct vsp1_hgt, histo.entity.subdev);
+}
+
+struct vsp1_hgt *vsp1_hgt_create(struct vsp1_device *vsp1);
+void vsp1_hgt_frame_end(struct vsp1_entity *hgt);
+
+#endif /* __VSP1_HGT_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_histo.c b/drivers/media/platform/vsp1/vsp1_histo.c
new file mode 100644
index 000000000000..afab77cf4fa5
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_histo.c
@@ -0,0 +1,646 @@
+/*
+ * vsp1_histo.c  --  R-Car VSP1 Histogram API
+ *
+ * Copyright (C) 2016 Renesas Electronics Corporation
+ * Copyright (C) 2016 Laurent Pinchart
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/device.h>
+#include <linux/gfp.h>
+
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-vmalloc.h>
+
+#include "vsp1.h"
+#include "vsp1_histo.h"
+#include "vsp1_pipe.h"
+
+#define HISTO_MIN_SIZE                          4U
+#define HISTO_MAX_SIZE                          8192U
+
+/* -----------------------------------------------------------------------------
+ * Buffer Operations
+ */
+
+static inline struct vsp1_histogram_buffer *
+to_vsp1_histogram_buffer(struct vb2_v4l2_buffer *vbuf)
+{
+        return container_of(vbuf, struct vsp1_histogram_buffer, buf);
+}
+
+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);
+
+        if (list_empty(&histo->irqqueue))
+                goto done;
+
+        buf = list_first_entry(&histo->irqqueue, struct vsp1_histogram_buffer,
+                               queue);
+        list_del(&buf->queue);
+        histo->readout = true;
+
+done:
+        spin_unlock_irqrestore(&histo->irqlock, flags);
+        return buf;
+}
+
+void vsp1_histogram_buffer_complete(struct vsp1_histogram *histo,
+                                    struct vsp1_histogram_buffer *buf,
+                                    size_t size)
+{
+        struct vsp1_pipeline *pipe = histo->pipe;
+        unsigned long flags;
+
+        /*
+         * The pipeline pointer is guaranteed to be valid as this function is
+         * called from the frame completion interrupt handler, which can only
+         * occur when video streaming is active.
+         */
+        buf->buf.sequence = pipe->sequence;
+        buf->buf.vb2_buf.timestamp = ktime_get_ns();
+        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);
+        histo->readout = false;
+        wake_up(&histo->wait_queue);
+        spin_unlock_irqrestore(&histo->irqlock, flags);
+}
+
+/* -----------------------------------------------------------------------------
+ * videobuf2 Queue Operations
+ */
+
+static int histo_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+                             unsigned int *nplanes, unsigned int sizes[],
+                             struct device *alloc_devs[])
+{
+        struct vsp1_histogram *histo = vb2_get_drv_priv(vq);
+
+        if (*nplanes) {
+                if (*nplanes != 1)
+                        return -EINVAL;
+
+                if (sizes[0] < histo->data_size)
+                        return -EINVAL;
+
+                return 0;
+        }
+
+        *nplanes = 1;
+        sizes[0] = histo->data_size;
+
+        return 0;
+}
+
+static int histo_buffer_prepare(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);
+
+        if (vb->num_planes != 1)
+                return -EINVAL;
+
+        if (vb2_plane_size(vb, 0) < histo->data_size)
+                return -EINVAL;
+
+        buf->addr = vb2_plane_vaddr(vb, 0);
+
+        return 0;
+}
+
+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);
+        list_add_tail(&buf->queue, &histo->irqqueue);
+        spin_unlock_irqrestore(&histo->irqlock, flags);
+}
+
+static int histo_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+        return 0;
+}
+
+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);
+
+        /* Remove all buffers from the IRQ queue. */
+        list_for_each_entry(buffer, &histo->irqqueue, queue)
+                vb2_buffer_done(&buffer->buf.vb2_buf, VB2_BUF_STATE_ERROR);
+        INIT_LIST_HEAD(&histo->irqqueue);
+
+        /* 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);
+}
+
+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,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Operations
+ */
+
+static int histo_enum_mbus_code(struct v4l2_subdev *subdev,
+                                struct v4l2_subdev_pad_config *cfg,
+                                struct v4l2_subdev_mbus_code_enum *code)
+{
+        struct vsp1_histogram *histo = subdev_to_histo(subdev);
+
+        if (code->pad == HISTO_PAD_SOURCE) {
+                code->code = MEDIA_BUS_FMT_FIXED;
+                return 0;
+        }
+
+        return vsp1_subdev_enum_mbus_code(subdev, cfg, code, histo->formats,
+                                          histo->num_formats);
+}
+
+static int histo_enum_frame_size(struct v4l2_subdev *subdev,
+                                 struct v4l2_subdev_pad_config *cfg,
+                                 struct v4l2_subdev_frame_size_enum *fse)
+{
+        if (fse->pad != HISTO_PAD_SINK)
+                return -EINVAL;
+
+        return vsp1_subdev_enum_frame_size(subdev, cfg, fse, HISTO_MIN_SIZE,
+                                           HISTO_MIN_SIZE, HISTO_MAX_SIZE,
+                                           HISTO_MAX_SIZE);
+}
+
+static int histo_get_selection(struct v4l2_subdev *subdev,
+                               struct v4l2_subdev_pad_config *cfg,
+                               struct v4l2_subdev_selection *sel)
+{
+        struct vsp1_histogram *histo = subdev_to_histo(subdev);
+        struct v4l2_subdev_pad_config *config;
+        struct v4l2_mbus_framefmt *format;
+        struct v4l2_rect *crop;
+        int ret = 0;
+
+        if (sel->pad != HISTO_PAD_SINK)
+                return -EINVAL;
+
+        mutex_lock(&histo->entity.lock);
+
+        config = vsp1_entity_get_pad_config(&histo->entity, cfg, sel->which);
+        if (!config) {
+                ret = -EINVAL;
+                goto done;
+        }
+
+        switch (sel->target) {
+        case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+        case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+                crop = vsp1_entity_get_pad_selection(&histo->entity, config,
+                                                     HISTO_PAD_SINK,
+                                                     V4L2_SEL_TGT_CROP);
+                sel->r.left = 0;
+                sel->r.top = 0;
+                sel->r.width = crop->width;
+                sel->r.height = crop->height;
+                break;
+
+        case V4L2_SEL_TGT_CROP_BOUNDS:
+        case V4L2_SEL_TGT_CROP_DEFAULT:
+                format = vsp1_entity_get_pad_format(&histo->entity, config,
+                                                    HISTO_PAD_SINK);
+                sel->r.left = 0;
+                sel->r.top = 0;
+                sel->r.width = format->width;
+                sel->r.height = format->height;
+                break;
+
+        case V4L2_SEL_TGT_COMPOSE:
+        case V4L2_SEL_TGT_CROP:
+                sel->r = *vsp1_entity_get_pad_selection(&histo->entity, config,
+                                                        sel->pad, sel->target);
+                break;
+
+        default:
+                ret = -EINVAL;
+                break;
+        }
+
+done:
+        mutex_unlock(&histo->entity.lock);
+        return ret;
+}
+
+static int histo_set_crop(struct v4l2_subdev *subdev,
+                          struct v4l2_subdev_pad_config *config,
+                         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, config,
+                                            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,
+                               format->width - sel->r.left);
+        sel->r.height = clamp_t(unsigned int, sel->r.height, HISTO_MIN_SIZE,
+                                format->height - sel->r.top);
+
+        /* Set the crop rectangle and reset the compose rectangle. */
+        selection = vsp1_entity_get_pad_selection(&histo->entity, config,
+                                                  sel->pad, V4L2_SEL_TGT_CROP);
+        *selection = sel->r;
+
+        selection = vsp1_entity_get_pad_selection(&histo->entity, config,
+                                                  sel->pad,
+                                                  V4L2_SEL_TGT_COMPOSE);
+        *selection = sel->r;
+
+        return 0;
+}
+
+static int histo_set_compose(struct v4l2_subdev *subdev,
+                             struct v4l2_subdev_pad_config *config,
+                             struct v4l2_subdev_selection *sel)
+{
+        struct vsp1_histogram *histo = subdev_to_histo(subdev);
+        struct v4l2_rect *compose;
+        struct v4l2_rect *crop;
+        unsigned int ratio;
+
+        /*
+         * The compose rectangle is used to configure downscaling, the top left
+         * corner is fixed to (0,0) and the size to 1/2 or 1/4 of the crop
+         * rectangle.
+         */
+        sel->r.left = 0;
+        sel->r.top = 0;
+
+        crop = vsp1_entity_get_pad_selection(&histo->entity, config, sel->pad,
+                                             V4L2_SEL_TGT_CROP);
+
+        /*
+         * Clamp the width and height to acceptable values first and then
+         * compute the closest rounded dividing ratio.
+         *
+         * Ratio        Rounded ratio
+         * --------------------------
+         * [1.0 1.5[    1
+         * [1.5 3.0[    2
+         * [3.0 4.0]    4
+         *
+         * The rounded ratio can be computed using
+         *
+         * 1 << (ceil(ratio * 2) / 3)
+         */
+        sel->r.width = clamp(sel->r.width, crop->width / 4, crop->width);
+        ratio = 1 << (crop->width * 2 / sel->r.width / 3);
+        sel->r.width = crop->width / ratio;
+
+
+        sel->r.height = clamp(sel->r.height, crop->height / 4, crop->height);
+        ratio = 1 << (crop->height * 2 / sel->r.height / 3);
+        sel->r.height = crop->height / ratio;
+
+        compose = vsp1_entity_get_pad_selection(&histo->entity, config,
+                                                sel->pad,
+                                                V4L2_SEL_TGT_COMPOSE);
+        *compose = sel->r;
+
+        return 0;
+}
+
+static int histo_set_selection(struct v4l2_subdev *subdev,
+                               struct v4l2_subdev_pad_config *cfg,
+                               struct v4l2_subdev_selection *sel)
+{
+        struct vsp1_histogram *histo = subdev_to_histo(subdev);
+        struct v4l2_subdev_pad_config *config;
+        int ret;
+
+        if (sel->pad != HISTO_PAD_SINK)
+                return -EINVAL;
+
+        mutex_lock(&histo->entity.lock);
+
+        config = vsp1_entity_get_pad_config(&histo->entity, cfg, sel->which);
+        if (!config) {
+                ret = -EINVAL;
+                goto done;
+        }
+
+        if (sel->target == V4L2_SEL_TGT_CROP)
+                ret = histo_set_crop(subdev, config, sel);
+        else if (sel->target == V4L2_SEL_TGT_COMPOSE)
+                ret = histo_set_compose(subdev, config, sel);
+        else
+                ret = -EINVAL;
+
+done:
+        mutex_unlock(&histo->entity.lock);
+        return ret;
+}
+
+static int histo_get_format(struct v4l2_subdev *subdev,
+                            struct v4l2_subdev_pad_config *cfg,
+                            struct v4l2_subdev_format *fmt)
+{
+        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, cfg, fmt);
+}
+
+static int histo_set_format(struct v4l2_subdev *subdev,
+                            struct v4l2_subdev_pad_config *cfg,
+                            struct v4l2_subdev_format *fmt)
+{
+        struct vsp1_histogram *histo = subdev_to_histo(subdev);
+        struct v4l2_subdev_pad_config *config;
+        struct v4l2_mbus_framefmt *format;
+        struct v4l2_rect *selection;
+        unsigned int i;
+        int ret = 0;
+
+        if (fmt->pad != HISTO_PAD_SINK)
+                return histo_get_format(subdev, cfg, fmt);
+
+        mutex_lock(&histo->entity.lock);
+
+        config = vsp1_entity_get_pad_config(&histo->entity, cfg, fmt->which);
+        if (!config) {
+                ret = -EINVAL;
+                goto done;
+        }
+
+        /*
+         * Default to the first format if the requested format is not
+         * supported.
+         */
+        for (i = 0; i < histo->num_formats; ++i) {
+                if (fmt->format.code == histo->formats[i])
+                        break;
+        }
+        if (i == histo->num_formats)
+                fmt->format.code = histo->formats[0];
+
+        format = vsp1_entity_get_pad_format(&histo->entity, config, fmt->pad);
+
+        format->code = fmt->format.code;
+        format->width = clamp_t(unsigned int, fmt->format.width,
+                                HISTO_MIN_SIZE, HISTO_MAX_SIZE);
+        format->height = clamp_t(unsigned int, fmt->format.height,
+                                 HISTO_MIN_SIZE, HISTO_MAX_SIZE);
+        format->field = V4L2_FIELD_NONE;
+        format->colorspace = V4L2_COLORSPACE_SRGB;
+
+        fmt->format = *format;
+
+        /* Reset the crop and compose rectangles */
+        selection = vsp1_entity_get_pad_selection(&histo->entity, config,
+                                                  fmt->pad, V4L2_SEL_TGT_CROP);
+        selection->left = 0;
+        selection->top = 0;
+        selection->width = format->width;
+        selection->height = format->height;
+
+        selection = vsp1_entity_get_pad_selection(&histo->entity, config,
+                                                  fmt->pad,
+                                                  V4L2_SEL_TGT_COMPOSE);
+        selection->left = 0;
+        selection->top = 0;
+        selection->width = format->width;
+        selection->height = format->height;
+
+done:
+        mutex_unlock(&histo->entity.lock);
+        return ret;
+}
+
+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,
+        .set_fmt = histo_set_format,
+        .get_selection = histo_get_selection,
+        .set_selection = histo_set_selection,
+};
+
+static const struct v4l2_subdev_ops histo_ops = {
+        .pad    = &histo_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 ioctls
+ */
+
+static int histo_v4l2_querycap(struct file *file, void *fh,
+                               struct v4l2_capability *cap)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct vsp1_histogram *histo = vdev_to_histo(vfh->vdev);
+
+        cap->capabilities = V4L2_CAP_DEVICE_CAPS | V4L2_CAP_STREAMING
+                          | V4L2_CAP_VIDEO_CAPTURE_MPLANE
+                          | V4L2_CAP_VIDEO_OUTPUT_MPLANE
+                          | V4L2_CAP_META_CAPTURE;
+        cap->device_caps = V4L2_CAP_META_CAPTURE
+                         | V4L2_CAP_STREAMING;
+
+        strlcpy(cap->driver, "vsp1", sizeof(cap->driver));
+        strlcpy(cap->card, histo->video.name, sizeof(cap->card));
+        snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+                 dev_name(histo->entity.vsp1->dev));
+
+        return 0;
+}
+
+static int histo_v4l2_enum_format(struct file *file, void *fh,
+                                  struct v4l2_fmtdesc *f)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct vsp1_histogram *histo = vdev_to_histo(vfh->vdev);
+
+        if (f->index > 0 || f->type != histo->queue.type)
+                return -EINVAL;
+
+        f->pixelformat = histo->meta_format;
+
+        return 0;
+}
+
+static int histo_v4l2_get_format(struct file *file, void *fh,
+                                 struct v4l2_format *format)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct vsp1_histogram *histo = vdev_to_histo(vfh->vdev);
+        struct v4l2_meta_format *meta = &format->fmt.meta;
+
+        if (format->type != histo->queue.type)
+                return -EINVAL;
+
+        memset(meta, 0, sizeof(*meta));
+
+        meta->dataformat = histo->meta_format;
+        meta->buffersize = histo->data_size;
+
+        return 0;
+}
+
+static const struct v4l2_ioctl_ops histo_v4l2_ioctl_ops = {
+        .vidioc_querycap                = histo_v4l2_querycap,
+        .vidioc_enum_fmt_meta_cap       = histo_v4l2_enum_format,
+        .vidioc_g_fmt_meta_cap          = histo_v4l2_get_format,
+        .vidioc_s_fmt_meta_cap          = histo_v4l2_get_format,
+        .vidioc_try_fmt_meta_cap        = histo_v4l2_get_format,
+        .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
+        .vidioc_querybuf                = vb2_ioctl_querybuf,
+        .vidioc_qbuf                    = vb2_ioctl_qbuf,
+        .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
+        .vidioc_create_bufs             = vb2_ioctl_create_bufs,
+        .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
+        .vidioc_streamon                = vb2_ioctl_streamon,
+        .vidioc_streamoff               = vb2_ioctl_streamoff,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 File Operations
+ */
+
+static const struct v4l2_file_operations histo_v4l2_fops = {
+        .owner = THIS_MODULE,
+        .unlocked_ioctl = video_ioctl2,
+        .open = v4l2_fh_open,
+        .release = vb2_fop_release,
+        .poll = vb2_fop_poll,
+        .mmap = vb2_fop_mmap,
+};
+
+static void vsp1_histogram_cleanup(struct vsp1_histogram *histo)
+{
+        if (video_is_registered(&histo->video))
+                video_unregister_device(&histo->video);
+
+        media_entity_cleanup(&histo->video.entity);
+}
+
+void vsp1_histogram_destroy(struct vsp1_entity *entity)
+{
+        struct vsp1_histogram *histo = subdev_to_histo(&entity->subdev);
+
+        vsp1_histogram_cleanup(histo);
+}
+
+int vsp1_histogram_init(struct vsp1_device *vsp1, struct vsp1_histogram *histo,
+                        enum vsp1_entity_type type, const char *name,
+                        const struct vsp1_entity_operations *ops,
+                        const unsigned int *formats, unsigned int num_formats,
+                        size_t data_size, u32 meta_format)
+{
+        int ret;
+
+        histo->formats = formats;
+        histo->num_formats = num_formats;
+        histo->data_size = data_size;
+        histo->meta_format = meta_format;
+
+        histo->pad.flags = MEDIA_PAD_FL_SINK;
+        histo->video.vfl_dir = VFL_DIR_RX;
+
+        mutex_init(&histo->lock);
+        spin_lock_init(&histo->irqlock);
+        INIT_LIST_HEAD(&histo->irqqueue);
+        init_waitqueue_head(&histo->wait_queue);
+
+        /* Initialize the VSP entity... */
+        histo->entity.ops = ops;
+        histo->entity.type = type;
+
+        ret = vsp1_entity_init(vsp1, &histo->entity, name, 2, &histo_ops,
+                               MEDIA_ENT_F_PROC_VIDEO_STATISTICS);
+        if (ret < 0)
+                return ret;
+
+        /* ... and the media entity... */
+        ret = media_entity_pads_init(&histo->video.entity, 1, &histo->pad);
+        if (ret < 0)
+                return ret;
+
+        /* ... and the video node... */
+        histo->video.v4l2_dev = &vsp1->v4l2_dev;
+        histo->video.fops = &histo_v4l2_fops;
+        snprintf(histo->video.name, sizeof(histo->video.name),
+                 "%s histo", histo->entity.subdev.name);
+        histo->video.vfl_type = VFL_TYPE_GRABBER;
+        histo->video.release = video_device_release_empty;
+        histo->video.ioctl_ops = &histo_v4l2_ioctl_ops;
+
+        video_set_drvdata(&histo->video, histo);
+
+        /* ... and the buffers queue... */
+        histo->queue.type = V4L2_BUF_TYPE_META_CAPTURE;
+        histo->queue.io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+        histo->queue.lock = &histo->lock;
+        histo->queue.drv_priv = histo;
+        histo->queue.buf_struct_size = sizeof(struct vsp1_histogram_buffer);
+        histo->queue.ops = &histo_video_queue_qops;
+        histo->queue.mem_ops = &vb2_vmalloc_memops;
+        histo->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+        histo->queue.dev = vsp1->dev;
+        ret = vb2_queue_init(&histo->queue);
+        if (ret < 0) {
+                dev_err(vsp1->dev, "failed to initialize vb2 queue\n");
+                goto error;
+        }
+
+        /* ... and register the video device. */
+        histo->video.queue = &histo->queue;
+        ret = video_register_device(&histo->video, VFL_TYPE_GRABBER, -1);
+        if (ret < 0) {
+                dev_err(vsp1->dev, "failed to register video device\n");
+                goto error;
+        }
+
+        return 0;
+
+error:
+        vsp1_histogram_cleanup(histo);
+        return ret;
+}
diff --git a/drivers/media/platform/vsp1/vsp1_histo.h b/drivers/media/platform/vsp1/vsp1_histo.h
new file mode 100644
index 000000000000..af2874f6031d
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_histo.h
@@ -0,0 +1,84 @@
+/*
+ * vsp1_histo.h  --  R-Car VSP1 Histogram API
+ *
+ * Copyright (C) 2016 Renesas Electronics Corporation
+ * Copyright (C) 2016 Laurent Pinchart
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#ifndef __VSP1_HISTO_H__
+#define __VSP1_HISTO_H__
+
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-dev.h>
+#include <media/videobuf2-v4l2.h>
+
+#include "vsp1_entity.h"
+
+struct vsp1_device;
+struct vsp1_pipeline;
+
+#define HISTO_PAD_SINK                          0
+#define HISTO_PAD_SOURCE                        1
+
+struct vsp1_histogram_buffer {
+        struct vb2_v4l2_buffer buf;
+        struct list_head queue;
+        void *addr;
+};
+
+struct vsp1_histogram {
+        struct vsp1_pipeline *pipe;
+
+        struct vsp1_entity entity;
+        struct video_device video;
+        struct media_pad pad;
+
+        const u32 *formats;
+        unsigned int num_formats;
+        size_t data_size;
+        u32 meta_format;
+
+        struct mutex lock;
+        struct vb2_queue queue;
+
+        spinlock_t irqlock;
+        struct list_head irqqueue;
+
+        wait_queue_head_t wait_queue;
+        bool readout;
+};
+
+static inline struct vsp1_histogram *vdev_to_histo(struct video_device *vdev)
+{
+        return container_of(vdev, struct vsp1_histogram, video);
+}
+
+static inline struct vsp1_histogram *subdev_to_histo(struct v4l2_subdev *subdev)
+{
+        return container_of(subdev, struct vsp1_histogram, entity.subdev);
+}
+
+int vsp1_histogram_init(struct vsp1_device *vsp1, struct vsp1_histogram *histo,
+                        enum vsp1_entity_type type, const char *name,
+                        const struct vsp1_entity_operations *ops,
+                        const unsigned int *formats, unsigned int num_formats,
+                        size_t data_size, u32 meta_format);
+void vsp1_histogram_destroy(struct vsp1_entity *entity);
+
+struct vsp1_histogram_buffer *
+vsp1_histogram_buffer_get(struct vsp1_histogram *histo);
+void vsp1_histogram_buffer_complete(struct vsp1_histogram *histo,
+                                    struct vsp1_histogram_buffer *buf,
+                                    size_t size);
+
+#endif /* __VSP1_HISTO_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_hsit.c b/drivers/media/platform/vsp1/vsp1_hsit.c
index 94316afc54ff..764d405345ee 100644
--- a/drivers/media/platform/vsp1/vsp1_hsit.c
+++ b/drivers/media/platform/vsp1/vsp1_hsit.c
@@ -84,7 +84,8 @@ static int hsit_set_format(struct v4l2_subdev *subdev,
         format = vsp1_entity_get_pad_format(&hsit->entity, config, fmt->pad);
 
         if (fmt->pad == HSIT_PAD_SOURCE) {
-                /* The HST and HSI output format code and resolution can't be
+                /*
+                 * The HST and HSI output format code and resolution can't be
                  * modified.
                  */
                 fmt->format = *format;
diff --git a/drivers/media/platform/vsp1/vsp1_lif.c b/drivers/media/platform/vsp1/vsp1_lif.c
index e32acae1fc6e..702487f895b3 100644
--- a/drivers/media/platform/vsp1/vsp1_lif.c
+++ b/drivers/media/platform/vsp1/vsp1_lif.c
@@ -84,7 +84,8 @@ static int lif_set_format(struct v4l2_subdev *subdev,
         format = vsp1_entity_get_pad_format(&lif->entity, config, fmt->pad);
 
         if (fmt->pad == LIF_PAD_SOURCE) {
-                /* The LIF source format is always identical to its sink
+                /*
+                 * The LIF source format is always identical to its sink
                  * format.
                  */
                 fmt->format = *format;
@@ -176,7 +177,8 @@ struct vsp1_lif *vsp1_lif_create(struct vsp1_device *vsp1)
         lif->entity.ops = &lif_entity_ops;
         lif->entity.type = VSP1_ENTITY_LIF;
 
-        /* The LIF is never exposed to userspace, but media entity registration
+        /*
+         * The LIF 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.
          */
diff --git a/drivers/media/platform/vsp1/vsp1_pipe.c b/drivers/media/platform/vsp1/vsp1_pipe.c
index 280ba0804699..edebf3fa926f 100644
--- a/drivers/media/platform/vsp1/vsp1_pipe.c
+++ b/drivers/media/platform/vsp1/vsp1_pipe.c
@@ -23,6 +23,8 @@
 #include "vsp1_bru.h"
 #include "vsp1_dl.h"
 #include "vsp1_entity.h"
+#include "vsp1_hgo.h"
+#include "vsp1_hgt.h"
 #include "vsp1_pipe.h"
 #include "vsp1_rwpf.h"
 #include "vsp1_uds.h"
@@ -157,9 +159,15 @@ const struct vsp1_format_info *vsp1_get_format_info(struct vsp1_device *vsp1,
 {
         unsigned int i;
 
-        /* Special case, the VYUY format is supported on Gen2 only. */
-        if (vsp1->info->gen != 2 && fourcc == V4L2_PIX_FMT_VYUY)
-                return NULL;
+        /* 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];
@@ -198,11 +206,25 @@ void vsp1_pipeline_reset(struct vsp1_pipeline *pipe)
                 pipe->output = NULL;
         }
 
+        if (pipe->hgo) {
+                struct vsp1_hgo *hgo = to_hgo(&pipe->hgo->subdev);
+
+                hgo->histo.pipe = NULL;
+        }
+
+        if (pipe->hgt) {
+                struct vsp1_hgt *hgt = to_hgt(&pipe->hgt->subdev);
+
+                hgt->histo.pipe = NULL;
+        }
+
         INIT_LIST_HEAD(&pipe->entities);
         pipe->state = VSP1_PIPELINE_STOPPED;
         pipe->buffers_ready = 0;
         pipe->num_inputs = 0;
         pipe->bru = NULL;
+        pipe->hgo = NULL;
+        pipe->hgt = NULL;
         pipe->lif = NULL;
         pipe->uds = NULL;
 }
@@ -246,16 +268,17 @@ bool vsp1_pipeline_stopped(struct vsp1_pipeline *pipe)
 
 int vsp1_pipeline_stop(struct vsp1_pipeline *pipe)
 {
+        struct vsp1_device *vsp1 = pipe->output->entity.vsp1;
         struct vsp1_entity *entity;
         unsigned long flags;
         int ret;
 
         if (pipe->lif) {
-                /* When using display lists in continuous frame mode the only
+                /*
+                 * When using display lists in continuous frame mode the only
                  * way to stop the pipeline is to reset the hardware.
                  */
-                ret = vsp1_reset_wpf(pipe->output->entity.vsp1,
-                                     pipe->output->entity.index);
+                ret = vsp1_reset_wpf(vsp1, pipe->output->entity.index);
                 if (ret == 0) {
                         spin_lock_irqsave(&pipe->irqlock, flags);
                         pipe->state = VSP1_PIPELINE_STOPPED;
@@ -275,10 +298,20 @@ int vsp1_pipeline_stop(struct vsp1_pipeline *pipe)
 
         list_for_each_entry(entity, &pipe->entities, list_pipe) {
                 if (entity->route && entity->route->reg)
-                        vsp1_write(entity->vsp1, entity->route->reg,
+                        vsp1_write(vsp1, entity->route->reg,
                                    VI6_DPR_NODE_UNUSED);
         }
 
+        if (pipe->hgo)
+                vsp1_write(vsp1, VI6_DPR_HGO_SMPPT,
+                           (7 << VI6_DPR_SMPPT_TGW_SHIFT) |
+                           (VI6_DPR_NODE_UNUSED << VI6_DPR_SMPPT_PT_SHIFT));
+
+        if (pipe->hgt)
+                vsp1_write(vsp1, VI6_DPR_HGT_SMPPT,
+                           (7 << VI6_DPR_SMPPT_TGW_SHIFT) |
+                           (VI6_DPR_NODE_UNUSED << VI6_DPR_SMPPT_PT_SHIFT));
+
         v4l2_subdev_call(&pipe->output->entity.subdev, video, s_stream, 0);
 
         return ret;
@@ -302,6 +335,12 @@ void vsp1_pipeline_frame_end(struct vsp1_pipeline *pipe)
 
         vsp1_dlm_irq_frame_end(pipe->output->dlm);
 
+        if (pipe->hgo)
+                vsp1_hgo_frame_end(pipe->hgo);
+
+        if (pipe->hgt)
+                vsp1_hgt_frame_end(pipe->hgt);
+
         if (pipe->frame_end)
                 pipe->frame_end(pipe);
 
@@ -322,7 +361,8 @@ void vsp1_pipeline_propagate_alpha(struct vsp1_pipeline *pipe,
         if (!pipe->uds)
                 return;
 
-        /* The BRU background color has a fixed alpha value set to 255, the
+        /*
+         * The BRU background color has a fixed alpha value set to 255, the
          * output alpha value is thus always equal to 255.
          */
         if (pipe->uds_input->type == VSP1_ENTITY_BRU)
@@ -337,7 +377,8 @@ void vsp1_pipelines_suspend(struct vsp1_device *vsp1)
         unsigned int i;
         int ret;
 
-        /* To avoid increasing the system suspend time needlessly, loop over the
+        /*
+         * To avoid increasing the system suspend time needlessly, loop over the
          * pipelines twice, first to set them all to the stopping state, and
          * then to wait for the stop to complete.
          */
diff --git a/drivers/media/platform/vsp1/vsp1_pipe.h b/drivers/media/platform/vsp1/vsp1_pipe.h
index ac4ad2655551..91a784a13422 100644
--- a/drivers/media/platform/vsp1/vsp1_pipe.h
+++ b/drivers/media/platform/vsp1/vsp1_pipe.h
@@ -25,11 +25,12 @@ struct vsp1_rwpf;
 
 /*
  * struct vsp1_format_info - VSP1 video format description
- * @mbus: media bus format code
  * @fourcc: V4L2 pixel format FCC identifier
+ * @mbus: media bus format code
+ * @hwfmt: VSP1 hardware format
+ * @swap: swap register control
  * @planes: number of planes
  * @bpp: bits per pixel
- * @hwfmt: VSP1 hardware format
  * @swap_yc: the Y and C components are swapped (Y comes before C)
  * @swap_uv: the U and V components are swapped (V comes before U)
  * @hsub: horizontal subsampling factor
@@ -72,6 +73,8 @@ enum vsp1_pipeline_state {
  * @inputs: array of RPFs in the pipeline (indexed by RPF index)
  * @output: WPF at the output of the pipeline
  * @bru: BRU entity, if present
+ * @hgo: HGO entity, if present
+ * @hgt: HGT entity, if present
  * @lif: LIF entity, if present
  * @uds: UDS entity, if present
  * @uds_input: entity at the input of the UDS, if the UDS is present
@@ -100,6 +103,8 @@ struct vsp1_pipeline {
         struct vsp1_rwpf *inputs[VSP1_MAX_RPF];
         struct vsp1_rwpf *output;
         struct vsp1_entity *bru;
+        struct vsp1_entity *hgo;
+        struct vsp1_entity *hgt;
         struct vsp1_entity *lif;
         struct vsp1_entity *uds;
         struct vsp1_entity *uds_input;
diff --git a/drivers/media/platform/vsp1/vsp1_regs.h b/drivers/media/platform/vsp1/vsp1_regs.h
index 47b1dee044fb..cd3e32af6e3b 100644
--- a/drivers/media/platform/vsp1/vsp1_regs.h
+++ b/drivers/media/platform/vsp1/vsp1_regs.h
@@ -328,8 +328,8 @@
 #define VI6_DPR_ROUTE_RT_MASK           (0x3f << 0)
 #define VI6_DPR_ROUTE_RT_SHIFT          0
 
-#define VI6_DPR_HGO_SMPPT               0x2050
-#define VI6_DPR_HGT_SMPPT               0x2054
+#define VI6_DPR_HGO_SMPPT               0x2054
+#define VI6_DPR_HGT_SMPPT               0x2058
 #define VI6_DPR_SMPPT_TGW_MASK          (7 << 8)
 #define VI6_DPR_SMPPT_TGW_SHIFT         8
 #define VI6_DPR_SMPPT_PT_MASK           (0x3f << 0)
@@ -590,33 +590,55 @@
  */
 
 #define VI6_HGO_OFFSET                  0x3000
+#define VI6_HGO_OFFSET_HOFFSET_SHIFT    16
+#define VI6_HGO_OFFSET_VOFFSET_SHIFT    0
 #define VI6_HGO_SIZE                    0x3004
+#define VI6_HGO_SIZE_HSIZE_SHIFT        16
+#define VI6_HGO_SIZE_VSIZE_SHIFT        0
 #define VI6_HGO_MODE                    0x3008
+#define VI6_HGO_MODE_STEP               (1 << 10)
+#define VI6_HGO_MODE_MAXRGB             (1 << 7)
+#define VI6_HGO_MODE_OFSB_R             (1 << 6)
+#define VI6_HGO_MODE_OFSB_G             (1 << 5)
+#define VI6_HGO_MODE_OFSB_B             (1 << 4)
+#define VI6_HGO_MODE_HRATIO_SHIFT       2
+#define VI6_HGO_MODE_VRATIO_SHIFT       0
 #define VI6_HGO_LB_TH                   0x300c
 #define VI6_HGO_LBn_H(n)                (0x3010 + (n) * 8)
 #define VI6_HGO_LBn_V(n)                (0x3014 + (n) * 8)
-#define VI6_HGO_R_HISTO                 0x3030
+#define VI6_HGO_R_HISTO(n)              (0x3030 + (n) * 4)
 #define VI6_HGO_R_MAXMIN                0x3130
 #define VI6_HGO_R_SUM                   0x3134
 #define VI6_HGO_R_LB_DET                0x3138
-#define VI6_HGO_G_HISTO                 0x3140
+#define VI6_HGO_G_HISTO(n)              (0x3140 + (n) * 4)
 #define VI6_HGO_G_MAXMIN                0x3240
 #define VI6_HGO_G_SUM                   0x3244
 #define VI6_HGO_G_LB_DET                0x3248
-#define VI6_HGO_B_HISTO                 0x3250
+#define VI6_HGO_B_HISTO(n)              (0x3250 + (n) * 4)
 #define VI6_HGO_B_MAXMIN                0x3350
 #define VI6_HGO_B_SUM                   0x3354
 #define VI6_HGO_B_LB_DET                0x3358
+#define VI6_HGO_EXT_HIST_ADDR           0x335c
+#define VI6_HGO_EXT_HIST_DATA           0x3360
 #define VI6_HGO_REGRST                  0x33fc
+#define VI6_HGO_REGRST_RCLEA            (1 << 0)
 
 /* -----------------------------------------------------------------------------
  * HGT Control Registers
  */
 
 #define VI6_HGT_OFFSET                  0x3400
+#define VI6_HGT_OFFSET_HOFFSET_SHIFT    16
+#define VI6_HGT_OFFSET_VOFFSET_SHIFT    0
 #define VI6_HGT_SIZE                    0x3404
+#define VI6_HGT_SIZE_HSIZE_SHIFT        16
+#define VI6_HGT_SIZE_VSIZE_SHIFT        0
 #define VI6_HGT_MODE                    0x3408
+#define VI6_HGT_MODE_HRATIO_SHIFT       2
+#define VI6_HGT_MODE_VRATIO_SHIFT       0
 #define VI6_HGT_HUE_AREA(n)             (0x340c + (n) * 4)
+#define VI6_HGT_HUE_AREA_LOWER_SHIFT    16
+#define VI6_HGT_HUE_AREA_UPPER_SHIFT    0
 #define VI6_HGT_LB_TH                   0x3424
 #define VI6_HGT_LBn_H(n)                (0x3438 + (n) * 8)
 #define VI6_HGT_LBn_V(n)                (0x342c + (n) * 8)
@@ -625,6 +647,7 @@
 #define VI6_HGT_SUM                     0x3754
 #define VI6_HGT_LB_DET                  0x3758
 #define VI6_HGT_REGRST                  0x37fc
+#define VI6_HGT_REGRST_RCLEA            (1 << 0)
 
 /* -----------------------------------------------------------------------------
  * LIF Control Registers
diff --git a/drivers/media/platform/vsp1/vsp1_rpf.c b/drivers/media/platform/vsp1/vsp1_rpf.c
index b2e34a800ffa..8feddd59cf8d 100644
--- a/drivers/media/platform/vsp1/vsp1_rpf.c
+++ b/drivers/media/platform/vsp1/vsp1_rpf.c
@@ -72,7 +72,8 @@ static void rpf_configure(struct vsp1_entity *entity,
         }
 
         if (params == VSP1_ENTITY_PARAMS_PARTITION) {
-                unsigned int offsets[2];
+                struct vsp1_device *vsp1 = rpf->entity.vsp1;
+                struct vsp1_rwpf_memory mem = rpf->mem;
                 struct v4l2_rect crop;
 
                 /*
@@ -105,7 +106,7 @@ static void rpf_configure(struct vsp1_entity *entity,
                          * of the pipeline.
                          */
                         output = vsp1_entity_get_pad_format(wpf, wpf->config,
-                                                            RWPF_PAD_SOURCE);
+                                                            RWPF_PAD_SINK);
 
                         crop.width = pipe->partition.width * input_width
                                    / output->width;
@@ -120,22 +121,30 @@ static void rpf_configure(struct vsp1_entity *entity,
                                (crop.width << VI6_RPF_SRC_ESIZE_EHSIZE_SHIFT) |
                                (crop.height << VI6_RPF_SRC_ESIZE_EVSIZE_SHIFT));
 
-                offsets[0] = crop.top * format->plane_fmt[0].bytesperline
-                           + crop.left * fmtinfo->bpp[0] / 8;
-
-                if (format->num_planes > 1)
-                        offsets[1] = crop.top * format->plane_fmt[1].bytesperline
-                                   + crop.left / fmtinfo->hsub
-                                   * fmtinfo->bpp[1] / 8;
-                else
-                        offsets[1] = 0;
-
-                vsp1_rpf_write(rpf, dl, VI6_RPF_SRCM_ADDR_Y,
-                               rpf->mem.addr[0] + offsets[0]);
-                vsp1_rpf_write(rpf, dl, VI6_RPF_SRCM_ADDR_C0,
-                               rpf->mem.addr[1] + offsets[1]);
-                vsp1_rpf_write(rpf, dl, VI6_RPF_SRCM_ADDR_C1,
-                               rpf->mem.addr[2] + offsets[1]);
+                mem.addr[0] += crop.top * format->plane_fmt[0].bytesperline
+                             + crop.left * fmtinfo->bpp[0] / 8;
+
+                if (format->num_planes > 1) {
+                        unsigned int offset;
+
+                        offset = crop.top * format->plane_fmt[1].bytesperline
+                               + crop.left / fmtinfo->hsub
+                               * fmtinfo->bpp[1] / 8;
+                        mem.addr[1] += offset;
+                        mem.addr[2] += offset;
+                }
+
+                /*
+                 * On Gen3 hardware the SPUVS bit has no effect on 3-planar
+                 * formats. Swap the U and V planes manually in that case.
+                 */
+                if (vsp1->info->gen == 3 && format->num_planes == 3 &&
+                    fmtinfo->swap_uv)
+                        swap(mem.addr[1], mem.addr[2]);
+
+                vsp1_rpf_write(rpf, dl, VI6_RPF_SRCM_ADDR_Y, mem.addr[0]);
+                vsp1_rpf_write(rpf, dl, VI6_RPF_SRCM_ADDR_C0, mem.addr[1]);
+                vsp1_rpf_write(rpf, dl, VI6_RPF_SRCM_ADDR_C1, mem.addr[2]);
                 return;
         }
 
@@ -186,7 +195,8 @@ static void rpf_configure(struct vsp1_entity *entity,
                        (left << VI6_RPF_LOC_HCOORD_SHIFT) |
                        (top << VI6_RPF_LOC_VCOORD_SHIFT));
 
-        /* On Gen2 use the alpha channel (extended to 8 bits) when available or
+        /*
+         * On Gen2 use the alpha channel (extended to 8 bits) when available or
          * a fixed alpha value set through the V4L2_CID_ALPHA_COMPONENT control
          * otherwise.
          *
@@ -216,7 +226,8 @@ static void rpf_configure(struct vsp1_entity *entity,
                 u32 mult;
 
                 if (fmtinfo->alpha) {
-                        /* When the input contains an alpha channel enable the
+                        /*
+                         * When the input contains an alpha channel enable the
                          * alpha multiplier. If the input is premultiplied we
                          * need to multiply both the alpha channel and the pixel
                          * components by the global alpha value to keep them
@@ -231,7 +242,8 @@ static void rpf_configure(struct vsp1_entity *entity,
                                 VI6_RPF_MULT_ALPHA_P_MMD_RATIO :
                                 VI6_RPF_MULT_ALPHA_P_MMD_NONE);
                 } else {
-                        /* When the input doesn't contain an alpha channel the
+                        /*
+                         * When the input doesn't contain an alpha channel the
                          * global alpha value is applied in the unpacking unit,
                          * the alpha multiplier isn't needed and must be
                          * disabled.
diff --git a/drivers/media/platform/vsp1/vsp1_rwpf.c b/drivers/media/platform/vsp1/vsp1_rwpf.c
index 04104ef28fb5..cfd8f1904fa6 100644
--- a/drivers/media/platform/vsp1/vsp1_rwpf.c
+++ b/drivers/media/platform/vsp1/vsp1_rwpf.c
@@ -86,7 +86,8 @@ static int vsp1_rwpf_set_format(struct v4l2_subdev *subdev,
         format = vsp1_entity_get_pad_format(&rwpf->entity, config, fmt->pad);
 
         if (fmt->pad == RWPF_PAD_SOURCE) {
-                /* The RWPF performs format conversion but can't scale, only the
+                /*
+                 * The RWPF performs format conversion but can't scale, only the
                  * format code can be changed on the source pad.
                  */
                 format->code = fmt->format.code;
@@ -120,6 +121,11 @@ static int vsp1_rwpf_set_format(struct v4l2_subdev *subdev,
                                             RWPF_PAD_SOURCE);
         *format = fmt->format;
 
+        if (rwpf->flip.rotate) {
+                format->width = fmt->format.height;
+                format->height = fmt->format.width;
+        }
+
 done:
         mutex_unlock(&rwpf->entity.lock);
         return ret;
@@ -205,7 +211,8 @@ static int vsp1_rwpf_set_selection(struct v4l2_subdev *subdev,
         format = vsp1_entity_get_pad_format(&rwpf->entity, config,
                                             RWPF_PAD_SINK);
 
-        /* Restrict the crop rectangle coordinates to multiples of 2 to avoid
+        /*
+         * Restrict the crop rectangle coordinates to multiples of 2 to avoid
          * shifting the color plane.
          */
         if (format->code == MEDIA_BUS_FMT_AYUV8_1X32) {
diff --git a/drivers/media/platform/vsp1/vsp1_rwpf.h b/drivers/media/platform/vsp1/vsp1_rwpf.h
index 1c98aff3da5d..58215a7ab631 100644
--- a/drivers/media/platform/vsp1/vsp1_rwpf.h
+++ b/drivers/media/platform/vsp1/vsp1_rwpf.h
@@ -56,9 +56,14 @@ struct vsp1_rwpf {
 
         struct {
                 spinlock_t lock;
-                struct v4l2_ctrl *ctrls[2];
+                struct {
+                        struct v4l2_ctrl *vflip;
+                        struct v4l2_ctrl *hflip;
+                        struct v4l2_ctrl *rotate;
+                } ctrls;
                 unsigned int pending;
                 unsigned int active;
+                bool rotate;
         } flip;
 
         struct vsp1_rwpf_memory mem;
diff --git a/drivers/media/platform/vsp1/vsp1_sru.c b/drivers/media/platform/vsp1/vsp1_sru.c
index b4e568a3b4ed..30142793dfcd 100644
--- a/drivers/media/platform/vsp1/vsp1_sru.c
+++ b/drivers/media/platform/vsp1/vsp1_sru.c
@@ -191,7 +191,8 @@ static void sru_try_format(struct vsp1_sru *sru,
                                                     SRU_PAD_SINK);
                 fmt->code = format->code;
 
-                /* We can upscale by 2 in both direction, but not independently.
+                /*
+                 * We can upscale by 2 in both direction, but not independently.
                  * Compare the input and output rectangles areas (avoiding
                  * integer overflows on the output): if the requested output
                  * area is larger than 1.5^2 the input area upscale by two,
diff --git a/drivers/media/platform/vsp1/vsp1_uds.c b/drivers/media/platform/vsp1/vsp1_uds.c
index da8f89a31ea4..4226403ad235 100644
--- a/drivers/media/platform/vsp1/vsp1_uds.c
+++ b/drivers/media/platform/vsp1/vsp1_uds.c
@@ -293,7 +293,8 @@ static void uds_configure(struct vsp1_entity *entity,
 
         dev_dbg(uds->entity.vsp1->dev, "hscale %u vscale %u\n", hscale, vscale);
 
-        /* Multi-tap scaling can't be enabled along with alpha scaling when
+        /*
+         * Multi-tap scaling can't be enabled along with alpha scaling when
          * scaling down with a factor lower than or equal to 1/2 in either
          * direction.
          */
diff --git a/drivers/media/platform/vsp1/vsp1_video.c b/drivers/media/platform/vsp1/vsp1_video.c
index 3eaadbf7a876..eab3c3ea85d7 100644
--- a/drivers/media/platform/vsp1/vsp1_video.c
+++ b/drivers/media/platform/vsp1/vsp1_video.c
@@ -31,6 +31,8 @@
 #include "vsp1_bru.h"
 #include "vsp1_dl.h"
 #include "vsp1_entity.h"
+#include "vsp1_hgo.h"
+#include "vsp1_hgt.h"
 #include "vsp1_pipe.h"
 #include "vsp1_rwpf.h"
 #include "vsp1_uds.h"
@@ -103,7 +105,8 @@ static int __vsp1_video_try_format(struct vsp1_video *video,
         unsigned int height = pix->height;
         unsigned int i;
 
-        /* Backward compatibility: replace deprecated RGB formats by their XRGB
+        /*
+         * Backward compatibility: replace deprecated RGB formats by their XRGB
          * equivalent. This selects the format older userspace applications want
          * while still exposing the new format.
          */
@@ -114,7 +117,8 @@ static int __vsp1_video_try_format(struct vsp1_video *video,
                 }
         }
 
-        /* Retrieve format information and select the default format if the
+        /*
+         * Retrieve format information and select the default format if the
          * requested format isn't supported.
          */
         info = vsp1_get_format_info(video->vsp1, pix->pixelformat);
@@ -140,7 +144,8 @@ static int __vsp1_video_try_format(struct vsp1_video *video,
         pix->height = clamp(height, VSP1_VIDEO_MIN_HEIGHT,
                             VSP1_VIDEO_MAX_HEIGHT);
 
-        /* Compute and clamp the stride and image size. While not documented in
+        /*
+         * Compute and clamp the stride and image size. While not documented in
          * the datasheet, strides not aligned to a multiple of 128 bytes result
          * in image corruption.
          */
@@ -184,9 +189,13 @@ static void vsp1_video_pipeline_setup_partitions(struct vsp1_pipeline *pipe)
         struct vsp1_entity *entity;
         unsigned int div_size;
 
+        /*
+         * 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.config,
-                                            RWPF_PAD_SOURCE);
+                                            RWPF_PAD_SINK);
         div_size = format->width;
 
         /* Gen2 hardware doesn't require image partitioning. */
@@ -226,9 +235,13 @@ static struct v4l2_rect vsp1_video_partition(struct vsp1_pipeline *pipe,
         struct v4l2_rect partition;
         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.config,
-                                            RWPF_PAD_SOURCE);
+                                            RWPF_PAD_SINK);
 
         /* A single partition simply processes the output size in full. */
         if (pipe->partitions <= 1) {
@@ -449,7 +462,8 @@ static void vsp1_video_pipeline_frame_end(struct vsp1_pipeline *pipe)
         state = pipe->state;
         pipe->state = VSP1_PIPELINE_STOPPED;
 
-        /* If a stop has been requested, mark the pipeline as stopped and
+        /*
+         * If a stop has been requested, mark the pipeline as stopped and
          * return. Otherwise restart the pipeline if ready.
          */
         if (state == VSP1_PIPELINE_STOPPING)
@@ -474,7 +488,12 @@ static int vsp1_video_pipeline_build_branch(struct vsp1_pipeline *pipe,
         if (ret < 0)
                 return ret;
 
-        pad = media_entity_remote_pad(&input->entity.pads[RWPF_PAD_SOURCE]);
+        /*
+         * The main data path doesn't include the HGO or HGT, use
+         * vsp1_entity_remote_pad() to traverse the graph.
+         */
+
+        pad = vsp1_entity_remote_pad(&input->entity.pads[RWPF_PAD_SOURCE]);
 
         while (1) {
                 if (pad == NULL) {
@@ -491,7 +510,8 @@ static int vsp1_video_pipeline_build_branch(struct vsp1_pipeline *pipe,
                 entity = to_vsp1_entity(
                         media_entity_to_v4l2_subdev(pad->entity));
 
-                /* A BRU is present in the pipeline, store the BRU input pad
+                /*
+                 * A BRU is present in the pipeline, store the BRU input pad
                  * number in the input RPF for use when configuring the RPF.
                  */
                 if (entity->type == VSP1_ENTITY_BRU) {
@@ -526,13 +546,9 @@ static int vsp1_video_pipeline_build_branch(struct vsp1_pipeline *pipe,
                                         : &input->entity;
                 }
 
-                /* Follow the source link. The link setup operations ensure
-                 * that the output fan-out can't be more than one, there is thus
-                 * no need to verify here that only a single source link is
-                 * activated.
-                 */
+                /* Follow the source link, ignoring any HGO or HGT. */
                 pad = &entity->pads[entity->source_pad];
-                pad = media_entity_remote_pad(pad);
+                pad = vsp1_entity_remote_pad(pad);
         }
 
         /* The last entity must be the output WPF. */
@@ -587,6 +603,16 @@ static int vsp1_video_pipeline_build(struct vsp1_pipeline *pipe,
                         pipe->lif = e;
                 } else if (e->type == VSP1_ENTITY_BRU) {
                         pipe->bru = e;
+                } else if (e->type == VSP1_ENTITY_HGO) {
+                        struct vsp1_hgo *hgo = to_hgo(subdev);
+
+                        pipe->hgo = e;
+                        hgo->histo.pipe = pipe;
+                } else if (e->type == VSP1_ENTITY_HGT) {
+                        struct vsp1_hgt *hgt = to_hgt(subdev);
+
+                        pipe->hgt = e;
+                        hgt->histo.pipe = pipe;
                 }
         }
 
@@ -596,7 +622,8 @@ static int vsp1_video_pipeline_build(struct vsp1_pipeline *pipe,
         if (pipe->num_inputs == 0 || !pipe->output)
                 return -EPIPE;
 
-        /* Follow links downstream for each input and make sure the graph
+        /*
+         * Follow links downstream for each input and make sure the graph
          * contains no loop and that all branches end at the output WPF.
          */
         for (i = 0; i < video->vsp1->info->rpf_count; ++i) {
@@ -627,7 +654,8 @@ static struct vsp1_pipeline *vsp1_video_pipeline_get(struct vsp1_video *video)
         struct vsp1_pipeline *pipe;
         int ret;
 
-        /* Get a pipeline object for the video node. If a pipeline has already
+        /*
+         * Get a pipeline object for the video node. If a pipeline has already
          * been allocated just increment its reference count and return it.
          * Otherwise allocate a new pipeline and initialize it, it will be freed
          * when the last reference is released.
@@ -767,7 +795,8 @@ static int vsp1_video_setup_pipeline(struct vsp1_pipeline *pipe)
         if (pipe->uds) {
                 struct vsp1_uds *uds = to_uds(&pipe->uds->subdev);
 
-                /* If a BRU is present in the pipeline before the UDS, the alpha
+                /*
+                 * If a BRU is present in the pipeline before the UDS, the alpha
                  * component doesn't need to be scaled as the BRU output alpha
                  * value is fixed to 255. Otherwise we need to scale the alpha
                  * component only when available at the input RPF.
@@ -783,7 +812,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->dl);
+                vsp1_entity_route_setup(entity, pipe, pipe->dl);
 
                 if (entity->ops->configure)
                         entity->ops->configure(entity, pipe, pipe->dl,
@@ -797,6 +826,7 @@ static int vsp1_video_start_streaming(struct vb2_queue *vq, unsigned int count)
 {
         struct vsp1_video *video = vb2_get_drv_priv(vq);
         struct vsp1_pipeline *pipe = video->rwpf->pipe;
+        bool start_pipeline = false;
         unsigned long flags;
         int ret;
 
@@ -807,11 +837,23 @@ static int vsp1_video_start_streaming(struct vb2_queue *vq, unsigned int count)
                         mutex_unlock(&pipe->lock);
                         return ret;
                 }
+
+                start_pipeline = true;
         }
 
         pipe->stream_count++;
         mutex_unlock(&pipe->lock);
 
+        /*
+         * vsp1_pipeline_ready() is not sufficient to establish that all streams
+         * are prepared and the pipeline is configured, as multiple streams
+         * can race through streamon with buffers already queued; Therefore we
+         * don't even attempt to start the pipeline until the last stream has
+         * called through here.
+         */
+        if (!start_pipeline)
+                return 0;
+
         spin_lock_irqsave(&pipe->irqlock, flags);
         if (vsp1_pipeline_ready(pipe))
                 vsp1_video_pipeline_run(pipe);
@@ -968,7 +1010,8 @@ vsp1_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
         if (video->queue.owner && video->queue.owner != file->private_data)
                 return -EBUSY;
 
-        /* Get a pipeline for the video node and start streaming on it. No link
+        /*
+         * Get a pipeline for the video node and start streaming on it. No link
          * touching an entity in the pipeline can be activated or deactivated
          * once streaming is started.
          */
@@ -988,7 +1031,8 @@ vsp1_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 
         mutex_unlock(&mdev->graph_mutex);
 
-        /* Verify that the configured format matches the output of the connected
+        /*
+         * Verify that the configured format matches the output of the connected
          * subdev.
          */
         ret = vsp1_video_verify_format(video);
@@ -1050,6 +1094,7 @@ static int vsp1_video_open(struct file *file)
         ret = vsp1_device_get(video->vsp1);
         if (ret < 0) {
                 v4l2_fh_del(vfh);
+                v4l2_fh_exit(vfh);
                 kfree(vfh);
         }
 
diff --git a/drivers/media/platform/vsp1/vsp1_wpf.c b/drivers/media/platform/vsp1/vsp1_wpf.c
index 7c48f81cd5c1..32df109b119f 100644
--- a/drivers/media/platform/vsp1/vsp1_wpf.c
+++ b/drivers/media/platform/vsp1/vsp1_wpf.c
@@ -43,32 +43,90 @@ static inline void vsp1_wpf_write(struct vsp1_rwpf *wpf,
 enum wpf_flip_ctrl {
         WPF_CTRL_VFLIP = 0,
         WPF_CTRL_HFLIP = 1,
-        WPF_CTRL_MAX,
 };
 
+static int vsp1_wpf_set_rotation(struct vsp1_rwpf *wpf, unsigned int rotation)
+{
+        struct vsp1_video *video = wpf->video;
+        struct v4l2_mbus_framefmt *sink_format;
+        struct v4l2_mbus_framefmt *source_format;
+        bool rotate;
+        int ret = 0;
+
+        /*
+         * Only consider the 0°/180° from/to 90°/270° modifications, the rest
+         * is taken care of by the flipping configuration.
+         */
+        rotate = rotation == 90 || rotation == 270;
+        if (rotate == wpf->flip.rotate)
+                return 0;
+
+        /* Changing rotation isn't allowed when buffers are allocated. */
+        mutex_lock(&video->lock);
+
+        if (vb2_is_busy(&video->queue)) {
+                ret = -EBUSY;
+                goto done;
+        }
+
+        sink_format = vsp1_entity_get_pad_format(&wpf->entity,
+                                                 wpf->entity.config,
+                                                 RWPF_PAD_SINK);
+        source_format = vsp1_entity_get_pad_format(&wpf->entity,
+                                                   wpf->entity.config,
+                                                   RWPF_PAD_SOURCE);
+
+        mutex_lock(&wpf->entity.lock);
+
+        if (rotate) {
+                source_format->width = sink_format->height;
+                source_format->height = sink_format->width;
+        } else {
+                source_format->width = sink_format->width;
+                source_format->height = sink_format->height;
+        }
+
+        wpf->flip.rotate = rotate;
+
+        mutex_unlock(&wpf->entity.lock);
+
+done:
+        mutex_unlock(&video->lock);
+        return ret;
+}
+
 static int vsp1_wpf_s_ctrl(struct v4l2_ctrl *ctrl)
 {
         struct vsp1_rwpf *wpf =
                 container_of(ctrl->handler, struct vsp1_rwpf, ctrls);
-        unsigned int i;
+        unsigned int rotation;
         u32 flip = 0;
+        int ret;
 
-        switch (ctrl->id) {
-        case V4L2_CID_HFLIP:
-        case V4L2_CID_VFLIP:
-                for (i = 0; i < WPF_CTRL_MAX; ++i) {
-                        if (wpf->flip.ctrls[i])
-                                flip |= wpf->flip.ctrls[i]->val ? BIT(i) : 0;
-                }
+        /* Update the rotation. */
+        rotation = wpf->flip.ctrls.rotate ? wpf->flip.ctrls.rotate->val : 0;
+        ret = vsp1_wpf_set_rotation(wpf, rotation);
+        if (ret < 0)
+                return ret;
 
-                spin_lock_irq(&wpf->flip.lock);
-                wpf->flip.pending = flip;
-                spin_unlock_irq(&wpf->flip.lock);
-                break;
+        /*
+         * Compute the flip value resulting from all three controls, with
+         * rotation by 180° flipping the image in both directions. Store the
+         * result in the pending flip field for the next frame that will be
+         * processed.
+         */
+        if (wpf->flip.ctrls.vflip->val)
+                flip |= BIT(WPF_CTRL_VFLIP);
 
-        default:
-                return -EINVAL;
-        }
+        if (wpf->flip.ctrls.hflip && wpf->flip.ctrls.hflip->val)
+                flip |= BIT(WPF_CTRL_HFLIP);
+
+        if (rotation == 180 || rotation == 270)
+                flip ^= BIT(WPF_CTRL_VFLIP) | BIT(WPF_CTRL_HFLIP);
+
+        spin_lock_irq(&wpf->flip.lock);
+        wpf->flip.pending = flip;
+        spin_unlock_irq(&wpf->flip.lock);
 
         return 0;
 }
@@ -88,12 +146,14 @@ static int wpf_init_controls(struct vsp1_rwpf *wpf)
                 /* Only WPF0 supports flipping. */
                 num_flip_ctrls = 0;
         } else if (vsp1->info->features & VSP1_HAS_WPF_HFLIP) {
-                /* When horizontal flip is supported the WPF implements two
-                 * controls (horizontal flip and vertical flip).
+                /*
+                 * When horizontal flip is supported the WPF implements three
+                 * controls (horizontal flip, vertical flip and rotation).
                  */
-                num_flip_ctrls = 2;
+                num_flip_ctrls = 3;
         } else if (vsp1->info->features & VSP1_HAS_WPF_VFLIP) {
-                /* When only vertical flip is supported the WPF implements a
+                /*
+                 * When only vertical flip is supported the WPF implements a
                  * single control (vertical flip).
                  */
                 num_flip_ctrls = 1;
@@ -105,17 +165,19 @@ static int wpf_init_controls(struct vsp1_rwpf *wpf)
         vsp1_rwpf_init_ctrls(wpf, num_flip_ctrls);
 
         if (num_flip_ctrls >= 1) {
-                wpf->flip.ctrls[WPF_CTRL_VFLIP] =
+                wpf->flip.ctrls.vflip =
                         v4l2_ctrl_new_std(&wpf->ctrls, &vsp1_wpf_ctrl_ops,
                                           V4L2_CID_VFLIP, 0, 1, 1, 0);
         }
 
-        if (num_flip_ctrls == 2) {
-                wpf->flip.ctrls[WPF_CTRL_HFLIP] =
+        if (num_flip_ctrls == 3) {
+                wpf->flip.ctrls.hflip =
                         v4l2_ctrl_new_std(&wpf->ctrls, &vsp1_wpf_ctrl_ops,
                                           V4L2_CID_HFLIP, 0, 1, 1, 0);
-
-                v4l2_ctrl_cluster(2, wpf->flip.ctrls);
+                wpf->flip.ctrls.rotate =
+                        v4l2_ctrl_new_std(&wpf->ctrls, &vsp1_wpf_ctrl_ops,
+                                          V4L2_CID_ROTATE, 0, 270, 90, 0);
+                v4l2_ctrl_cluster(3, &wpf->flip.ctrls.vflip);
         }
 
         if (wpf->ctrls.error) {
@@ -139,7 +201,8 @@ static int wpf_s_stream(struct v4l2_subdev *subdev, int enable)
         if (enable)
                 return 0;
 
-        /* Write to registers directly when stopping the stream as there will be
+        /*
+         * Write to registers directly when stopping the stream as there will be
          * no pipeline run to apply the display list.
          */
         vsp1_write(vsp1, VI6_WPF_IRQ_ENB(wpf->entity.index), 0);
@@ -216,10 +279,11 @@ static void wpf_configure(struct vsp1_entity *entity,
 
         if (params == VSP1_ENTITY_PARAMS_PARTITION) {
                 const struct v4l2_pix_format_mplane *format = &wpf->format;
+                const struct vsp1_format_info *fmtinfo = wpf->fmtinfo;
                 struct vsp1_rwpf_memory mem = wpf->mem;
                 unsigned int flip = wpf->flip.active;
-                unsigned int width = source_format->width;
-                unsigned int height = source_format->height;
+                unsigned int width = sink_format->width;
+                unsigned int height = sink_format->height;
                 unsigned int offset;
 
                 /*
@@ -242,45 +306,86 @@ static void wpf_configure(struct vsp1_entity *entity,
                 /*
                  * Update the memory offsets based on flipping configuration.
                  * The destination addresses point to the locations where the
-                 * VSP starts writing to memory, which can be different corners
-                 * of the image depending on vertical flipping.
+                 * VSP starts writing to memory, which can be any corner of the
+                 * image depending on the combination of flipping and rotation.
                  */
-                if (pipe->partitions > 1) {
-                        const struct vsp1_format_info *fmtinfo = wpf->fmtinfo;
 
-                        /*
-                         * Horizontal flipping is handled through a line buffer
-                         * and doesn't modify the start address, but still needs
-                         * to be handled when image partitioning is in effect to
-                         * order the partitions correctly.
-                         */
-                        if (flip & BIT(WPF_CTRL_HFLIP))
-                                offset = format->width - pipe->partition.left
-                                        - pipe->partition.width;
+                /*
+                 * First take the partition left coordinate into account.
+                 * Compute the offset to order the partitions correctly on the
+                 * output based on whether flipping is enabled. Consider
+                 * horizontal flipping when rotation is disabled but vertical
+                 * flipping when rotation is enabled, as rotating the image
+                 * switches the horizontal and vertical directions. The offset
+                 * is applied horizontally or vertically accordingly.
+                 */
+                if (flip & BIT(WPF_CTRL_HFLIP) && !wpf->flip.rotate)
+                        offset = format->width - pipe->partition.left
+                                - pipe->partition.width;
+                else if (flip & BIT(WPF_CTRL_VFLIP) && wpf->flip.rotate)
+                        offset = format->height - pipe->partition.left
+                                - pipe->partition.width;
+                else
+                        offset = pipe->partition.left;
+
+                for (i = 0; i < format->num_planes; ++i) {
+                        unsigned int hsub = i > 0 ? fmtinfo->hsub : 1;
+                        unsigned int vsub = i > 0 ? fmtinfo->vsub : 1;
+
+                        if (wpf->flip.rotate)
+                                mem.addr[i] += offset / vsub
+                                             * format->plane_fmt[i].bytesperline;
                         else
-                                offset = pipe->partition.left;
-
-                        mem.addr[0] += offset * fmtinfo->bpp[0] / 8;
-                        if (format->num_planes > 1) {
-                                mem.addr[1] += offset / fmtinfo->hsub
-                                             * fmtinfo->bpp[1] / 8;
-                                mem.addr[2] += offset / fmtinfo->hsub
-                                             * fmtinfo->bpp[2] / 8;
-                        }
+                                mem.addr[i] += offset / hsub
+                                             * fmtinfo->bpp[i] / 8;
                 }
 
                 if (flip & BIT(WPF_CTRL_VFLIP)) {
-                        mem.addr[0] += (format->height - 1)
+                        /*
+                         * When rotating the output (after rotation) image
+                         * height is equal to the partition width (before
+                         * rotation). Otherwise it is equal to the output
+                         * image height.
+                         */
+                        if (wpf->flip.rotate)
+                                height = pipe->partition.width;
+                        else
+                                height = format->height;
+
+                        mem.addr[0] += (height - 1)
                                      * format->plane_fmt[0].bytesperline;
 
                         if (format->num_planes > 1) {
-                                offset = (format->height / wpf->fmtinfo->vsub - 1)
+                                offset = (height / fmtinfo->vsub - 1)
                                        * format->plane_fmt[1].bytesperline;
                                 mem.addr[1] += offset;
                                 mem.addr[2] += offset;
                         }
                 }
 
+                if (wpf->flip.rotate && !(flip & BIT(WPF_CTRL_HFLIP))) {
+                        unsigned int hoffset = max(0, (int)format->width - 16);
+
+                        /*
+                         * Compute the output coordinate. The partition
+                         * horizontal (left) offset becomes a vertical offset.
+                         */
+                        for (i = 0; i < format->num_planes; ++i) {
+                                unsigned int hsub = i > 0 ? fmtinfo->hsub : 1;
+
+                                mem.addr[i] += hoffset / hsub
+                                             * fmtinfo->bpp[i] / 8;
+                        }
+                }
+
+                /*
+                 * On Gen3 hardware the SPUVS bit has no effect on 3-planar
+                 * formats. Swap the U and V planes manually in that case.
+                 */
+                if (vsp1->info->gen == 3 && format->num_planes == 3 &&
+                    fmtinfo->swap_uv)
+                        swap(mem.addr[1], mem.addr[2]);
+
                 vsp1_wpf_write(wpf, dl, VI6_WPF_DSTM_ADDR_Y, mem.addr[0]);
                 vsp1_wpf_write(wpf, dl, VI6_WPF_DSTM_ADDR_C0, mem.addr[1]);
                 vsp1_wpf_write(wpf, dl, VI6_WPF_DSTM_ADDR_C1, mem.addr[2]);
@@ -294,6 +399,9 @@ static void wpf_configure(struct vsp1_entity *entity,
 
                 outfmt = fmtinfo->hwfmt << VI6_WPF_OUTFMT_WRFMT_SHIFT;
 
+                if (wpf->flip.rotate)
+                        outfmt |= VI6_WPF_OUTFMT_ROT;
+
                 if (fmtinfo->alpha)
                         outfmt |= VI6_WPF_OUTFMT_PXA;
                 if (fmtinfo->swap_yc)
@@ -327,7 +435,8 @@ static void wpf_configure(struct vsp1_entity *entity,
 
         vsp1_dl_list_write(dl, VI6_WPF_WRBCK_CTRL, 0);
 
-        /* Sources. If the pipeline has a single input and BRU is not used,
+        /*
+         * Sources. If the pipeline has a single input and BRU 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.
@@ -354,9 +463,18 @@ static void wpf_configure(struct vsp1_entity *entity,
                            VI6_WFP_IRQ_ENB_DFEE);
 }
 
+static unsigned int wpf_max_width(struct vsp1_entity *entity,
+                                  struct vsp1_pipeline *pipe)
+{
+        struct vsp1_rwpf *wpf = to_rwpf(&entity->subdev);
+
+        return wpf->flip.rotate ? 256 : wpf->max_width;
+}
+
 static const struct vsp1_entity_operations wpf_entity_ops = {
         .destroy = vsp1_wpf_destroy,
         .configure = wpf_configure,
+        .max_width = wpf_max_width,
 };
 
 /* -----------------------------------------------------------------------------
diff --git a/drivers/media/radio/si4713/si4713.c b/drivers/media/radio/si4713/si4713.c
index 60f026a58076..f4a53f1e856e 100644
--- a/drivers/media/radio/si4713/si4713.c
+++ b/drivers/media/radio/si4713/si4713.c
@@ -1656,9 +1656,18 @@ static const struct i2c_device_id si4713_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, si4713_id);
 
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id si4713_of_match[] = {
+        { .compatible = "silabs,si4713" },
+        { },
+};
+MODULE_DEVICE_TABLE(of, si4713_of_match);
+#endif
+
 static struct i2c_driver si4713_i2c_driver = {
         .driver         = {
                 .name   = "si4713",
+                .of_match_table = of_match_ptr(si4713_of_match),
         },
         .probe          = si4713_probe,
         .remove         = si4713_remove,
diff --git a/drivers/media/radio/wl128x/fmdrv_common.c b/drivers/media/radio/wl128x/fmdrv_common.c
index 74a1b3ecb30a..588e2d61c3b4 100644
--- a/drivers/media/radio/wl128x/fmdrv_common.c
+++ b/drivers/media/radio/wl128x/fmdrv_common.c
@@ -1550,9 +1550,8 @@ int fmc_prepare(struct fmdev *fmdev)
         atomic_set(&fmdev->tx_cnt, 1);
         fmdev->resp_comp = NULL;
 
-        init_timer(&fmdev->irq_info.timer);
-        fmdev->irq_info.timer.function = &int_timeout_handler;
-        fmdev->irq_info.timer.data = (unsigned long)fmdev;
+        setup_timer(&fmdev->irq_info.timer, &int_timeout_handler,
+                    (unsigned long)fmdev);
         /*TODO: add FM_STIC_EVENT later */
         fmdev->irq_info.mask = FM_MAL_EVENT;
 
diff --git a/drivers/media/rc/Kconfig b/drivers/media/rc/Kconfig
index d1d3fd00ed89..e422f3d56f76 100644
--- a/drivers/media/rc/Kconfig
+++ b/drivers/media/rc/Kconfig
@@ -426,4 +426,13 @@ config IR_SERIAL_TRANSMITTER
         ---help---
            Serial Port Transmitter support
 
+config IR_SIR
+        tristate "Built-in SIR IrDA port"
+        depends on RC_CORE
+        ---help---
+           Say Y if you want to use a IrDA SIR port Transceivers.
+
+           To compile this driver as a module, choose M here: the module will
+           be called sir-ir.
+
 endif #RC_DEVICES
diff --git a/drivers/media/rc/Makefile b/drivers/media/rc/Makefile
index 679aa0af85cd..245e2c2d0b22 100644
--- a/drivers/media/rc/Makefile
+++ b/drivers/media/rc/Makefile
@@ -39,4 +39,5 @@ obj-$(CONFIG_RC_ST) += st_rc.o
 obj-$(CONFIG_IR_SUNXI) += sunxi-cir.o
 obj-$(CONFIG_IR_IMG) += img-ir/
 obj-$(CONFIG_IR_SERIAL) += serial_ir.o
+obj-$(CONFIG_IR_SIR) += sir_ir.o
 obj-$(CONFIG_IR_MTK) += mtk-cir.o
diff --git a/drivers/media/rc/gpio-ir-recv.c b/drivers/media/rc/gpio-ir-recv.c
index 4a4895e4d599..b4f773b9dc1d 100644
--- a/drivers/media/rc/gpio-ir-recv.c
+++ b/drivers/media/rc/gpio-ir-recv.c
@@ -158,7 +158,7 @@ static int gpio_ir_recv_probe(struct platform_device *pdev)
         rcdev->input_id.version = 0x0100;
         rcdev->dev.parent = &pdev->dev;
         rcdev->driver_name = GPIO_IR_DRIVER_NAME;
-        rcdev->min_timeout = 0;
+        rcdev->min_timeout = 1;
         rcdev->timeout = IR_DEFAULT_TIMEOUT;
         rcdev->max_timeout = 10 * IR_DEFAULT_TIMEOUT;
         if (pdata->allowed_protos)
diff --git a/drivers/media/rc/igorplugusb.c b/drivers/media/rc/igorplugusb.c
index 0f0ed4ea4d06..cb6d4f1247da 100644
--- a/drivers/media/rc/igorplugusb.c
+++ b/drivers/media/rc/igorplugusb.c
@@ -205,7 +205,7 @@ static int igorplugusb_probe(struct usb_interface *intf,
         rc->allowed_protocols = RC_BIT_ALL_IR_DECODER & ~(RC_BIT_NEC |
                         RC_BIT_NECX | RC_BIT_NEC32 | RC_BIT_RC6_6A_20 |
                         RC_BIT_RC6_6A_24 | RC_BIT_RC6_6A_32 | RC_BIT_RC6_MCE |
-                        RC_BIT_SONY20 | RC_BIT_MCE_KBD | RC_BIT_SANYO);
+                        RC_BIT_SONY20 | RC_BIT_SANYO);
 
         rc->priv = ir;
         rc->driver_name = DRIVER_NAME;
diff --git a/drivers/media/rc/imon.c b/drivers/media/rc/imon.c
index 89823d24a384..3489010601b5 100644
--- a/drivers/media/rc/imon.c
+++ b/drivers/media/rc/imon.c
@@ -2412,9 +2412,8 @@ static struct imon_context *imon_init_intf1(struct usb_interface *intf,
         mutex_lock(&ictx->lock);
 
         if (ictx->display_type == IMON_DISPLAY_TYPE_VGA) {
-                init_timer(&ictx->ttimer);
-                ictx->ttimer.data = (unsigned long)ictx;
-                ictx->ttimer.function = imon_touch_display_timeout;
+                setup_timer(&ictx->ttimer, imon_touch_display_timeout,
+                            (unsigned long)ictx);
         }
 
         ictx->usbdev_intf1 = usb_get_dev(interface_to_usbdev(intf));
diff --git a/drivers/media/rc/ir-lirc-codec.c b/drivers/media/rc/ir-lirc-codec.c
index 8517d5153fcf..de85f1d7ce43 100644
--- a/drivers/media/rc/ir-lirc-codec.c
+++ b/drivers/media/rc/ir-lirc-codec.c
@@ -139,7 +139,7 @@ static ssize_t ir_lirc_transmit_ir(struct file *file, const char __user *buf,
         }
 
         if (!dev->tx_ir) {
-                ret = -ENOSYS;
+                ret = -EINVAL;
                 goto out;
         }
 
@@ -221,19 +221,19 @@ static long ir_lirc_ioctl(struct file *filep, unsigned int cmd,
         /* TX settings */
         case LIRC_SET_TRANSMITTER_MASK:
                 if (!dev->s_tx_mask)
-                        return -ENOSYS;
+                        return -ENOTTY;
 
                 return dev->s_tx_mask(dev, val);
 
         case LIRC_SET_SEND_CARRIER:
                 if (!dev->s_tx_carrier)
-                        return -ENOSYS;
+                        return -ENOTTY;
 
                 return dev->s_tx_carrier(dev, val);
 
         case LIRC_SET_SEND_DUTY_CYCLE:
                 if (!dev->s_tx_duty_cycle)
-                        return -ENOSYS;
+                        return -ENOTTY;
 
                 if (val <= 0 || val >= 100)
                         return -EINVAL;
@@ -243,7 +243,7 @@ static long ir_lirc_ioctl(struct file *filep, unsigned int cmd,
         /* RX settings */
         case LIRC_SET_REC_CARRIER:
                 if (!dev->s_rx_carrier_range)
-                        return -ENOSYS;
+                        return -ENOTTY;
 
                 if (val <= 0)
                         return -EINVAL;
@@ -253,6 +253,9 @@ static long ir_lirc_ioctl(struct file *filep, unsigned int cmd,
                                                val);
 
         case LIRC_SET_REC_CARRIER_RANGE:
+                if (!dev->s_rx_carrier_range)
+                        return -ENOTTY;
+
                 if (val <= 0)
                         return -EINVAL;
 
@@ -260,37 +263,40 @@ static long ir_lirc_ioctl(struct file *filep, unsigned int cmd,
                 return 0;
 
         case LIRC_GET_REC_RESOLUTION:
+                if (!dev->rx_resolution)
+                        return -ENOTTY;
+
                 val = dev->rx_resolution;
                 break;
 
         case LIRC_SET_WIDEBAND_RECEIVER:
                 if (!dev->s_learning_mode)
-                        return -ENOSYS;
+                        return -ENOTTY;
 
                 return dev->s_learning_mode(dev, !!val);
 
         case LIRC_SET_MEASURE_CARRIER_MODE:
                 if (!dev->s_carrier_report)
-                        return -ENOSYS;
+                        return -ENOTTY;
 
                 return dev->s_carrier_report(dev, !!val);
 
         /* Generic timeout support */
         case LIRC_GET_MIN_TIMEOUT:
                 if (!dev->max_timeout)
-                        return -ENOSYS;
+                        return -ENOTTY;
                 val = DIV_ROUND_UP(dev->min_timeout, 1000);
                 break;
 
         case LIRC_GET_MAX_TIMEOUT:
                 if (!dev->max_timeout)
-                        return -ENOSYS;
+                        return -ENOTTY;
                 val = dev->max_timeout / 1000;
                 break;
 
         case LIRC_SET_REC_TIMEOUT:
                 if (!dev->max_timeout)
-                        return -ENOSYS;
+                        return -ENOTTY;
 
                 tmp = val * 1000;
 
@@ -305,6 +311,9 @@ static long ir_lirc_ioctl(struct file *filep, unsigned int cmd,
                 break;
 
         case LIRC_SET_REC_TIMEOUT_REPORTS:
+                if (!dev->timeout)
+                        return -ENOTTY;
+
                 lirc->send_timeout_reports = !!val;
                 break;
 
@@ -361,8 +370,11 @@ static int ir_lirc_register(struct rc_dev *dev)
         if (rc)
                 goto rbuf_init_failed;
 
-        if (dev->driver_type != RC_DRIVER_IR_RAW_TX)
+        if (dev->driver_type != RC_DRIVER_IR_RAW_TX) {
                 features |= LIRC_CAN_REC_MODE2;
+                if (dev->rx_resolution)
+                        features |= LIRC_CAN_GET_REC_RESOLUTION;
+        }
         if (dev->tx_ir) {
                 features |= LIRC_CAN_SEND_PULSE;
                 if (dev->s_tx_mask)
diff --git a/drivers/media/rc/ir-mce_kbd-decoder.c b/drivers/media/rc/ir-mce_kbd-decoder.c
index 5226d510e847..6a4d58b88d91 100644
--- a/drivers/media/rc/ir-mce_kbd-decoder.c
+++ b/drivers/media/rc/ir-mce_kbd-decoder.c
@@ -23,7 +23,7 @@
  * - MCIR-2 29-bit IR signals used for mouse movement and buttons
  * - MCIR-2 32-bit IR signals used for standard keyboard keys
  *
- * The media keys on the keyboard send RC-6 signals that are inditinguishable
+ * The media keys on the keyboard send RC-6 signals that are indistinguishable
  * from the keys of the same name on the stock MCE remote, and will be handled
  * by the standard RC-6 decoder, and be made available to the system via the
  * input device for the remote, rather than the keyboard/mouse one.
@@ -339,6 +339,7 @@ again:
                 }
 
                 data->state = STATE_INACTIVE;
+                input_event(data->idev, EV_MSC, MSC_SCAN, scancode);
                 input_sync(data->idev);
                 return 0;
         }
@@ -418,9 +419,53 @@ static int ir_mce_kbd_unregister(struct rc_dev *dev)
         return 0;
 }
 
+static const struct ir_raw_timings_manchester ir_mce_kbd_timings = {
+        .leader         = MCIR2_PREFIX_PULSE,
+        .invert         = 1,
+        .clock          = MCIR2_UNIT,
+        .trailer_space  = MCIR2_UNIT * 10,
+};
+
+/**
+ * ir_mce_kbd_encode() - Encode a scancode as a stream of raw events
+ *
+ * @protocol:   protocol to encode
+ * @scancode:   scancode to encode
+ * @events:     array of raw ir events to write into
+ * @max:        maximum size of @events
+ *
+ * Returns:     The number of events written.
+ *              -ENOBUFS if there isn't enough space in the array to fit the
+ *              encoding. In this case all @max events will have been written.
+ */
+static int ir_mce_kbd_encode(enum rc_type protocol, u32 scancode,
+                             struct ir_raw_event *events, unsigned int max)
+{
+        struct ir_raw_event *e = events;
+        int len, ret;
+        u64 raw;
+
+        if (protocol == RC_TYPE_MCIR2_KBD) {
+                raw = scancode |
+                      ((u64)MCIR2_KEYBOARD_HEADER << MCIR2_KEYBOARD_NBITS);
+                len = MCIR2_KEYBOARD_NBITS + MCIR2_HEADER_NBITS + 1;
+        } else {
+                raw = scancode |
+                      ((u64)MCIR2_MOUSE_HEADER << MCIR2_MOUSE_NBITS);
+                len = MCIR2_MOUSE_NBITS + MCIR2_HEADER_NBITS + 1;
+        }
+
+        ret = ir_raw_gen_manchester(&e, max, &ir_mce_kbd_timings, len, raw);
+        if (ret < 0)
+                return ret;
+
+        return e - events;
+}
+
 static struct ir_raw_handler mce_kbd_handler = {
-        .protocols      = RC_BIT_MCE_KBD,
+        .protocols      = RC_BIT_MCIR2_KBD | RC_BIT_MCIR2_MSE,
         .decode         = ir_mce_kbd_decode,
+        .encode         = ir_mce_kbd_encode,
         .raw_register   = ir_mce_kbd_register,
         .raw_unregister = ir_mce_kbd_unregister,
 };
diff --git a/drivers/media/rc/keymaps/Makefile b/drivers/media/rc/keymaps/Makefile
index ffe9e612f8d6..2945f99907b5 100644
--- a/drivers/media/rc/keymaps/Makefile
+++ b/drivers/media/rc/keymaps/Makefile
@@ -57,7 +57,6 @@ obj-$(CONFIG_RC_MAP) += rc-adstech-dvb-t-pci.o \
                         rc-kworld-pc150u.o \
                         rc-kworld-plus-tv-analog.o \
                         rc-leadtek-y04g0051.o \
-                        rc-lirc.o \
                         rc-lme2510.o \
                         rc-manli.o \
                         rc-medion-x10.o \
diff --git a/drivers/media/rc/keymaps/rc-dvico-mce.c b/drivers/media/rc/keymaps/rc-dvico-mce.c
index e5f098c50235..d1e861f4d095 100644
--- a/drivers/media/rc/keymaps/rc-dvico-mce.c
+++ b/drivers/media/rc/keymaps/rc-dvico-mce.c
@@ -12,58 +12,58 @@
 #include <linux/module.h>
 
 static struct rc_map_table rc_map_dvico_mce_table[] = {
-        { 0xfe02, KEY_TV },
-        { 0xfe0e, KEY_MP3 },
-        { 0xfe1a, KEY_DVD },
-        { 0xfe1e, KEY_FAVORITES },
-        { 0xfe16, KEY_SETUP },
-        { 0xfe46, KEY_POWER2 },
-        { 0xfe0a, KEY_EPG },
-        { 0xfe49, KEY_BACK },
-        { 0xfe4d, KEY_MENU },
-        { 0xfe51, KEY_UP },
-        { 0xfe5b, KEY_LEFT },
-        { 0xfe5f, KEY_RIGHT },
-        { 0xfe53, KEY_DOWN },
-        { 0xfe5e, KEY_OK },
-        { 0xfe59, KEY_INFO },
-        { 0xfe55, KEY_TAB },
-        { 0xfe0f, KEY_PREVIOUSSONG },/* Replay */
-        { 0xfe12, KEY_NEXTSONG },       /* Skip */
-        { 0xfe42, KEY_ENTER      },     /* Windows/Start */
-        { 0xfe15, KEY_VOLUMEUP },
-        { 0xfe05, KEY_VOLUMEDOWN },
-        { 0xfe11, KEY_CHANNELUP },
-        { 0xfe09, KEY_CHANNELDOWN },
-        { 0xfe52, KEY_CAMERA },
-        { 0xfe5a, KEY_TUNER },  /* Live */
-        { 0xfe19, KEY_OPEN },
-        { 0xfe0b, KEY_1 },
-        { 0xfe17, KEY_2 },
-        { 0xfe1b, KEY_3 },
-        { 0xfe07, KEY_4 },
-        { 0xfe50, KEY_5 },
-        { 0xfe54, KEY_6 },
-        { 0xfe48, KEY_7 },
-        { 0xfe4c, KEY_8 },
-        { 0xfe58, KEY_9 },
-        { 0xfe13, KEY_ANGLE },  /* Aspect */
-        { 0xfe03, KEY_0 },
-        { 0xfe1f, KEY_ZOOM },
-        { 0xfe43, KEY_REWIND },
-        { 0xfe47, KEY_PLAYPAUSE },
-        { 0xfe4f, KEY_FASTFORWARD },
-        { 0xfe57, KEY_MUTE },
-        { 0xfe0d, KEY_STOP },
-        { 0xfe01, KEY_RECORD },
-        { 0xfe4e, KEY_POWER },
+        { 0x0102, KEY_TV },
+        { 0x010e, KEY_MP3 },
+        { 0x011a, KEY_DVD },
+        { 0x011e, KEY_FAVORITES },
+        { 0x0116, KEY_SETUP },
+        { 0x0146, KEY_POWER2 },
+        { 0x010a, KEY_EPG },
+        { 0x0149, KEY_BACK },
+        { 0x014d, KEY_MENU },
+        { 0x0151, KEY_UP },
+        { 0x015b, KEY_LEFT },
+        { 0x015f, KEY_RIGHT },
+        { 0x0153, KEY_DOWN },
+        { 0x015e, KEY_OK },
+        { 0x0159, KEY_INFO },
+        { 0x0155, KEY_TAB },
+        { 0x010f, KEY_PREVIOUSSONG },/* Replay */
+        { 0x0112, KEY_NEXTSONG },       /* Skip */
+        { 0x0142, KEY_ENTER      },     /* Windows/Start */
+        { 0x0115, KEY_VOLUMEUP },
+        { 0x0105, KEY_VOLUMEDOWN },
+        { 0x0111, KEY_CHANNELUP },
+        { 0x0109, KEY_CHANNELDOWN },
+        { 0x0152, KEY_CAMERA },
+        { 0x015a, KEY_TUNER },  /* Live */
+        { 0x0119, KEY_OPEN },
+        { 0x010b, KEY_1 },
+        { 0x0117, KEY_2 },
+        { 0x011b, KEY_3 },
+        { 0x0107, KEY_4 },
+        { 0x0150, KEY_5 },
+        { 0x0154, KEY_6 },
+        { 0x0148, KEY_7 },
+        { 0x014c, KEY_8 },
+        { 0x0158, KEY_9 },
+        { 0x0113, KEY_ANGLE },  /* Aspect */
+        { 0x0103, KEY_0 },
+        { 0x011f, KEY_ZOOM },
+        { 0x0143, KEY_REWIND },
+        { 0x0147, KEY_PLAYPAUSE },
+        { 0x014f, KEY_FASTFORWARD },
+        { 0x0157, KEY_MUTE },
+        { 0x010d, KEY_STOP },
+        { 0x0101, KEY_RECORD },
+        { 0x014e, KEY_POWER },
 };
 
 static struct rc_map_list dvico_mce_map = {
         .map = {
                 .scan    = rc_map_dvico_mce_table,
                 .size    = ARRAY_SIZE(rc_map_dvico_mce_table),
-                .rc_type = RC_TYPE_UNKNOWN,     /* Legacy IR type */
+                .rc_type = RC_TYPE_NEC,
                 .name    = RC_MAP_DVICO_MCE,
         }
 };
diff --git a/drivers/media/rc/keymaps/rc-dvico-portable.c b/drivers/media/rc/keymaps/rc-dvico-portable.c
index 94ceeee94b3f..ac4cb515cbf1 100644
--- a/drivers/media/rc/keymaps/rc-dvico-portable.c
+++ b/drivers/media/rc/keymaps/rc-dvico-portable.c
@@ -12,49 +12,49 @@
 #include <linux/module.h>
 
 static struct rc_map_table rc_map_dvico_portable_table[] = {
-        { 0xfc02, KEY_SETUP },       /* Profile */
-        { 0xfc43, KEY_POWER2 },
-        { 0xfc06, KEY_EPG },
-        { 0xfc5a, KEY_BACK },
-        { 0xfc05, KEY_MENU },
-        { 0xfc47, KEY_INFO },
-        { 0xfc01, KEY_TAB },
-        { 0xfc42, KEY_PREVIOUSSONG },/* Replay */
-        { 0xfc49, KEY_VOLUMEUP },
-        { 0xfc09, KEY_VOLUMEDOWN },
-        { 0xfc54, KEY_CHANNELUP },
-        { 0xfc0b, KEY_CHANNELDOWN },
-        { 0xfc16, KEY_CAMERA },
-        { 0xfc40, KEY_TUNER },  /* ATV/DTV */
-        { 0xfc45, KEY_OPEN },
-        { 0xfc19, KEY_1 },
-        { 0xfc18, KEY_2 },
-        { 0xfc1b, KEY_3 },
-        { 0xfc1a, KEY_4 },
-        { 0xfc58, KEY_5 },
-        { 0xfc59, KEY_6 },
-        { 0xfc15, KEY_7 },
-        { 0xfc14, KEY_8 },
-        { 0xfc17, KEY_9 },
-        { 0xfc44, KEY_ANGLE },  /* Aspect */
-        { 0xfc55, KEY_0 },
-        { 0xfc07, KEY_ZOOM },
-        { 0xfc0a, KEY_REWIND },
-        { 0xfc08, KEY_PLAYPAUSE },
-        { 0xfc4b, KEY_FASTFORWARD },
-        { 0xfc5b, KEY_MUTE },
-        { 0xfc04, KEY_STOP },
-        { 0xfc56, KEY_RECORD },
-        { 0xfc57, KEY_POWER },
-        { 0xfc41, KEY_UNKNOWN },    /* INPUT */
-        { 0xfc00, KEY_UNKNOWN },    /* HD */
+        { 0x0302, KEY_SETUP },       /* Profile */
+        { 0x0343, KEY_POWER2 },
+        { 0x0306, KEY_EPG },
+        { 0x035a, KEY_BACK },
+        { 0x0305, KEY_MENU },
+        { 0x0347, KEY_INFO },
+        { 0x0301, KEY_TAB },
+        { 0x0342, KEY_PREVIOUSSONG },/* Replay */
+        { 0x0349, KEY_VOLUMEUP },
+        { 0x0309, KEY_VOLUMEDOWN },
+        { 0x0354, KEY_CHANNELUP },
+        { 0x030b, KEY_CHANNELDOWN },
+        { 0x0316, KEY_CAMERA },
+        { 0x0340, KEY_TUNER },  /* ATV/DTV */
+        { 0x0345, KEY_OPEN },
+        { 0x0319, KEY_1 },
+        { 0x0318, KEY_2 },
+        { 0x031b, KEY_3 },
+        { 0x031a, KEY_4 },
+        { 0x0358, KEY_5 },
+        { 0x0359, KEY_6 },
+        { 0x0315, KEY_7 },
+        { 0x0314, KEY_8 },
+        { 0x0317, KEY_9 },
+        { 0x0344, KEY_ANGLE },  /* Aspect */
+        { 0x0355, KEY_0 },
+        { 0x0307, KEY_ZOOM },
+        { 0x030a, KEY_REWIND },
+        { 0x0308, KEY_PLAYPAUSE },
+        { 0x034b, KEY_FASTFORWARD },
+        { 0x035b, KEY_MUTE },
+        { 0x0304, KEY_STOP },
+        { 0x0356, KEY_RECORD },
+        { 0x0357, KEY_POWER },
+        { 0x0341, KEY_UNKNOWN },    /* INPUT */
+        { 0x0300, KEY_UNKNOWN },    /* HD */
 };
 
 static struct rc_map_list dvico_portable_map = {
         .map = {
                 .scan    = rc_map_dvico_portable_table,
                 .size    = ARRAY_SIZE(rc_map_dvico_portable_table),
-                .rc_type = RC_TYPE_UNKNOWN,     /* Legacy IR type */
+                .rc_type = RC_TYPE_NEC,
                 .name    = RC_MAP_DVICO_PORTABLE,
         }
 };
diff --git a/drivers/media/rc/keymaps/rc-lirc.c b/drivers/media/rc/keymaps/rc-lirc.c
deleted file mode 100644
index e172f5db5803..000000000000
--- a/drivers/media/rc/keymaps/rc-lirc.c
+++ /dev/null
@@ -1,42 +0,0 @@
-/* rc-lirc.c - Empty dummy keytable, for use when its preferred to pass
- * all raw IR data to the lirc userspace decoder.
- *
- * Copyright (c) 2010 by Jarod Wilson <jarod@redhat.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- */
-
-#include <media/rc-core.h>
-#include <linux/module.h>
-
-static struct rc_map_table lirc[] = {
-        { },
-};
-
-static struct rc_map_list lirc_map = {
-        .map = {
-                .scan    = lirc,
-                .size    = ARRAY_SIZE(lirc),
-                .rc_type = RC_TYPE_OTHER,
-                .name    = RC_MAP_LIRC,
-        }
-};
-
-static int __init init_rc_map_lirc(void)
-{
-        return rc_map_register(&lirc_map);
-}
-
-static void __exit exit_rc_map_lirc(void)
-{
-        rc_map_unregister(&lirc_map);
-}
-
-module_init(init_rc_map_lirc)
-module_exit(exit_rc_map_lirc)
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Jarod Wilson <jarod@redhat.com>");
diff --git a/drivers/media/rc/lirc_dev.c b/drivers/media/rc/lirc_dev.c
index 1688893a65bb..8d60c9f00df9 100644
--- a/drivers/media/rc/lirc_dev.c
+++ b/drivers/media/rc/lirc_dev.c
@@ -54,7 +54,8 @@ struct irctl {
         struct lirc_buffer *buf;
         unsigned int chunk_size;
 
-        struct cdev *cdev;
+        struct device dev;
+        struct cdev cdev;
 
         struct task_struct *task;
         long jiffies_to_wait;
@@ -76,15 +77,21 @@ static void lirc_irctl_init(struct irctl *ir)
         ir->d.minor = NOPLUG;
 }
 
-static void lirc_irctl_cleanup(struct irctl *ir)
+static void lirc_release(struct device *ld)
 {
-        device_destroy(lirc_class, MKDEV(MAJOR(lirc_base_dev), ir->d.minor));
+        struct irctl *ir = container_of(ld, struct irctl, dev);
+
+        put_device(ir->dev.parent);
 
         if (ir->buf != ir->d.rbuf) {
                 lirc_buffer_free(ir->buf);
                 kfree(ir->buf);
         }
-        ir->buf = NULL;
+
+        mutex_lock(&lirc_dev_lock);
+        irctls[ir->d.minor] = NULL;
+        mutex_unlock(&lirc_dev_lock);
+        kfree(ir);
 }
 
 /*  helper function
@@ -157,32 +164,21 @@ static int lirc_cdev_add(struct irctl *ir)
         struct cdev *cdev;
         int retval;
 
-        cdev = cdev_alloc();
-        if (!cdev)
-                return -ENOMEM;
+        cdev = &ir->cdev;
 
         if (d->fops) {
-                cdev->ops = d->fops;
+                cdev_init(cdev, d->fops);
                 cdev->owner = d->owner;
         } else {
-                cdev->ops = &lirc_dev_fops;
+                cdev_init(cdev, &lirc_dev_fops);
                 cdev->owner = THIS_MODULE;
         }
         retval = kobject_set_name(&cdev->kobj, "lirc%d", d->minor);
         if (retval)
-                goto err_out;
-
-        retval = cdev_add(cdev, MKDEV(MAJOR(lirc_base_dev), d->minor), 1);
-        if (retval)
-                goto err_out;
-
-        ir->cdev = cdev;
-
-        return 0;
+                return retval;
 
-err_out:
-        cdev_del(cdev);
-        return retval;
+        cdev->kobj.parent = &ir->dev.kobj;
+        return cdev_add(cdev, ir->dev.devt, 1);
 }
 
 static int lirc_allocate_buffer(struct irctl *ir)
@@ -304,9 +300,12 @@ static int lirc_allocate_driver(struct lirc_driver *d)
 
         ir->d = *d;
 
-        device_create(lirc_class, ir->d.dev,
-                      MKDEV(MAJOR(lirc_base_dev), ir->d.minor), NULL,
-                      "lirc%u", ir->d.minor);
+        ir->dev.devt = MKDEV(MAJOR(lirc_base_dev), ir->d.minor);
+        ir->dev.class = lirc_class;
+        ir->dev.parent = d->dev;
+        ir->dev.release = lirc_release;
+        dev_set_name(&ir->dev, "lirc%d", ir->d.minor);
+        device_initialize(&ir->dev);
 
         if (d->sample_rate) {
                 ir->jiffies_to_wait = HZ / d->sample_rate;
@@ -329,14 +328,22 @@ static int lirc_allocate_driver(struct lirc_driver *d)
                 goto out_sysfs;
 
         ir->attached = 1;
+
+        err = device_add(&ir->dev);
+        if (err)
+                goto out_cdev;
+
         mutex_unlock(&lirc_dev_lock);
 
+        get_device(ir->dev.parent);
+
         dev_info(ir->d.dev, "lirc_dev: driver %s registered at minor = %d\n",
                  ir->d.name, ir->d.minor);
         return minor;
-
+out_cdev:
+        cdev_del(&ir->cdev);
 out_sysfs:
-        device_destroy(lirc_class, MKDEV(MAJOR(lirc_base_dev), ir->d.minor));
+        put_device(&ir->dev);
 out_lock:
         mutex_unlock(&lirc_dev_lock);
 
@@ -364,7 +371,6 @@ EXPORT_SYMBOL(lirc_register_driver);
 int lirc_unregister_driver(int minor)
 {
         struct irctl *ir;
-        struct cdev *cdev;
 
         if (minor < 0 || minor >= MAX_IRCTL_DEVICES) {
                 pr_err("minor (%d) must be between 0 and %d!\n",
@@ -378,8 +384,6 @@ int lirc_unregister_driver(int minor)
                 return -ENOENT;
         }
 
-        cdev = ir->cdev;
-
         mutex_lock(&lirc_dev_lock);
 
         if (ir->d.minor != minor) {
@@ -401,22 +405,20 @@ int lirc_unregister_driver(int minor)
                 dev_dbg(ir->d.dev, LOGHEAD "releasing opened driver\n",
                         ir->d.name, ir->d.minor);
                 wake_up_interruptible(&ir->buf->wait_poll);
-                mutex_lock(&ir->irctl_lock);
+        }
 
-                if (ir->d.set_use_dec)
-                        ir->d.set_use_dec(ir->d.data);
+        mutex_lock(&ir->irctl_lock);
 
-                module_put(cdev->owner);
-                mutex_unlock(&ir->irctl_lock);
-        } else {
-                lirc_irctl_cleanup(ir);
-                cdev_del(cdev);
-                kfree(ir);
-                irctls[minor] = NULL;
-        }
+        if (ir->d.set_use_dec)
+                ir->d.set_use_dec(ir->d.data);
 
+        mutex_unlock(&ir->irctl_lock);
         mutex_unlock(&lirc_dev_lock);
 
+        device_del(&ir->dev);
+        cdev_del(&ir->cdev);
+        put_device(&ir->dev);
+
         return 0;
 }
 EXPORT_SYMBOL(lirc_unregister_driver);
@@ -424,7 +426,6 @@ EXPORT_SYMBOL(lirc_unregister_driver);
 int lirc_dev_fop_open(struct inode *inode, struct file *file)
 {
         struct irctl *ir;
-        struct cdev *cdev;
         int retval = 0;
 
         if (iminor(inode) >= MAX_IRCTL_DEVICES) {
@@ -461,18 +462,14 @@ int lirc_dev_fop_open(struct inode *inode, struct file *file)
                         goto error;
         }
 
-        cdev = ir->cdev;
-        if (try_module_get(cdev->owner)) {
-                ir->open++;
-                if (ir->d.set_use_inc)
-                        retval = ir->d.set_use_inc(ir->d.data);
-
-                if (retval) {
-                        module_put(cdev->owner);
-                        ir->open--;
-                } else if (ir->buf) {
+        ir->open++;
+        if (ir->d.set_use_inc)
+                retval = ir->d.set_use_inc(ir->d.data);
+        if (retval) {
+                ir->open--;
+        } else {
+                if (ir->buf)
                         lirc_buffer_clear(ir->buf);
-                }
                 if (ir->task)
                         wake_up_process(ir->task);
         }
@@ -487,7 +484,6 @@ EXPORT_SYMBOL(lirc_dev_fop_open);
 int lirc_dev_fop_close(struct inode *inode, struct file *file)
 {
         struct irctl *ir = irctls[iminor(inode)];
-        struct cdev *cdev;
         int ret;
 
         if (!ir) {
@@ -495,25 +491,14 @@ int lirc_dev_fop_close(struct inode *inode, struct file *file)
                 return -EINVAL;
         }
 
-        cdev = ir->cdev;
-
         ret = mutex_lock_killable(&lirc_dev_lock);
         WARN_ON(ret);
 
         rc_close(ir->d.rdev);
 
         ir->open--;
-        if (ir->attached) {
-                if (ir->d.set_use_dec)
-                        ir->d.set_use_dec(ir->d.data);
-                module_put(cdev->owner);
-        } else {
-                lirc_irctl_cleanup(ir);
-                cdev_del(cdev);
-                irctls[ir->d.minor] = NULL;
-                kfree(ir);
-        }
-
+        if (ir->d.set_use_dec)
+                ir->d.set_use_dec(ir->d.data);
         if (!ret)
                 mutex_unlock(&lirc_dev_lock);
 
@@ -623,7 +608,7 @@ long lirc_dev_fop_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
                 result = put_user(ir->d.max_timeout, (__u32 __user *)arg);
                 break;
         default:
-                result = -EINVAL;
+                result = -ENOTTY;
         }
 
         mutex_unlock(&ir->irctl_lock);
@@ -780,15 +765,12 @@ static int __init lirc_dev_init(void)
                 return retval;
         }
 
-
         pr_info("IR Remote Control driver registered, major %d\n",
                                                 MAJOR(lirc_base_dev));
 
         return 0;
 }
 
-
-
 static void __exit lirc_dev_exit(void)
 {
         class_destroy(lirc_class);
diff --git a/drivers/media/rc/mceusb.c b/drivers/media/rc/mceusb.c
index 238d8eaf7d94..93b16fe3ab38 100644
--- a/drivers/media/rc/mceusb.c
+++ b/drivers/media/rc/mceusb.c
@@ -1288,8 +1288,8 @@ static int mceusb_dev_probe(struct usb_interface *intf,
                         }
                 }
         }
-        if (ep_in == NULL) {
-                dev_dbg(&intf->dev, "inbound and/or endpoint not found");
+        if (!ep_in || !ep_out) {
+                dev_dbg(&intf->dev, "required endpoints not found\n");
                 return -ENODEV;
         }
 
diff --git a/drivers/media/rc/rc-core-priv.h b/drivers/media/rc/rc-core-priv.h
index a70a5c557434..0455b273c2fc 100644
--- a/drivers/media/rc/rc-core-priv.h
+++ b/drivers/media/rc/rc-core-priv.h
@@ -185,7 +185,7 @@ struct ir_raw_timings_manchester {
 
 int ir_raw_gen_manchester(struct ir_raw_event **ev, unsigned int max,
                           const struct ir_raw_timings_manchester *timings,
-                          unsigned int n, unsigned int data);
+                          unsigned int n, u64 data);
 
 /**
  * ir_raw_gen_pulse_space() - generate pulse and space raw events.
diff --git a/drivers/media/rc/rc-ir-raw.c b/drivers/media/rc/rc-ir-raw.c
index 7fa84b64a2ae..90f66dc7c0d7 100644
--- a/drivers/media/rc/rc-ir-raw.c
+++ b/drivers/media/rc/rc-ir-raw.c
@@ -258,13 +258,13 @@ static void ir_raw_disable_protocols(struct rc_dev *dev, u64 protocols)
  */
 int ir_raw_gen_manchester(struct ir_raw_event **ev, unsigned int max,
                           const struct ir_raw_timings_manchester *timings,
-                          unsigned int n, unsigned int data)
+                          unsigned int n, u64 data)
 {
         bool need_pulse;
-        unsigned int i;
+        u64 i;
         int ret = -ENOBUFS;
 
-        i = 1 << (n - 1);
+        i = BIT_ULL(n - 1);
 
         if (timings->leader) {
                 if (!max--)
diff --git a/drivers/media/rc/rc-main.c b/drivers/media/rc/rc-main.c
index d84533699668..6ec73357fa47 100644
--- a/drivers/media/rc/rc-main.c
+++ b/drivers/media/rc/rc-main.c
@@ -746,6 +746,8 @@ static int rc_validate_filter(struct rc_dev *dev,
                 [RC_TYPE_NECX] = 0xffffff,
                 [RC_TYPE_NEC32] = 0xffffffff,
                 [RC_TYPE_SANYO] = 0x1fffff,
+                [RC_TYPE_MCIR2_KBD] = 0xffff,
+                [RC_TYPE_MCIR2_MSE] = 0x1fffff,
                 [RC_TYPE_RC6_0] = 0xffff,
                 [RC_TYPE_RC6_6A_20] = 0xfffff,
                 [RC_TYPE_RC6_6A_24] = 0xffffff,
@@ -878,7 +880,8 @@ static const struct {
         { RC_BIT_RC5_SZ,        "rc-5-sz",      "ir-rc5-decoder"        },
         { RC_BIT_SANYO,         "sanyo",        "ir-sanyo-decoder"      },
         { RC_BIT_SHARP,         "sharp",        "ir-sharp-decoder"      },
-        { RC_BIT_MCE_KBD,       "mce_kbd",      "ir-mce_kbd-decoder"    },
+        { RC_BIT_MCIR2_KBD |
+          RC_BIT_MCIR2_MSE,     "mce_kbd",      "ir-mce_kbd-decoder"    },
         { RC_BIT_XMP,           "xmp",          "ir-xmp-decoder"        },
         { RC_BIT_CEC,           "cec",          NULL                    },
 };
@@ -1346,7 +1349,8 @@ static const char * const proto_variant_names[] = {
         [RC_TYPE_NECX] = "nec-x",
         [RC_TYPE_NEC32] = "nec-32",
         [RC_TYPE_SANYO] = "sanyo",
-        [RC_TYPE_MCE_KBD] = "mce_kbd",
+        [RC_TYPE_MCIR2_KBD] = "mcir2-kbd",
+        [RC_TYPE_MCIR2_MSE] = "mcir2-mse",
         [RC_TYPE_RC6_0] = "rc-6-0",
         [RC_TYPE_RC6_6A_20] = "rc-6-6a-20",
         [RC_TYPE_RC6_6A_24] = "rc-6-6a-24",
diff --git a/drivers/media/rc/serial_ir.c b/drivers/media/rc/serial_ir.c
index 41b54e40176c..2f0a0d248936 100644
--- a/drivers/media/rc/serial_ir.c
+++ b/drivers/media/rc/serial_ir.c
@@ -56,7 +56,7 @@ struct serial_ir_hw {
 static int type;
 static int io;
 static int irq;
-static bool iommap;
+static ulong iommap;
 static int ioshift;
 static bool softcarrier = true;
 static bool share_irq;
@@ -837,7 +837,7 @@ module_param(io, int, 0444);
 MODULE_PARM_DESC(io, "I/O address base (0x3f8 or 0x2f8)");
 
 /* some architectures (e.g. intel xscale) have memory mapped registers */
-module_param(iommap, bool, 0444);
+module_param(iommap, ulong, 0444);
 MODULE_PARM_DESC(iommap, "physical base for memory mapped I/O (0 = no memory mapped io)");
 
 /*
diff --git a/drivers/media/rc/sir_ir.c b/drivers/media/rc/sir_ir.c
new file mode 100644
index 000000000000..e12ec50bf0bf
--- /dev/null
+++ b/drivers/media/rc/sir_ir.c
@@ -0,0 +1,438 @@
+/*
+ * IR SIR driver, (C) 2000 Milan Pikula <www@fornax.sk>
+ *
+ * sir_ir - Device driver for use with SIR (serial infra red)
+ * mode of IrDA on many notebooks.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/serial_reg.h>
+#include <linux/ktime.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+
+#include <media/rc-core.h>
+
+/* SECTION: Definitions */
+#define PULSE '['
+
+/* 9bit * 1s/115200bit in milli seconds = 78.125ms*/
+#define TIME_CONST (9000000ul / 115200ul)
+
+/* timeout for sequences in jiffies (=5/100s), must be longer than TIME_CONST */
+#define SIR_TIMEOUT     (HZ * 5 / 100)
+
+/* onboard sir ports are typically com3 */
+static int io = 0x3e8;
+static int irq = 4;
+static int threshold = 3;
+
+static DEFINE_SPINLOCK(timer_lock);
+static struct timer_list timerlist;
+/* time of last signal change detected */
+static ktime_t last;
+/* time of last UART data ready interrupt */
+static ktime_t last_intr_time;
+static int last_value;
+static struct rc_dev *rcdev;
+
+static struct platform_device *sir_ir_dev;
+
+static DEFINE_SPINLOCK(hardware_lock);
+
+/* SECTION: Prototypes */
+
+/* Communication with user-space */
+static void add_read_queue(int flag, unsigned long val);
+static int init_chrdev(void);
+/* Hardware */
+static irqreturn_t sir_interrupt(int irq, void *dev_id);
+static void send_space(unsigned long len);
+static void send_pulse(unsigned long len);
+static int init_hardware(void);
+static void drop_hardware(void);
+/* Initialisation */
+static int init_port(void);
+static void drop_port(void);
+
+static inline unsigned int sinp(int offset)
+{
+        return inb(io + offset);
+}
+
+static inline void soutp(int offset, int value)
+{
+        outb(value, io + offset);
+}
+
+/* SECTION: Communication with user-space */
+static int sir_tx_ir(struct rc_dev *dev, unsigned int *tx_buf,
+                     unsigned int count)
+{
+        unsigned long flags;
+        int i;
+
+        local_irq_save(flags);
+        for (i = 0; i < count;) {
+                if (tx_buf[i])
+                        send_pulse(tx_buf[i]);
+                i++;
+                if (i >= count)
+                        break;
+                if (tx_buf[i])
+                        send_space(tx_buf[i]);
+                i++;
+        }
+        local_irq_restore(flags);
+
+        return count;
+}
+
+static void add_read_queue(int flag, unsigned long val)
+{
+        DEFINE_IR_RAW_EVENT(ev);
+
+        pr_debug("add flag %d with val %lu\n", flag, val);
+
+        /*
+         * statistically, pulses are ~TIME_CONST/2 too long. we could
+         * maybe make this more exact, but this is good enough
+         */
+        if (flag) {
+                /* pulse */
+                if (val > TIME_CONST / 2)
+                        val -= TIME_CONST / 2;
+                else /* should not ever happen */
+                        val = 1;
+                ev.pulse = true;
+        } else {
+                val += TIME_CONST / 2;
+        }
+        ev.duration = US_TO_NS(val);
+
+        ir_raw_event_store_with_filter(rcdev, &ev);
+}
+
+static int init_chrdev(void)
+{
+        rcdev = devm_rc_allocate_device(&sir_ir_dev->dev, RC_DRIVER_IR_RAW);
+        if (!rcdev)
+                return -ENOMEM;
+
+        rcdev->input_name = "SIR IrDA port";
+        rcdev->input_phys = KBUILD_MODNAME "/input0";
+        rcdev->input_id.bustype = BUS_HOST;
+        rcdev->input_id.vendor = 0x0001;
+        rcdev->input_id.product = 0x0001;
+        rcdev->input_id.version = 0x0100;
+        rcdev->tx_ir = sir_tx_ir;
+        rcdev->allowed_protocols = RC_BIT_ALL_IR_DECODER;
+        rcdev->driver_name = KBUILD_MODNAME;
+        rcdev->map_name = RC_MAP_RC6_MCE;
+        rcdev->timeout = IR_DEFAULT_TIMEOUT;
+        rcdev->dev.parent = &sir_ir_dev->dev;
+
+        return devm_rc_register_device(&sir_ir_dev->dev, rcdev);
+}
+
+/* SECTION: Hardware */
+static void sir_timeout(unsigned long data)
+{
+        /*
+         * if last received signal was a pulse, but receiving stopped
+         * within the 9 bit frame, we need to finish this pulse and
+         * simulate a signal change to from pulse to space. Otherwise
+         * upper layers will receive two sequences next time.
+         */
+
+        unsigned long flags;
+        unsigned long pulse_end;
+
+        /* avoid interference with interrupt */
+        spin_lock_irqsave(&timer_lock, flags);
+        if (last_value) {
+                /* clear unread bits in UART and restart */
+                outb(UART_FCR_CLEAR_RCVR, io + UART_FCR);
+                /* determine 'virtual' pulse end: */
+                pulse_end = min_t(unsigned long,
+                                  ktime_us_delta(last, last_intr_time),
+                                  IR_MAX_DURATION);
+                dev_dbg(&sir_ir_dev->dev, "timeout add %d for %lu usec\n",
+                        last_value, pulse_end);
+                add_read_queue(last_value, pulse_end);
+                last_value = 0;
+                last = last_intr_time;
+        }
+        spin_unlock_irqrestore(&timer_lock, flags);
+        ir_raw_event_handle(rcdev);
+}
+
+static irqreturn_t sir_interrupt(int irq, void *dev_id)
+{
+        unsigned char data;
+        ktime_t curr_time;
+        static unsigned long delt;
+        unsigned long deltintr;
+        unsigned long flags;
+        int iir, lsr;
+
+        while ((iir = inb(io + UART_IIR) & UART_IIR_ID)) {
+                switch (iir & UART_IIR_ID) { /* FIXME toto treba preriedit */
+                case UART_IIR_MSI:
+                        (void)inb(io + UART_MSR);
+                        break;
+                case UART_IIR_RLSI:
+                case UART_IIR_THRI:
+                        (void)inb(io + UART_LSR);
+                        break;
+                case UART_IIR_RDI:
+                        /* avoid interference with timer */
+                        spin_lock_irqsave(&timer_lock, flags);
+                        do {
+                                del_timer(&timerlist);
+                                data = inb(io + UART_RX);
+                                curr_time = ktime_get();
+                                delt = min_t(unsigned long,
+                                             ktime_us_delta(last, curr_time),
+                                             IR_MAX_DURATION);
+                                deltintr = min_t(unsigned long,
+                                                 ktime_us_delta(last_intr_time,
+                                                                curr_time),
+                                                 IR_MAX_DURATION);
+                                dev_dbg(&sir_ir_dev->dev, "t %lu, d %d\n",
+                                        deltintr, (int)data);
+                                /*
+                                 * if nothing came in last X cycles,
+                                 * it was gap
+                                 */
+                                if (deltintr > TIME_CONST * threshold) {
+                                        if (last_value) {
+                                                dev_dbg(&sir_ir_dev->dev, "GAP\n");
+                                                /* simulate signal change */
+                                                add_read_queue(last_value,
+                                                               delt -
+                                                               deltintr);
+                                                last_value = 0;
+                                                last = last_intr_time;
+                                                delt = deltintr;
+                                        }
+                                }
+                                data = 1;
+                                if (data ^ last_value) {
+                                        /*
+                                         * deltintr > 2*TIME_CONST, remember?
+                                         * the other case is timeout
+                                         */
+                                        add_read_queue(last_value,
+                                                       delt - TIME_CONST);
+                                        last_value = data;
+                                        last = curr_time;
+                                        last = ktime_sub_us(last,
+                                                            TIME_CONST);
+                                }
+                                last_intr_time = curr_time;
+                                if (data) {
+                                        /*
+                                         * start timer for end of
+                                         * sequence detection
+                                         */
+                                        timerlist.expires = jiffies +
+                                                                SIR_TIMEOUT;
+                                        add_timer(&timerlist);
+                                }
+
+                                lsr = inb(io + UART_LSR);
+                        } while (lsr & UART_LSR_DR); /* data ready */
+                        spin_unlock_irqrestore(&timer_lock, flags);
+                        break;
+                default:
+                        break;
+                }
+        }
+        ir_raw_event_handle(rcdev);
+        return IRQ_RETVAL(IRQ_HANDLED);
+}
+
+static void send_space(unsigned long len)
+{
+        usleep_range(len, len + 25);
+}
+
+static void send_pulse(unsigned long len)
+{
+        long bytes_out = len / TIME_CONST;
+
+        if (bytes_out == 0)
+                bytes_out++;
+
+        while (bytes_out--) {
+                outb(PULSE, io + UART_TX);
+                /* FIXME treba seriozne cakanie z char/serial.c */
+                while (!(inb(io + UART_LSR) & UART_LSR_THRE))
+                        ;
+        }
+}
+
+static int init_hardware(void)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&hardware_lock, flags);
+        /* reset UART */
+        outb(0, io + UART_MCR);
+        outb(0, io + UART_IER);
+        /* init UART */
+        /* set DLAB, speed = 115200 */
+        outb(UART_LCR_DLAB | UART_LCR_WLEN7, io + UART_LCR);
+        outb(1, io + UART_DLL); outb(0, io + UART_DLM);
+        /* 7N1+start = 9 bits at 115200 ~ 3 bits at 44000 */
+        outb(UART_LCR_WLEN7, io + UART_LCR);
+        /* FIFO operation */
+        outb(UART_FCR_ENABLE_FIFO, io + UART_FCR);
+        /* interrupts */
+        /* outb(UART_IER_RLSI|UART_IER_RDI|UART_IER_THRI, io + UART_IER); */
+        outb(UART_IER_RDI, io + UART_IER);
+        /* turn on UART */
+        outb(UART_MCR_DTR | UART_MCR_RTS | UART_MCR_OUT2, io + UART_MCR);
+        spin_unlock_irqrestore(&hardware_lock, flags);
+        return 0;
+}
+
+static void drop_hardware(void)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&hardware_lock, flags);
+
+        /* turn off interrupts */
+        outb(0, io + UART_IER);
+
+        spin_unlock_irqrestore(&hardware_lock, flags);
+}
+
+/* SECTION: Initialisation */
+
+static int init_port(void)
+{
+        int retval;
+
+        setup_timer(&timerlist, sir_timeout, 0);
+
+        /* get I/O port access and IRQ line */
+        if (!request_region(io, 8, KBUILD_MODNAME)) {
+                pr_err("i/o port 0x%.4x already in use.\n", io);
+                return -EBUSY;
+        }
+        retval = request_irq(irq, sir_interrupt, 0,
+                             KBUILD_MODNAME, NULL);
+        if (retval < 0) {
+                release_region(io, 8);
+                pr_err("IRQ %d already in use.\n", irq);
+                return retval;
+        }
+        pr_info("I/O port 0x%.4x, IRQ %d.\n", io, irq);
+
+        return 0;
+}
+
+static void drop_port(void)
+{
+        free_irq(irq, NULL);
+        del_timer_sync(&timerlist);
+        release_region(io, 8);
+}
+
+static int init_sir_ir(void)
+{
+        int retval;
+
+        retval = init_port();
+        if (retval < 0)
+                return retval;
+        init_hardware();
+        return 0;
+}
+
+static int sir_ir_probe(struct platform_device *dev)
+{
+        int retval;
+
+        retval = init_chrdev();
+        if (retval < 0)
+                return retval;
+
+        return init_sir_ir();
+}
+
+static int sir_ir_remove(struct platform_device *dev)
+{
+        return 0;
+}
+
+static struct platform_driver sir_ir_driver = {
+        .probe          = sir_ir_probe,
+        .remove         = sir_ir_remove,
+        .driver         = {
+                .name   = "sir_ir",
+        },
+};
+
+static int __init sir_ir_init(void)
+{
+        int retval;
+
+        retval = platform_driver_register(&sir_ir_driver);
+        if (retval)
+                return retval;
+
+        sir_ir_dev = platform_device_alloc("sir_ir", 0);
+        if (!sir_ir_dev) {
+                retval = -ENOMEM;
+                goto pdev_alloc_fail;
+        }
+
+        retval = platform_device_add(sir_ir_dev);
+        if (retval)
+                goto pdev_add_fail;
+
+        return 0;
+
+pdev_add_fail:
+        platform_device_put(sir_ir_dev);
+pdev_alloc_fail:
+        platform_driver_unregister(&sir_ir_driver);
+        return retval;
+}
+
+static void __exit sir_ir_exit(void)
+{
+        drop_hardware();
+        drop_port();
+        platform_device_unregister(sir_ir_dev);
+        platform_driver_unregister(&sir_ir_driver);
+}
+
+module_init(sir_ir_init);
+module_exit(sir_ir_exit);
+
+MODULE_DESCRIPTION("Infrared receiver driver for SIR type serial ports");
+MODULE_AUTHOR("Milan Pikula");
+MODULE_LICENSE("GPL");
+
+module_param(io, int, 0444);
+MODULE_PARM_DESC(io, "I/O address base (0x3f8 or 0x2f8)");
+
+module_param(irq, int, 0444);
+MODULE_PARM_DESC(irq, "Interrupt (4 or 3)");
+
+module_param(threshold, int, 0444);
+MODULE_PARM_DESC(threshold, "space detection threshold (3)");
diff --git a/drivers/media/rc/st_rc.c b/drivers/media/rc/st_rc.c
index f0d7190e3919..a08e1dd06124 100644
--- a/drivers/media/rc/st_rc.c
+++ b/drivers/media/rc/st_rc.c
@@ -165,8 +165,7 @@ static void st_rc_hardware_init(struct st_rc_device *dev)
         unsigned int rx_sampling_freq_div;
 
         /* Enable the IP */
-        if (dev->rstc)
-                reset_control_deassert(dev->rstc);
+        reset_control_deassert(dev->rstc);
 
         clk_prepare_enable(dev->sys_clock);
         baseclock = clk_get_rate(dev->sys_clock);
@@ -281,10 +280,11 @@ static int st_rc_probe(struct platform_device *pdev)
         else
                 rc_dev->rx_base = rc_dev->base;
 
-
         rc_dev->rstc = reset_control_get_optional(dev, NULL);
-        if (IS_ERR(rc_dev->rstc))
-                rc_dev->rstc = NULL;
+        if (IS_ERR(rc_dev->rstc)) {
+                ret = PTR_ERR(rc_dev->rstc);
+                goto err;
+        }
 
         rc_dev->dev = dev;
         platform_set_drvdata(pdev, rc_dev);
@@ -298,7 +298,7 @@ static int st_rc_probe(struct platform_device *pdev)
         rdev->open = st_rc_open;
         rdev->close = st_rc_close;
         rdev->driver_name = IR_ST_NAME;
-        rdev->map_name = RC_MAP_LIRC;
+        rdev->map_name = RC_MAP_EMPTY;
         rdev->input_name = "ST Remote Control Receiver";
 
         ret = rc_register_device(rdev);
@@ -352,8 +352,7 @@ static int st_rc_suspend(struct device *dev)
                 writel(0x00, rc_dev->rx_base + IRB_RX_EN);
                 writel(0x00, rc_dev->rx_base + IRB_RX_INT_EN);
                 clk_disable_unprepare(rc_dev->sys_clock);
-                if (rc_dev->rstc)
-                        reset_control_assert(rc_dev->rstc);
+                reset_control_assert(rc_dev->rstc);
         }
 
         return 0;
diff --git a/drivers/media/rc/sunxi-cir.c b/drivers/media/rc/sunxi-cir.c
index 25b006167810..4b785dd775c1 100644
--- a/drivers/media/rc/sunxi-cir.c
+++ b/drivers/media/rc/sunxi-cir.c
@@ -174,16 +174,11 @@ static int sunxi_ir_probe(struct platform_device *pdev)
 
         /* Reset (optional) */
         ir->rst = devm_reset_control_get_optional(dev, NULL);
-        if (IS_ERR(ir->rst)) {
-                ret = PTR_ERR(ir->rst);
-                if (ret == -EPROBE_DEFER)
-                        return ret;
-                ir->rst = NULL;
-        } else {
-                ret = reset_control_deassert(ir->rst);
-                if (ret)
-                        return ret;
-        }
+        if (IS_ERR(ir->rst))
+                return PTR_ERR(ir->rst);
+        ret = reset_control_deassert(ir->rst);
+        if (ret)
+                return ret;
 
         ret = clk_set_rate(ir->clk, SUNXI_IR_BASE_CLK);
         if (ret) {
@@ -291,8 +286,7 @@ exit_clkdisable_clk:
 exit_clkdisable_apb_clk:
         clk_disable_unprepare(ir->apb_clk);
 exit_reset_assert:
-        if (ir->rst)
-                reset_control_assert(ir->rst);
+        reset_control_assert(ir->rst);
 
         return ret;
 }
@@ -304,8 +298,7 @@ static int sunxi_ir_remove(struct platform_device *pdev)
 
         clk_disable_unprepare(ir->clk);
         clk_disable_unprepare(ir->apb_clk);
-        if (ir->rst)
-                reset_control_assert(ir->rst);
+        reset_control_assert(ir->rst);
 
         spin_lock_irqsave(&ir->ir_lock, flags);
         /* disable IR IRQ */
diff --git a/drivers/media/rc/winbond-cir.c b/drivers/media/rc/winbond-cir.c
index dc1c8305ad23..5a4d4a611197 100644
--- a/drivers/media/rc/winbond-cir.c
+++ b/drivers/media/rc/winbond-cir.c
@@ -1082,7 +1082,9 @@ wbcir_probe(struct pnp_dev *device, const struct pnp_device_id *dev_id)
         data->dev->tx_ir = wbcir_tx;
         data->dev->priv = data;
         data->dev->dev.parent = &device->dev;
-        data->dev->timeout = MS_TO_NS(100);
+        data->dev->min_timeout = 1;
+        data->dev->timeout = IR_DEFAULT_TIMEOUT;
+        data->dev->max_timeout = 10 * IR_DEFAULT_TIMEOUT;
         data->dev->rx_resolution = US_TO_NS(2);
         data->dev->allowed_protocols = RC_BIT_ALL_IR_DECODER;
         data->dev->allowed_wakeup_protocols = RC_BIT_NEC | RC_BIT_NECX |
diff --git a/drivers/media/tuners/si2157.c b/drivers/media/tuners/si2157.c
index 57b250847cd3..e35b1faf0ddc 100644
--- a/drivers/media/tuners/si2157.c
+++ b/drivers/media/tuners/si2157.c
@@ -106,6 +106,9 @@ static int si2157_init(struct dvb_frontend *fe)
         if (dev->chiptype == SI2157_CHIPTYPE_SI2146) {
                 memcpy(cmd.args, "\xc0\x05\x01\x00\x00\x0b\x00\x00\x01", 9);
                 cmd.wlen = 9;
+        } else if (dev->chiptype == SI2157_CHIPTYPE_SI2141) {
+                memcpy(cmd.args, "\xc0\x00\x0d\x0e\x00\x01\x01\x01\x01\x03", 10);
+                cmd.wlen = 10;
         } else {
                 memcpy(cmd.args, "\xc0\x00\x0c\x00\x00\x01\x01\x01\x01\x01\x01\x02\x00\x00\x01", 15);
                 cmd.wlen = 15;
@@ -115,6 +118,15 @@ static int si2157_init(struct dvb_frontend *fe)
         if (ret)
                 goto err;
 
+        /* Si2141 needs a second command before it answers the revision query */
+        if (dev->chiptype == SI2157_CHIPTYPE_SI2141) {
+                memcpy(cmd.args, "\xc0\x08\x01\x02\x00\x00\x01", 7);
+                cmd.wlen = 7;
+                ret = si2157_cmd_execute(client, &cmd);
+                if (ret)
+                        goto err;
+        }
+
         /* query chip revision */
         memcpy(cmd.args, "\x02", 1);
         cmd.wlen = 1;
@@ -131,12 +143,16 @@ static int si2157_init(struct dvb_frontend *fe)
         #define SI2157_A30 ('A' << 24 | 57 << 16 | '3' << 8 | '0' << 0)
         #define SI2147_A30 ('A' << 24 | 47 << 16 | '3' << 8 | '0' << 0)
         #define SI2146_A10 ('A' << 24 | 46 << 16 | '1' << 8 | '0' << 0)
+        #define SI2141_A10 ('A' << 24 | 41 << 16 | '1' << 8 | '0' << 0)
 
         switch (chip_id) {
         case SI2158_A20:
         case SI2148_A20:
                 fw_name = SI2158_A20_FIRMWARE;
                 break;
+        case SI2141_A10:
+                fw_name = SI2141_A10_FIRMWARE;
+                break;
         case SI2157_A30:
         case SI2147_A30:
         case SI2146_A10:
@@ -371,7 +387,7 @@ static int si2157_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
 
 static const struct dvb_tuner_ops si2157_ops = {
         .info = {
-                .name           = "Silicon Labs Si2146/2147/2148/2157/2158",
+                .name           = "Silicon Labs Si2141/Si2146/2147/2148/2157/2158",
                 .frequency_min  = 42000000,
                 .frequency_max  = 870000000,
         },
@@ -471,6 +487,7 @@ static int si2157_probe(struct i2c_client *client,
 #endif
 
         dev_info(&client->dev, "Silicon Labs %s successfully attached\n",
+                        dev->chiptype == SI2157_CHIPTYPE_SI2141 ?  "Si2141" :
                         dev->chiptype == SI2157_CHIPTYPE_SI2146 ?
                         "Si2146" : "Si2147/2148/2157/2158");
 
@@ -508,6 +525,7 @@ static int si2157_remove(struct i2c_client *client)
 static const struct i2c_device_id si2157_id_table[] = {
         {"si2157", SI2157_CHIPTYPE_SI2157},
         {"si2146", SI2157_CHIPTYPE_SI2146},
+        {"si2141", SI2157_CHIPTYPE_SI2141},
         {}
 };
 MODULE_DEVICE_TABLE(i2c, si2157_id_table);
@@ -524,7 +542,8 @@ static struct i2c_driver si2157_driver = {
 
 module_i2c_driver(si2157_driver);
 
-MODULE_DESCRIPTION("Silicon Labs Si2146/2147/2148/2157/2158 silicon tuner driver");
+MODULE_DESCRIPTION("Silicon Labs Si2141/Si2146/2147/2148/2157/2158 silicon tuner driver");
 MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
 MODULE_LICENSE("GPL");
 MODULE_FIRMWARE(SI2158_A20_FIRMWARE);
+MODULE_FIRMWARE(SI2141_A10_FIRMWARE);
diff --git a/drivers/media/tuners/si2157_priv.h b/drivers/media/tuners/si2157_priv.h
index d6b2c7b44053..e6436f74abaa 100644
--- a/drivers/media/tuners/si2157_priv.h
+++ b/drivers/media/tuners/si2157_priv.h
@@ -42,6 +42,7 @@ struct si2157_dev {
 
 #define SI2157_CHIPTYPE_SI2157 0
 #define SI2157_CHIPTYPE_SI2146 1
+#define SI2157_CHIPTYPE_SI2141 2
 
 /* firmware command struct */
 #define SI2157_ARGLEN      30
@@ -52,5 +53,6 @@ struct si2157_cmd {
 };
 
 #define SI2158_A20_FIRMWARE "dvb-tuner-si2158-a20-01.fw"
+#define SI2141_A10_FIRMWARE "dvb-tuner-si2141-a10-01.fw"
 
 #endif
diff --git a/drivers/media/tuners/xc5000.c b/drivers/media/tuners/xc5000.c
index 91947cf1950e..e823aafce276 100644
--- a/drivers/media/tuners/xc5000.c
+++ b/drivers/media/tuners/xc5000.c
@@ -1184,8 +1184,7 @@ static int xc_load_fw_and_init_tuner(struct dvb_frontend *fe, int force)
                 /* Start the tuner self-calibration process */
                 ret = xc_initialize(priv);
                 if (ret) {
-                        printk(KERN_ERR
-                               "xc5000: Can't request Self-callibration.");
+                        printk(KERN_ERR "xc5000: Can't request self-calibration.");
                         continue;
                 }
 
diff --git a/drivers/media/usb/Kconfig b/drivers/media/usb/Kconfig
index c9644b62f91a..b24e753c4766 100644
--- a/drivers/media/usb/Kconfig
+++ b/drivers/media/usb/Kconfig
@@ -63,6 +63,7 @@ endif
 if MEDIA_CEC_SUPPORT
         comment "USB HDMI CEC adapters"
 source "drivers/media/usb/pulse8-cec/Kconfig"
+source "drivers/media/usb/rainshadow-cec/Kconfig"
 endif
 
 endif #MEDIA_USB_SUPPORT
diff --git a/drivers/media/usb/Makefile b/drivers/media/usb/Makefile
index 0f15e3351ddc..738b993ec8b0 100644
--- a/drivers/media/usb/Makefile
+++ b/drivers/media/usb/Makefile
@@ -25,3 +25,4 @@ obj-$(CONFIG_VIDEO_USBTV) += usbtv/
 obj-$(CONFIG_VIDEO_GO7007) += go7007/
 obj-$(CONFIG_DVB_AS102) += as102/
 obj-$(CONFIG_USB_PULSE8_CEC) += pulse8-cec/
+obj-$(CONFIG_USB_RAINSHADOW_CEC) += rainshadow-cec/
diff --git a/drivers/media/usb/au0828/au0828-cards.c b/drivers/media/usb/au0828/au0828-cards.c
index 313f659f0bfb..43bfa778b070 100644
--- a/drivers/media/usb/au0828/au0828-cards.c
+++ b/drivers/media/usb/au0828/au0828-cards.c
@@ -153,7 +153,7 @@ static void hauppauge_eeprom(struct au0828_dev *dev, u8 *eeprom_data)
 {
         struct tveeprom tv;
 
-        tveeprom_hauppauge_analog(&dev->i2c_client, &tv, eeprom_data);
+        tveeprom_hauppauge_analog(&tv, eeprom_data);
         dev->board.tuner_type = tv.tuner_type;
 
         /* Make sure we support the board model */
diff --git a/drivers/media/usb/au0828/au0828-video.c b/drivers/media/usb/au0828/au0828-video.c
index 16f9125a985a..2a255bd32bb3 100644
--- a/drivers/media/usb/au0828/au0828-video.c
+++ b/drivers/media/usb/au0828/au0828-video.c
@@ -809,16 +809,9 @@ static void au0828_analog_stream_reset(struct au0828_dev *dev)
  */
 static int au0828_stream_interrupt(struct au0828_dev *dev)
 {
-        int ret = 0;
-
         dev->stream_state = STREAM_INTERRUPT;
         if (test_bit(DEV_DISCONNECTED, &dev->dev_state))
                 return -ENODEV;
-        else if (ret) {
-                set_bit(DEV_MISCONFIGURED, &dev->dev_state);
-                dprintk(1, "%s device is misconfigured!\n", __func__);
-                return ret;
-        }
         return 0;
 }
 
diff --git a/drivers/media/usb/cx231xx/cx231xx-audio.c b/drivers/media/usb/cx231xx/cx231xx-audio.c
index cf80842dfa08..a050d125934c 100644
--- a/drivers/media/usb/cx231xx/cx231xx-audio.c
+++ b/drivers/media/usb/cx231xx/cx231xx-audio.c
@@ -670,10 +670,8 @@ static int cx231xx_audio_init(struct cx231xx *dev)
 
         spin_lock_init(&adev->slock);
         err = snd_pcm_new(card, "Cx231xx Audio", 0, 0, 1, &pcm);
-        if (err < 0) {
-                snd_card_free(card);
-                return err;
-        }
+        if (err < 0)
+                goto err_free_card;
 
         snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_CAPTURE,
                         &snd_cx231xx_pcm_capture);
@@ -687,10 +685,9 @@ static int cx231xx_audio_init(struct cx231xx *dev)
         INIT_WORK(&dev->wq_trigger, audio_trigger);
 
         err = snd_card_register(card);
-        if (err < 0) {
-                snd_card_free(card);
-                return err;
-        }
+        if (err < 0)
+                goto err_free_card;
+
         adev->sndcard = card;
         adev->udev = dev->udev;
 
@@ -700,6 +697,11 @@ static int cx231xx_audio_init(struct cx231xx *dev)
                                             hs_config_info[0].interface_info.
                                             audio_index + 1];
 
+        if (uif->altsetting[0].desc.bNumEndpoints < isoc_pipe + 1) {
+                err = -ENODEV;
+                goto err_free_card;
+        }
+
         adev->end_point_addr =
             uif->altsetting[0].endpoint[isoc_pipe].desc.
                         bEndpointAddress;
@@ -709,13 +711,20 @@ static int cx231xx_audio_init(struct cx231xx *dev)
                 "audio EndPoint Addr 0x%x, Alternate settings: %i\n",
                 adev->end_point_addr, adev->num_alt);
         adev->alt_max_pkt_size = kmalloc(32 * adev->num_alt, GFP_KERNEL);
-
-        if (adev->alt_max_pkt_size == NULL)
-                return -ENOMEM;
+        if (!adev->alt_max_pkt_size) {
+                err = -ENOMEM;
+                goto err_free_card;
+        }
 
         for (i = 0; i < adev->num_alt; i++) {
-                u16 tmp =
-                    le16_to_cpu(uif->altsetting[i].endpoint[isoc_pipe].desc.
+                u16 tmp;
+
+                if (uif->altsetting[i].desc.bNumEndpoints < isoc_pipe + 1) {
+                        err = -ENODEV;
+                        goto err_free_pkt_size;
+                }
+
+                tmp = le16_to_cpu(uif->altsetting[i].endpoint[isoc_pipe].desc.
                                 wMaxPacketSize);
                 adev->alt_max_pkt_size[i] =
                     (tmp & 0x07ff) * (((tmp & 0x1800) >> 11) + 1);
@@ -725,6 +734,13 @@ static int cx231xx_audio_init(struct cx231xx *dev)
         }
 
         return 0;
+
+err_free_pkt_size:
+        kfree(adev->alt_max_pkt_size);
+err_free_card:
+        snd_card_free(card);
+
+        return err;
 }
 
 static int cx231xx_audio_fini(struct cx231xx *dev)
diff --git a/drivers/media/usb/cx231xx/cx231xx-cards.c b/drivers/media/usb/cx231xx/cx231xx-cards.c
index f730fdbc9156..a1007d005290 100644
--- a/drivers/media/usb/cx231xx/cx231xx-cards.c
+++ b/drivers/media/usb/cx231xx/cx231xx-cards.c
@@ -1165,8 +1165,7 @@ void cx231xx_card_setup(struct cx231xx *dev)
                         e->client.addr = 0xa0 >> 1;
 
                         read_eeprom(dev, &e->client, e->eeprom, sizeof(e->eeprom));
-                        tveeprom_hauppauge_analog(&e->client,
-                                                &e->tvee, e->eeprom + 0xc0);
+                        tveeprom_hauppauge_analog(&e->tvee, e->eeprom + 0xc0);
                         kfree(e);
                         break;
                 }
@@ -1426,6 +1425,9 @@ static int cx231xx_init_v4l2(struct cx231xx *dev,
 
         uif = udev->actconfig->interface[idx];
 
+        if (uif->altsetting[0].desc.bNumEndpoints < isoc_pipe + 1)
+                return -ENODEV;
+
         dev->video_mode.end_point_addr = uif->altsetting[0].endpoint[isoc_pipe].desc.bEndpointAddress;
         dev->video_mode.num_alt = uif->num_altsetting;
 
@@ -1439,7 +1441,12 @@ static int cx231xx_init_v4l2(struct cx231xx *dev,
                 return -ENOMEM;
 
         for (i = 0; i < dev->video_mode.num_alt; i++) {
-                u16 tmp = le16_to_cpu(uif->altsetting[i].endpoint[isoc_pipe].desc.wMaxPacketSize);
+                u16 tmp;
+
+                if (uif->altsetting[i].desc.bNumEndpoints < isoc_pipe + 1)
+                        return -ENODEV;
+
+                tmp = le16_to_cpu(uif->altsetting[i].endpoint[isoc_pipe].desc.wMaxPacketSize);
                 dev->video_mode.alt_max_pkt_size[i] = (tmp & 0x07ff) * (((tmp & 0x1800) >> 11) + 1);
                 dev_dbg(dev->dev,
                         "Alternate setting %i, max size= %i\n", i,
@@ -1456,6 +1463,9 @@ static int cx231xx_init_v4l2(struct cx231xx *dev,
         }
         uif = udev->actconfig->interface[idx];
 
+        if (uif->altsetting[0].desc.bNumEndpoints < isoc_pipe + 1)
+                return -ENODEV;
+
         dev->vbi_mode.end_point_addr =
             uif->altsetting[0].endpoint[isoc_pipe].desc.
                         bEndpointAddress;
@@ -1472,8 +1482,12 @@ static int cx231xx_init_v4l2(struct cx231xx *dev,
                 return -ENOMEM;
 
         for (i = 0; i < dev->vbi_mode.num_alt; i++) {
-                u16 tmp =
-                    le16_to_cpu(uif->altsetting[i].endpoint[isoc_pipe].
+                u16 tmp;
+
+                if (uif->altsetting[i].desc.bNumEndpoints < isoc_pipe + 1)
+                        return -ENODEV;
+
+                tmp = le16_to_cpu(uif->altsetting[i].endpoint[isoc_pipe].
                                 desc.wMaxPacketSize);
                 dev->vbi_mode.alt_max_pkt_size[i] =
                     (tmp & 0x07ff) * (((tmp & 0x1800) >> 11) + 1);
@@ -1493,6 +1507,9 @@ static int cx231xx_init_v4l2(struct cx231xx *dev,
         }
         uif = udev->actconfig->interface[idx];
 
+        if (uif->altsetting[0].desc.bNumEndpoints < isoc_pipe + 1)
+                return -ENODEV;
+
         dev->sliced_cc_mode.end_point_addr =
             uif->altsetting[0].endpoint[isoc_pipe].desc.
                         bEndpointAddress;
@@ -1507,7 +1524,12 @@ static int cx231xx_init_v4l2(struct cx231xx *dev,
                 return -ENOMEM;
 
         for (i = 0; i < dev->sliced_cc_mode.num_alt; i++) {
-                u16 tmp = le16_to_cpu(uif->altsetting[i].endpoint[isoc_pipe].
+                u16 tmp;
+
+                if (uif->altsetting[i].desc.bNumEndpoints < isoc_pipe + 1)
+                        return -ENODEV;
+
+                tmp = le16_to_cpu(uif->altsetting[i].endpoint[isoc_pipe].
                                 desc.wMaxPacketSize);
                 dev->sliced_cc_mode.alt_max_pkt_size[i] =
                     (tmp & 0x07ff) * (((tmp & 0x1800) >> 11) + 1);
@@ -1676,6 +1698,11 @@ static int cx231xx_usb_probe(struct usb_interface *interface,
                 }
                 uif = udev->actconfig->interface[idx];
 
+                if (uif->altsetting[0].desc.bNumEndpoints < isoc_pipe + 1) {
+                        retval = -ENODEV;
+                        goto err_video_alt;
+                }
+
                 dev->ts1_mode.end_point_addr =
                     uif->altsetting[0].endpoint[isoc_pipe].
                                 desc.bEndpointAddress;
@@ -1693,7 +1720,14 @@ static int cx231xx_usb_probe(struct usb_interface *interface,
                 }
 
                 for (i = 0; i < dev->ts1_mode.num_alt; i++) {
-                        u16 tmp = le16_to_cpu(uif->altsetting[i].
+                        u16 tmp;
+
+                        if (uif->altsetting[i].desc.bNumEndpoints < isoc_pipe + 1) {
+                                retval = -ENODEV;
+                                goto err_video_alt;
+                        }
+
+                        tmp = le16_to_cpu(uif->altsetting[i].
                                                 endpoint[isoc_pipe].desc.
                                                 wMaxPacketSize);
                         dev->ts1_mode.alt_max_pkt_size[i] =
diff --git a/drivers/media/usb/cx231xx/cx231xx-i2c.c b/drivers/media/usb/cx231xx/cx231xx-i2c.c
index dff514e147da..8d95b1154e12 100644
--- a/drivers/media/usb/cx231xx/cx231xx-i2c.c
+++ b/drivers/media/usb/cx231xx/cx231xx-i2c.c
@@ -491,20 +491,24 @@ void cx231xx_do_i2c_scan(struct cx231xx *dev, int i2c_port)
 {
         unsigned char buf;
         int i, rc;
-        struct i2c_client client;
+        struct i2c_adapter *adap;
+        struct i2c_msg msg = {
+                .flags = I2C_M_RD,
+                .len = 1,
+                .buf = &buf,
+        };
 
         if (!i2c_scan)
                 return;
 
         /* Don't generate I2C errors during scan */
         dev->i2c_scan_running = true;
-
-        memset(&client, 0, sizeof(client));
-        client.adapter = cx231xx_get_i2c_adap(dev, i2c_port);
+        adap = cx231xx_get_i2c_adap(dev, i2c_port);
 
         for (i = 0; i < 128; i++) {
-                client.addr = i;
-                rc = i2c_master_recv(&client, &buf, 0);
+                msg.addr = i;
+                rc = i2c_transfer(adap, &msg, 1);
+
                 if (rc < 0)
                         continue;
                 dev_info(dev->dev,
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf.c b/drivers/media/usb/dvb-usb-v2/mxl111sf.c
index 80c635980526..abf69d8fa469 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf.c
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf.c
@@ -919,7 +919,12 @@ static int mxl111sf_init(struct dvb_usb_device *d)
         struct mxl111sf_state *state = d_to_priv(d);
         int ret;
         static u8 eeprom[256];
-        struct i2c_client c;
+        u8 reg = 0;
+        struct i2c_msg msg[2] = {
+                { .addr = 0xa0 >> 1, .len = 1, .buf = &reg },
+                { .addr = 0xa0 >> 1, .flags = I2C_M_RD,
+                  .len = sizeof(eeprom), .buf = eeprom },
+        };
 
         ret = get_chip_info(state);
         if (mxl_fail(ret))
@@ -930,14 +935,11 @@ static int mxl111sf_init(struct dvb_usb_device *d)
         if (state->chip_rev > MXL111SF_V6)
                 mxl111sf_config_pin_mux_modes(state, PIN_MUX_TS_SPI_IN_MODE_1);
 
-        c.adapter = &d->i2c_adap;
-        c.addr = 0xa0 >> 1;
-
-        ret = tveeprom_read(&c, eeprom, sizeof(eeprom));
+        ret = i2c_transfer(&d->i2c_adap, msg, 2);
         if (mxl_fail(ret))
                 return 0;
-        tveeprom_hauppauge_analog(&c, &state->tv, (0x84 == eeprom[0xa0]) ?
-                        eeprom + 0xa0 : eeprom + 0x80);
+        tveeprom_hauppauge_analog(&state->tv, (0x84 == eeprom[0xa0]) ?
+                                  eeprom + 0xa0 : eeprom + 0x80);
 #if 0
         switch (state->tv.model) {
         case 117001:
diff --git a/drivers/media/usb/dvb-usb/cxusb.c b/drivers/media/usb/dvb-usb/cxusb.c
index 51620e02292f..99a3f3625944 100644
--- a/drivers/media/usb/dvb-usb/cxusb.c
+++ b/drivers/media/usb/dvb-usb/cxusb.c
@@ -458,8 +458,8 @@ static int cxusb_rc_query(struct dvb_usb_device *d)
         cxusb_ctrl_msg(d, CMD_GET_IR_CODE, NULL, 0, ircode, 4);
 
         if (ircode[2] || ircode[3])
-                rc_keydown(d->rc_dev, RC_TYPE_UNKNOWN,
-                           RC_SCANCODE_RC5(ircode[2], ircode[3]), 0);
+                rc_keydown(d->rc_dev, RC_TYPE_NEC,
+                           RC_SCANCODE_NEC(~ircode[2] & 0xff, ircode[3]), 0);
         return 0;
 }
 
@@ -473,8 +473,8 @@ static int cxusb_bluebird2_rc_query(struct dvb_usb_device *d)
                 return 0;
 
         if (ircode[1] || ircode[2])
-                rc_keydown(d->rc_dev, RC_TYPE_UNKNOWN,
-                           RC_SCANCODE_RC5(ircode[1], ircode[2]), 0);
+                rc_keydown(d->rc_dev, RC_TYPE_NEC,
+                           RC_SCANCODE_NEC(~ircode[1] & 0xff, ircode[2]), 0);
         return 0;
 }
 
@@ -1239,6 +1239,82 @@ static int cxusb_mygica_t230_frontend_attach(struct dvb_usb_adapter *adap)
         return 0;
 }
 
+static int cxusb_mygica_t230c_frontend_attach(struct dvb_usb_adapter *adap)
+{
+        struct dvb_usb_device *d = adap->dev;
+        struct cxusb_state *st = d->priv;
+        struct i2c_adapter *adapter;
+        struct i2c_client *client_demod;
+        struct i2c_client *client_tuner;
+        struct i2c_board_info info;
+        struct si2168_config si2168_config;
+        struct si2157_config si2157_config;
+
+        /* Select required USB configuration */
+        if (usb_set_interface(d->udev, 0, 0) < 0)
+                err("set interface failed");
+
+        /* Unblock all USB pipes */
+        usb_clear_halt(d->udev,
+                usb_sndbulkpipe(d->udev, d->props.generic_bulk_ctrl_endpoint));
+        usb_clear_halt(d->udev,
+                usb_rcvbulkpipe(d->udev, d->props.generic_bulk_ctrl_endpoint));
+        usb_clear_halt(d->udev,
+                usb_rcvbulkpipe(d->udev, d->props.adapter[0].fe[0].stream.endpoint));
+
+        /* attach frontend */
+        memset(&si2168_config, 0, sizeof(si2168_config));
+        si2168_config.i2c_adapter = &adapter;
+        si2168_config.fe = &adap->fe_adap[0].fe;
+        si2168_config.ts_mode = SI2168_TS_PARALLEL;
+        si2168_config.ts_clock_inv = 1;
+        memset(&info, 0, sizeof(struct i2c_board_info));
+        strlcpy(info.type, "si2168", I2C_NAME_SIZE);
+        info.addr = 0x64;
+        info.platform_data = &si2168_config;
+        request_module(info.type);
+        client_demod = i2c_new_device(&d->i2c_adap, &info);
+        if (client_demod == NULL || client_demod->dev.driver == NULL)
+                return -ENODEV;
+
+        if (!try_module_get(client_demod->dev.driver->owner)) {
+                i2c_unregister_device(client_demod);
+                return -ENODEV;
+        }
+
+        /* attach tuner */
+        memset(&si2157_config, 0, sizeof(si2157_config));
+        si2157_config.fe = adap->fe_adap[0].fe;
+        memset(&info, 0, sizeof(struct i2c_board_info));
+        strlcpy(info.type, "si2141", I2C_NAME_SIZE);
+        info.addr = 0x60;
+        info.platform_data = &si2157_config;
+        request_module("si2157");
+        client_tuner = i2c_new_device(adapter, &info);
+        if (client_tuner == NULL || client_tuner->dev.driver == NULL) {
+                module_put(client_demod->dev.driver->owner);
+                i2c_unregister_device(client_demod);
+                return -ENODEV;
+        }
+        if (!try_module_get(client_tuner->dev.driver->owner)) {
+                i2c_unregister_device(client_tuner);
+                module_put(client_demod->dev.driver->owner);
+                i2c_unregister_device(client_demod);
+                return -ENODEV;
+        }
+
+        st->i2c_client_demod = client_demod;
+        st->i2c_client_tuner = client_tuner;
+
+        /* hook fe: need to resync the slave fifo when signal locks. */
+        mutex_init(&st->stream_mutex);
+        st->last_lock = 0;
+        st->fe_read_status = adap->fe_adap[0].fe->ops.read_status;
+        adap->fe_adap[0].fe->ops.read_status = cxusb_read_status;
+
+        return 0;
+}
+
 /*
  * DViCO has shipped two devices with the same USB ID, but only one of them
  * needs a firmware download.  Check the device class details to see if they
@@ -1321,6 +1397,7 @@ static struct dvb_usb_device_properties cxusb_aver_a868r_properties;
 static struct dvb_usb_device_properties cxusb_d680_dmb_properties;
 static struct dvb_usb_device_properties cxusb_mygica_d689_properties;
 static struct dvb_usb_device_properties cxusb_mygica_t230_properties;
+static struct dvb_usb_device_properties cxusb_mygica_t230c_properties;
 
 static int cxusb_probe(struct usb_interface *intf,
                        const struct usb_device_id *id)
@@ -1353,6 +1430,8 @@ static int cxusb_probe(struct usb_interface *intf,
                                      THIS_MODULE, NULL, adapter_nr) ||
             0 == dvb_usb_device_init(intf, &cxusb_mygica_t230_properties,
                                      THIS_MODULE, NULL, adapter_nr) ||
+            0 == dvb_usb_device_init(intf, &cxusb_mygica_t230c_properties,
+                                     THIS_MODULE, NULL, adapter_nr) ||
             0)
                 return 0;
 
@@ -1404,6 +1483,7 @@ enum cxusb_table_index {
         CONEXANT_D680_DMB,
         MYGICA_D689,
         MYGICA_T230,
+        MYGICA_T230C,
         NR__cxusb_table_index
 };
 
@@ -1471,6 +1551,9 @@ static struct usb_device_id cxusb_table[NR__cxusb_table_index + 1] = {
         [MYGICA_T230] = {
                 USB_DEVICE(USB_VID_CONEXANT, USB_PID_MYGICA_T230)
         },
+        [MYGICA_T230C] = {
+                USB_DEVICE(USB_VID_CONEXANT, USB_PID_MYGICA_T230+1)
+        },
         {}              /* Terminating entry */
 };
 MODULE_DEVICE_TABLE (usb, cxusb_table);
@@ -1563,7 +1646,7 @@ static struct dvb_usb_device_properties cxusb_bluebird_lgh064f_properties = {
                 .rc_codes       = RC_MAP_DVICO_PORTABLE,
                 .module_name    = KBUILD_MODNAME,
                 .rc_query       = cxusb_rc_query,
-                .allowed_protos = RC_BIT_UNKNOWN,
+                .allowed_protos = RC_BIT_NEC,
         },
 
         .generic_bulk_ctrl_endpoint = 0x01,
@@ -1620,7 +1703,7 @@ static struct dvb_usb_device_properties cxusb_bluebird_dee1601_properties = {
                 .rc_codes       = RC_MAP_DVICO_MCE,
                 .module_name    = KBUILD_MODNAME,
                 .rc_query       = cxusb_rc_query,
-                .allowed_protos = RC_BIT_UNKNOWN,
+                .allowed_protos = RC_BIT_NEC,
         },
 
         .generic_bulk_ctrl_endpoint = 0x01,
@@ -1685,7 +1768,7 @@ static struct dvb_usb_device_properties cxusb_bluebird_lgz201_properties = {
                 .rc_codes       = RC_MAP_DVICO_PORTABLE,
                 .module_name    = KBUILD_MODNAME,
                 .rc_query       = cxusb_rc_query,
-                .allowed_protos = RC_BIT_UNKNOWN,
+                .allowed_protos = RC_BIT_NEC,
         },
 
         .generic_bulk_ctrl_endpoint = 0x01,
@@ -1741,7 +1824,7 @@ static struct dvb_usb_device_properties cxusb_bluebird_dtt7579_properties = {
                 .rc_codes       = RC_MAP_DVICO_PORTABLE,
                 .module_name    = KBUILD_MODNAME,
                 .rc_query       = cxusb_rc_query,
-                .allowed_protos = RC_BIT_UNKNOWN,
+                .allowed_protos = RC_BIT_NEC,
         },
 
         .generic_bulk_ctrl_endpoint = 0x01,
@@ -1796,7 +1879,7 @@ static struct dvb_usb_device_properties cxusb_bluebird_dualdig4_properties = {
                 .rc_codes       = RC_MAP_DVICO_MCE,
                 .module_name    = KBUILD_MODNAME,
                 .rc_query       = cxusb_bluebird2_rc_query,
-                .allowed_protos = RC_BIT_UNKNOWN,
+                .allowed_protos = RC_BIT_NEC,
         },
 
         .num_device_descs = 1,
@@ -1850,7 +1933,7 @@ static struct dvb_usb_device_properties cxusb_bluebird_nano2_properties = {
                 .rc_codes       = RC_MAP_DVICO_PORTABLE,
                 .module_name    = KBUILD_MODNAME,
                 .rc_query       = cxusb_bluebird2_rc_query,
-                .allowed_protos = RC_BIT_UNKNOWN,
+                .allowed_protos = RC_BIT_NEC,
         },
 
         .num_device_descs = 1,
@@ -1906,7 +1989,7 @@ static struct dvb_usb_device_properties cxusb_bluebird_nano2_needsfirmware_prope
                 .rc_codes       = RC_MAP_DVICO_PORTABLE,
                 .module_name    = KBUILD_MODNAME,
                 .rc_query       = cxusb_rc_query,
-                .allowed_protos = RC_BIT_UNKNOWN,
+                .allowed_protos = RC_BIT_NEC,
         },
 
         .num_device_descs = 1,
@@ -2005,7 +2088,7 @@ struct dvb_usb_device_properties cxusb_bluebird_dualdig4_rev2_properties = {
                 .rc_codes       = RC_MAP_DVICO_MCE,
                 .module_name    = KBUILD_MODNAME,
                 .rc_query       = cxusb_rc_query,
-                .allowed_protos = RC_BIT_UNKNOWN,
+                .allowed_protos = RC_BIT_NEC,
         },
 
         .num_device_descs = 1,
@@ -2165,7 +2248,7 @@ static struct dvb_usb_device_properties cxusb_mygica_t230_properties = {
 
         .rc.core = {
                 .rc_interval    = 100,
-                .rc_codes       = RC_MAP_D680_DMB,
+                .rc_codes       = RC_MAP_TOTAL_MEDIA_IN_HAND_02,
                 .module_name    = KBUILD_MODNAME,
                 .rc_query       = cxusb_d680_dmb_rc_query,
                 .allowed_protos = RC_BIT_UNKNOWN,
@@ -2181,6 +2264,60 @@ static struct dvb_usb_device_properties cxusb_mygica_t230_properties = {
         }
 };
 
+static struct dvb_usb_device_properties cxusb_mygica_t230c_properties = {
+        .caps = DVB_USB_IS_AN_I2C_ADAPTER,
+
+        .usb_ctrl         = CYPRESS_FX2,
+
+        .size_of_priv     = sizeof(struct cxusb_state),
+
+        .num_adapters = 1,
+        .adapter = {
+                {
+                .num_frontends = 1,
+                .fe = {{
+                        .streaming_ctrl   = cxusb_streaming_ctrl,
+                        .frontend_attach  = cxusb_mygica_t230c_frontend_attach,
+
+                        /* parameter for the MPEG2-data transfer */
+                        .stream = {
+                                .type = USB_BULK,
+                                .count = 5,
+                                .endpoint = 0x02,
+                                .u = {
+                                        .bulk = {
+                                                .buffersize = 8192,
+                                        }
+                                }
+                        },
+                } },
+                },
+        },
+
+        .power_ctrl       = cxusb_d680_dmb_power_ctrl,
+
+        .i2c_algo         = &cxusb_i2c_algo,
+
+        .generic_bulk_ctrl_endpoint = 0x01,
+
+        .rc.core = {
+                .rc_interval    = 100,
+                .rc_codes       = RC_MAP_TOTAL_MEDIA_IN_HAND_02,
+                .module_name    = KBUILD_MODNAME,
+                .rc_query       = cxusb_d680_dmb_rc_query,
+                .allowed_protos = RC_BIT_UNKNOWN,
+        },
+
+        .num_device_descs = 1,
+        .devices = {
+                {
+                        "Mygica T230C DVB-T/T2/C",
+                        { NULL },
+                        { &cxusb_table[MYGICA_T230C], NULL },
+                },
+        }
+};
+
 static struct usb_driver cxusb_driver = {
         .name           = "dvb_usb_cxusb",
         .probe          = cxusb_probe,
diff --git a/drivers/media/usb/dvb-usb/dib0700_core.c b/drivers/media/usb/dvb-usb/dib0700_core.c
index dd5edd3a17ee..08acdd32e412 100644
--- a/drivers/media/usb/dvb-usb/dib0700_core.c
+++ b/drivers/media/usb/dvb-usb/dib0700_core.c
@@ -809,6 +809,9 @@ int dib0700_rc_setup(struct dvb_usb_device *d, struct usb_interface *intf)
 
         /* Starting in firmware 1.20, the RC info is provided on a bulk pipe */
 
+        if (intf->altsetting[0].desc.bNumEndpoints < rc_ep + 1)
+                return -ENODEV;
+
         purb = usb_alloc_urb(0, GFP_KERNEL);
         if (purb == NULL)
                 return -ENOMEM;
diff --git a/drivers/media/usb/dvb-usb/dibusb-mc-common.c b/drivers/media/usb/dvb-usb/dibusb-mc-common.c
index c989cac9343d..0c2bc97436d5 100644
--- a/drivers/media/usb/dvb-usb/dibusb-mc-common.c
+++ b/drivers/media/usb/dvb-usb/dibusb-mc-common.c
@@ -11,6 +11,8 @@
 
 #include "dibusb.h"
 
+MODULE_LICENSE("GPL");
+
 /* 3000MC/P stuff */
 // Config Adjacent channels  Perf -cal22
 static struct dibx000_agc_config dib3000p_mt2060_agc_config = {
diff --git a/drivers/media/usb/dvb-usb/digitv.c b/drivers/media/usb/dvb-usb/digitv.c
index 4284f6984dc1..475a3c0cdee7 100644
--- a/drivers/media/usb/dvb-usb/digitv.c
+++ b/drivers/media/usb/dvb-usb/digitv.c
@@ -33,6 +33,9 @@ static int digitv_ctrl_msg(struct dvb_usb_device *d,
 
         wo = (rbuf == NULL || rlen == 0); /* write-only */
 
+        if (wlen > 4 || rlen > 4)
+                return -EIO;
+
         memset(st->sndbuf, 0, 7);
         memset(st->rcvbuf, 0, 7);
 
diff --git a/drivers/media/usb/dvb-usb/dw2102.c b/drivers/media/usb/dvb-usb/dw2102.c
index 4f42d57f81d9..6e654e5026dd 100644
--- a/drivers/media/usb/dvb-usb/dw2102.c
+++ b/drivers/media/usb/dvb-usb/dw2102.c
@@ -204,6 +204,20 @@ static int dw2102_serit_i2c_transfer(struct i2c_adapter *adap,
 
         switch (num) {
         case 2:
+                if (msg[0].len != 1) {
+                        warn("i2c rd: len=%d is not 1!\n",
+                             msg[0].len);
+                        num = -EOPNOTSUPP;
+                        break;
+                }
+
+                if (2 + msg[1].len > sizeof(buf6)) {
+                        warn("i2c rd: len=%d is too big!\n",
+                             msg[1].len);
+                        num = -EOPNOTSUPP;
+                        break;
+                }
+
                 /* read si2109 register by number */
                 buf6[0] = msg[0].addr << 1;
                 buf6[1] = msg[0].len;
@@ -219,6 +233,13 @@ static int dw2102_serit_i2c_transfer(struct i2c_adapter *adap,
         case 1:
                 switch (msg[0].addr) {
                 case 0x68:
+                        if (2 + msg[0].len > sizeof(buf6)) {
+                                warn("i2c wr: len=%d is too big!\n",
+                                     msg[0].len);
+                                num = -EOPNOTSUPP;
+                                break;
+                        }
+
                         /* write to si2109 register */
                         buf6[0] = msg[0].addr << 1;
                         buf6[1] = msg[0].len;
@@ -262,6 +283,13 @@ static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg ms
                 /* first write first register number */
                 u8 ibuf[MAX_XFER_SIZE], obuf[3];
 
+                if (2 + msg[0].len != sizeof(obuf)) {
+                        warn("i2c rd: len=%d is not 1!\n",
+                             msg[0].len);
+                        ret = -EOPNOTSUPP;
+                        goto unlock;
+                }
+
                 if (2 + msg[1].len > sizeof(ibuf)) {
                         warn("i2c rd: len=%d is too big!\n",
                              msg[1].len);
@@ -462,6 +490,12 @@ static int dw3101_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
                 /* first write first register number */
                 u8 ibuf[MAX_XFER_SIZE], obuf[3];
 
+                if (2 + msg[0].len != sizeof(obuf)) {
+                        warn("i2c rd: len=%d is not 1!\n",
+                             msg[0].len);
+                        ret = -EOPNOTSUPP;
+                        goto unlock;
+                }
                 if (2 + msg[1].len > sizeof(ibuf)) {
                         warn("i2c rd: len=%d is too big!\n",
                              msg[1].len);
@@ -696,6 +730,13 @@ static int su3000_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
                         msg[0].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;
+                                break;
+                        }
+
                         /* always i2c write*/
                         state->data[0] = 0x08;
                         state->data[1] = msg[0].addr;
@@ -711,6 +752,19 @@ static int su3000_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
                 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;
+                }
+
                 state->data[0] = 0x09;
                 state->data[1] = msg[0].len;
                 state->data[2] = msg[1].len;
diff --git a/drivers/media/usb/dvb-usb/ttusb2.c b/drivers/media/usb/dvb-usb/ttusb2.c
index ecc207fbaf3c..9e0d6a4166d2 100644
--- a/drivers/media/usb/dvb-usb/ttusb2.c
+++ b/drivers/media/usb/dvb-usb/ttusb2.c
@@ -78,6 +78,9 @@ static int ttusb2_msg(struct dvb_usb_device *d, u8 cmd,
         u8 *s, *r = NULL;
         int ret = 0;
 
+        if (4 + rlen > 64)
+                return -EIO;
+
         s = kzalloc(wlen+4, GFP_KERNEL);
         if (!s)
                 return -ENOMEM;
@@ -381,6 +384,22 @@ static int ttusb2_i2c_xfer(struct i2c_adapter *adap,struct i2c_msg msg[],int num
                 write_read = i+1 < num && (msg[i+1].flags & I2C_M_RD);
                 read = msg[i].flags & I2C_M_RD;
 
+                if (3 + msg[i].len > sizeof(obuf)) {
+                        err("i2c wr len=%d too high", msg[i].len);
+                        break;
+                }
+                if (write_read) {
+                        if (3 + msg[i+1].len > sizeof(ibuf)) {
+                                err("i2c rd len=%d too high", msg[i+1].len);
+                                break;
+                        }
+                } else if (read) {
+                        if (3 + msg[i].len > sizeof(ibuf)) {
+                                err("i2c rd len=%d too high", msg[i].len);
+                                break;
+                        }
+                }
+
                 obuf[0] = (msg[i].addr << 1) | (write_read | read);
                 if (read)
                         obuf[1] = 0;
diff --git a/drivers/media/usb/em28xx/Kconfig b/drivers/media/usb/em28xx/Kconfig
index aa131cf9989b..4cc029f18aa8 100644
--- a/drivers/media/usb/em28xx/Kconfig
+++ b/drivers/media/usb/em28xx/Kconfig
@@ -12,7 +12,7 @@ config VIDEO_EM28XX_V4L2
         select VIDEO_TVP5150 if MEDIA_SUBDRV_AUTOSELECT
         select VIDEO_MSP3400 if MEDIA_SUBDRV_AUTOSELECT
         select VIDEO_MT9V011 if MEDIA_SUBDRV_AUTOSELECT && MEDIA_CAMERA_SUPPORT
-
+        select VIDEO_OV2640 if MEDIA_SUBDRV_AUTOSELECT && MEDIA_CAMERA_SUPPORT
         ---help---
           This is a video4linux driver for Empia 28xx based TV cards.
 
@@ -39,6 +39,7 @@ config VIDEO_EM28XX_DVB
         depends on VIDEO_EM28XX && DVB_CORE
         select DVB_LGDT330X if MEDIA_SUBDRV_AUTOSELECT
         select DVB_LGDT3305 if MEDIA_SUBDRV_AUTOSELECT
+        select DVB_LGDT3306A if MEDIA_SUBDRV_AUTOSELECT
         select DVB_ZL10353 if MEDIA_SUBDRV_AUTOSELECT
         select DVB_TDA10023 if MEDIA_SUBDRV_AUTOSELECT
         select DVB_S921 if MEDIA_SUBDRV_AUTOSELECT
@@ -61,6 +62,10 @@ config VIDEO_EM28XX_DVB
         select MEDIA_TUNER_SI2157 if MEDIA_SUBDRV_AUTOSELECT
         select DVB_TC90522 if MEDIA_SUBDRV_AUTOSELECT
         select MEDIA_TUNER_QM1D1C0042 if MEDIA_SUBDRV_AUTOSELECT
+        select MEDIA_TUNER_SIMPLE if MEDIA_SUBDRV_AUTOSELECT
+        select MEDIA_TUNER_XC2028 if MEDIA_SUBDRV_AUTOSELECT
+        select MEDIA_TUNER_XC5000 if MEDIA_SUBDRV_AUTOSELECT
+        select MEDIA_TUNER_MT2060 if MEDIA_SUBDRV_AUTOSELECT
         ---help---
           This adds support for DVB cards based on the
           Empiatech em28xx chips.
diff --git a/drivers/media/usb/em28xx/em28xx-camera.c b/drivers/media/usb/em28xx/em28xx-camera.c
index 89c890ba7dd6..ae87dd3e671f 100644
--- a/drivers/media/usb/em28xx/em28xx-camera.c
+++ b/drivers/media/usb/em28xx/em28xx-camera.c
@@ -23,9 +23,7 @@
 
 #include <linux/i2c.h>
 #include <linux/usb.h>
-#include <media/soc_camera.h>
 #include <media/i2c/mt9v011.h>
-#include <media/v4l2-clk.h>
 #include <media/v4l2-common.h>
 
 /* Possible i2c addresses of Micron sensors */
@@ -43,13 +41,6 @@ static unsigned short omnivision_sensor_addrs[] = {
         I2C_CLIENT_END
 };
 
-static struct soc_camera_link camlink = {
-        .bus_id = 0,
-        .flags = 0,
-        .module_name = "em28xx",
-        .unbalanced_power = true,
-};
-
 /* FIXME: Should be replaced by a proper mt9m111 driver */
 static int em28xx_initialize_mt9m111(struct em28xx *dev)
 {
@@ -106,55 +97,35 @@ static int em28xx_probe_sensor_micron(struct em28xx *dev)
 {
         int ret, i;
         char *name;
-        u8 reg;
-        __be16 id_be;
         u16 id;
 
-        struct i2c_client client = dev->i2c_client[dev->def_i2c_bus];
+        struct i2c_client *client = &dev->i2c_client[dev->def_i2c_bus];
 
         dev->em28xx_sensor = EM28XX_NOSENSOR;
         for (i = 0; micron_sensor_addrs[i] != I2C_CLIENT_END; i++) {
-                client.addr = micron_sensor_addrs[i];
-                /* NOTE: i2c_smbus_read_word_data() doesn't work with BE data */
+                client->addr = micron_sensor_addrs[i];
                 /* Read chip ID from register 0x00 */
-                reg = 0x00;
-                ret = i2c_master_send(&client, &reg, 1);
+                ret = i2c_smbus_read_word_data(client, 0x00); /* assumes LE */
                 if (ret < 0) {
                         if (ret != -ENXIO)
                                 dev_err(&dev->intf->dev,
                                         "couldn't read from i2c device 0x%02x: error %i\n",
-                                       client.addr << 1, ret);
-                        continue;
-                }
-                ret = i2c_master_recv(&client, (u8 *)&id_be, 2);
-                if (ret < 0) {
-                        dev_err(&dev->intf->dev,
-                                "couldn't read from i2c device 0x%02x: error %i\n",
-                                client.addr << 1, ret);
+                                       client->addr << 1, ret);
                         continue;
                 }
-                id = be16_to_cpu(id_be);
+                id = swab16(ret); /* LE -> BE */
                 /* Read chip ID from register 0xff */
-                reg = 0xff;
-                ret = i2c_master_send(&client, &reg, 1);
+                ret = i2c_smbus_read_word_data(client, 0xff);
                 if (ret < 0) {
                         dev_err(&dev->intf->dev,
                                 "couldn't read from i2c device 0x%02x: error %i\n",
-                                client.addr << 1, ret);
-                        continue;
-                }
-                ret = i2c_master_recv(&client, (u8 *)&id_be, 2);
-                if (ret < 0) {
-                        dev_err(&dev->intf->dev,
-                                "couldn't read from i2c device 0x%02x: error %i\n",
-                                client.addr << 1, ret);
+                                client->addr << 1, ret);
                         continue;
                 }
                 /* Validate chip ID to be sure we have a Micron device */
-                if (id != be16_to_cpu(id_be))
+                if (id != swab16(ret))
                         continue;
                 /* Check chip ID */
-                id = be16_to_cpu(id_be);
                 switch (id) {
                 case 0x1222:
                         name = "MT9V012"; /* MI370 */ /* 640x480 */
@@ -197,7 +168,6 @@ static int em28xx_probe_sensor_micron(struct em28xx *dev)
                         dev_info(&dev->intf->dev,
                                  "sensor %s detected\n", name);
 
-                dev->i2c_client[dev->def_i2c_bus].addr = client.addr;
                 return 0;
         }
 
@@ -213,30 +183,30 @@ static int em28xx_probe_sensor_omnivision(struct em28xx *dev)
         char *name;
         u8 reg;
         u16 id;
-        struct i2c_client client = dev->i2c_client[dev->def_i2c_bus];
+        struct i2c_client *client = &dev->i2c_client[dev->def_i2c_bus];
 
         dev->em28xx_sensor = EM28XX_NOSENSOR;
         /* NOTE: these devices have the register auto incrementation disabled
          * by default, so we have to use single byte reads !              */
         for (i = 0; omnivision_sensor_addrs[i] != I2C_CLIENT_END; i++) {
-                client.addr = omnivision_sensor_addrs[i];
+                client->addr = omnivision_sensor_addrs[i];
                 /* Read manufacturer ID from registers 0x1c-0x1d (BE) */
                 reg = 0x1c;
-                ret = i2c_smbus_read_byte_data(&client, reg);
+                ret = i2c_smbus_read_byte_data(client, reg);
                 if (ret < 0) {
                         if (ret != -ENXIO)
                                 dev_err(&dev->intf->dev,
                                         "couldn't read from i2c device 0x%02x: error %i\n",
-                                        client.addr << 1, ret);
+                                        client->addr << 1, ret);
                         continue;
                 }
                 id = ret << 8;
                 reg = 0x1d;
-                ret = i2c_smbus_read_byte_data(&client, reg);
+                ret = i2c_smbus_read_byte_data(client, reg);
                 if (ret < 0) {
                         dev_err(&dev->intf->dev,
                                 "couldn't read from i2c device 0x%02x: error %i\n",
-                                client.addr << 1, ret);
+                                client->addr << 1, ret);
                         continue;
                 }
                 id += ret;
@@ -245,20 +215,20 @@ static int em28xx_probe_sensor_omnivision(struct em28xx *dev)
                         continue;
                 /* Read product ID from registers 0x0a-0x0b (BE) */
                 reg = 0x0a;
-                ret = i2c_smbus_read_byte_data(&client, reg);
+                ret = i2c_smbus_read_byte_data(client, reg);
                 if (ret < 0) {
                         dev_err(&dev->intf->dev,
                                 "couldn't read from i2c device 0x%02x: error %i\n",
-                                client.addr << 1, ret);
+                                client->addr << 1, ret);
                         continue;
                 }
                 id = ret << 8;
                 reg = 0x0b;
-                ret = i2c_smbus_read_byte_data(&client, reg);
+                ret = i2c_smbus_read_byte_data(client, reg);
                 if (ret < 0) {
                         dev_err(&dev->intf->dev,
                                 "couldn't read from i2c device 0x%02x: error %i\n",
-                                client.addr << 1, ret);
+                                client->addr << 1, ret);
                         continue;
                 }
                 id += ret;
@@ -309,7 +279,6 @@ static int em28xx_probe_sensor_omnivision(struct em28xx *dev)
                         dev_info(&dev->intf->dev,
                                  "sensor %s detected\n", name);
 
-                dev->i2c_client[dev->def_i2c_bus].addr = client.addr;
                 return 0;
         }
 
@@ -341,17 +310,9 @@ int em28xx_detect_sensor(struct em28xx *dev)
 
 int em28xx_init_camera(struct em28xx *dev)
 {
-        char clk_name[V4L2_CLK_NAME_SIZE];
         struct i2c_client *client = &dev->i2c_client[dev->def_i2c_bus];
         struct i2c_adapter *adap = &dev->i2c_adap[dev->def_i2c_bus];
         struct em28xx_v4l2 *v4l2 = dev->v4l2;
-        int ret = 0;
-
-        v4l2_clk_name_i2c(clk_name, sizeof(clk_name),
-                          i2c_adapter_id(adap), client->addr);
-        v4l2->clk = v4l2_clk_register_fixed(clk_name, -EINVAL);
-        if (IS_ERR(v4l2->clk))
-                return PTR_ERR(v4l2->clk);
 
         switch (dev->em28xx_sensor) {
         case EM28XX_MT9V011:
@@ -381,12 +342,9 @@ int em28xx_init_camera(struct em28xx *dev)
                 pdata.xtal = v4l2->sensor_xtal;
                 if (NULL ==
                     v4l2_i2c_new_subdev_board(&v4l2->v4l2_dev, adap,
-                                              &mt9v011_info, NULL)) {
-                        ret = -ENODEV;
-                        break;
-                }
-                /* probably means GRGB 16 bit bayer */
-                v4l2->vinmode = 0x0d;
+                                              &mt9v011_info, NULL))
+                        return -ENODEV;
+                v4l2->vinmode = EM28XX_VINMODE_RGB8_GRBG;
                 v4l2->vinctl = 0x00;
 
                 break;
@@ -397,8 +355,7 @@ int em28xx_init_camera(struct em28xx *dev)
 
                 em28xx_initialize_mt9m001(dev);
 
-                /* probably means BGGR 16 bit bayer */
-                v4l2->vinmode = 0x0c;
+                v4l2->vinmode = EM28XX_VINMODE_RGB8_BGGR;
                 v4l2->vinctl = 0x00;
 
                 break;
@@ -410,7 +367,7 @@ int em28xx_init_camera(struct em28xx *dev)
                 em28xx_write_reg(dev, EM28XX_R0F_XCLK, dev->board.xclk);
                 em28xx_initialize_mt9m111(dev);
 
-                v4l2->vinmode = 0x0a;
+                v4l2->vinmode = EM28XX_VINMODE_YUV422_UYVY;
                 v4l2->vinctl = 0x00;
 
                 break;
@@ -421,7 +378,6 @@ int em28xx_init_camera(struct em28xx *dev)
                         .type = "ov2640",
                         .flags = I2C_CLIENT_SCCB,
                         .addr = client->addr,
-                        .platform_data = &camlink,
                 };
                 struct v4l2_subdev_format format = {
                         .which = V4L2_SUBDEV_FORMAT_ACTIVE,
@@ -441,10 +397,8 @@ int em28xx_init_camera(struct em28xx *dev)
                 subdev =
                      v4l2_i2c_new_subdev_board(&v4l2->v4l2_dev, adap,
                                                &ov2640_info, NULL);
-                if (NULL == subdev) {
-                        ret = -ENODEV;
-                        break;
-                }
+                if (subdev == NULL)
+                        return -ENODEV;
 
                 format.format.code = MEDIA_BUS_FMT_YUYV8_2X8;
                 format.format.width = 640;
@@ -454,21 +408,16 @@ int em28xx_init_camera(struct em28xx *dev)
                 /* NOTE: for UXGA=1600x1200 switch to 12MHz */
                 dev->board.xclk = EM28XX_XCLK_FREQUENCY_24MHZ;
                 em28xx_write_reg(dev, EM28XX_R0F_XCLK, dev->board.xclk);
-                v4l2->vinmode = 0x08;
+                v4l2->vinmode = EM28XX_VINMODE_YUV422_YUYV;
                 v4l2->vinctl = 0x00;
 
                 break;
         }
         case EM28XX_NOSENSOR:
         default:
-                ret = -EINVAL;
+                return -EINVAL;
         }
 
-        if (ret < 0) {
-                v4l2_clk_unregister_fixed(v4l2->clk);
-                v4l2->clk = NULL;
-        }
-
-        return ret;
+        return 0;
 }
 EXPORT_SYMBOL_GPL(em28xx_init_camera);
diff --git a/drivers/media/usb/em28xx/em28xx-cards.c b/drivers/media/usb/em28xx/em28xx-cards.c
index 5f90d0899a45..a12b599a1fa2 100644
--- a/drivers/media/usb/em28xx/em28xx-cards.c
+++ b/drivers/media/usb/em28xx/em28xx-cards.c
@@ -2600,6 +2600,8 @@ struct usb_device_id em28xx_id_table[] = {
                         .driver_info = EM28178_BOARD_TERRATEC_T2_STICK_HD },
         { USB_DEVICE(0x3275, 0x0085),
                         .driver_info = EM28178_BOARD_PLEX_PX_BCUD },
+        { USB_DEVICE(0xeb1a, 0x5051), /* Ion Video 2 PC MKII / Startech svid2usb23 / Raygo R12-41373 */
+                        .driver_info = EM2860_BOARD_TVP5150_REFERENCE_DESIGN },
         { },
 };
 MODULE_DEVICE_TABLE(usb, em28xx_id_table);
@@ -2917,7 +2919,9 @@ static void em28xx_card_setup(struct em28xx *dev)
          * If sensor is not found, then it isn't a webcam.
          */
         if (dev->board.is_webcam) {
-                if (em28xx_detect_sensor(dev) < 0)
+                em28xx_detect_sensor(dev);
+                if (dev->em28xx_sensor == EM28XX_NOSENSOR)
+                        /* NOTE: error/unknown sensor/no sensor */
                         dev->board.is_webcam = 0;
         }
 
@@ -2974,8 +2978,7 @@ static void em28xx_card_setup(struct em28xx *dev)
 #endif
                 /* Call first TVeeprom */
 
-                dev->i2c_client[dev->def_i2c_bus].addr = 0xa0 >> 1;
-                tveeprom_hauppauge_analog(&dev->i2c_client[dev->def_i2c_bus], &tv, dev->eedata);
+                tveeprom_hauppauge_analog(&tv, dev->eedata);
 
                 dev->tuner_type = tv.tuner_type;
 
@@ -3666,9 +3669,11 @@ static int em28xx_usb_probe(struct usb_interface *interface,
                 try_bulk = usb_xfer_mode > 0;
         }
 
-        /* Disable V4L2 if the device doesn't have a decoder */
+        /* Disable V4L2 if the device doesn't have a decoder or image sensor */
         if (has_video &&
-            dev->board.decoder == EM28XX_NODECODER && !dev->board.is_webcam) {
+            dev->board.decoder == EM28XX_NODECODER &&
+            dev->em28xx_sensor == EM28XX_NOSENSOR) {
+
                 dev_err(&interface->dev,
                         "Currently, V4L2 is not supported on this model\n");
                 has_video = false;
diff --git a/drivers/media/usb/em28xx/em28xx-reg.h b/drivers/media/usb/em28xx/em28xx-reg.h
index afe7a66d7dc8..747525ca7ed5 100644
--- a/drivers/media/usb/em28xx/em28xx-reg.h
+++ b/drivers/media/usb/em28xx/em28xx-reg.h
@@ -93,6 +93,24 @@
 #define EM28XX_XCLK_FREQUENCY_24MHZ     0x0b
 
 #define EM28XX_R10_VINMODE      0x10
+          /* used by all non-camera devices: */
+#define   EM28XX_VINMODE_YUV422_CbYCrY  0x10
+          /* used by camera devices: */
+#define   EM28XX_VINMODE_YUV422_YUYV    0x08
+#define   EM28XX_VINMODE_YUV422_YVYU    0x09
+#define   EM28XX_VINMODE_YUV422_UYVY    0x0a
+#define   EM28XX_VINMODE_YUV422_VYUY    0x0b
+#define   EM28XX_VINMODE_RGB8_BGGR      0x0c
+#define   EM28XX_VINMODE_RGB8_GRBG      0x0d
+#define   EM28XX_VINMODE_RGB8_GBRG      0x0e
+#define   EM28XX_VINMODE_RGB8_RGGB      0x0f
+          /*
+           * apparently:
+           *   bit 0: swap component 1+2 with 3+4
+           *                 => e.g.: YUYV => YVYU, BGGR => GRBG
+           *   bit 1: swap component 1 with 2 and 3 with 4
+           *                 => e.g.: YUYV => UYVY, BGGR => GBRG
+           */
 
 #define EM28XX_R11_VINCTRL      0x11
 
diff --git a/drivers/media/usb/em28xx/em28xx-video.c b/drivers/media/usb/em28xx/em28xx-video.c
index 8d93100334ea..8d253a5df0a9 100644
--- a/drivers/media/usb/em28xx/em28xx-video.c
+++ b/drivers/media/usb/em28xx/em28xx-video.c
@@ -43,7 +43,6 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-event.h>
-#include <media/v4l2-clk.h>
 #include <media/drv-intf/msp3400.h>
 #include <media/tuner.h>
 
@@ -117,6 +116,11 @@ static struct em28xx_fmt format[] = {
                 .depth    = 16,
                 .reg      = EM28XX_OUTFMT_RGB_16_656,
         }, {
+                .name     = "8 bpp Bayer RGRG..GBGB",
+                .fourcc   = V4L2_PIX_FMT_SRGGB8,
+                .depth    = 8,
+                .reg      = EM28XX_OUTFMT_RGB_8_RGRG,
+        }, {
                 .name     = "8 bpp Bayer BGBG..GRGR",
                 .fourcc   = V4L2_PIX_FMT_SBGGR8,
                 .depth    = 8,
@@ -2140,11 +2144,6 @@ static int em28xx_v4l2_fini(struct em28xx *dev)
         v4l2_ctrl_handler_free(&v4l2->ctrl_handler);
         v4l2_device_unregister(&v4l2->v4l2_dev);
 
-        if (v4l2->clk) {
-                v4l2_clk_unregister_fixed(v4l2->clk);
-                v4l2->clk = NULL;
-        }
-
         kref_put(&v4l2->ref, em28xx_free_v4l2);
 
         mutex_unlock(&dev->lock);
@@ -2465,7 +2464,7 @@ static int em28xx_v4l2_init(struct em28xx *dev)
         /*
          * Default format, used for tvp5150 or saa711x output formats
          */
-        v4l2->vinmode = 0x10;
+        v4l2->vinmode = EM28XX_VINMODE_YUV422_CbYCrY;
         v4l2->vinctl  = EM28XX_VINCTRL_INTERLACED |
                         EM28XX_VINCTRL_CCIR656_ENABLE;
 
diff --git a/drivers/media/usb/em28xx/em28xx.h b/drivers/media/usb/em28xx/em28xx.h
index e9f379959fa5..e8d97d5ec161 100644
--- a/drivers/media/usb/em28xx/em28xx.h
+++ b/drivers/media/usb/em28xx/em28xx.h
@@ -510,7 +510,6 @@ struct em28xx_v4l2 {
 
         struct v4l2_device v4l2_dev;
         struct v4l2_ctrl_handler ctrl_handler;
-        struct v4l2_clk *clk;
 
         struct video_device vdev;
         struct video_device vbi_dev;
diff --git a/drivers/media/usb/go7007/go7007-v4l2.c b/drivers/media/usb/go7007/go7007-v4l2.c
index 4eaba0c24629..ed5ec9773969 100644
--- a/drivers/media/usb/go7007/go7007-v4l2.c
+++ b/drivers/media/usb/go7007/go7007-v4l2.c
@@ -792,14 +792,13 @@ static int vidioc_subscribe_event(struct v4l2_fh *fh,
 {
 
         switch (sub->type) {
-        case V4L2_EVENT_CTRL:
-                return v4l2_ctrl_subscribe_event(fh, sub);
         case V4L2_EVENT_MOTION_DET:
                 /* Allow for up to 30 events (1 second for NTSC) to be
                  * stored. */
                 return v4l2_event_subscribe(fh, sub, 30, NULL);
+        default:
+                return v4l2_ctrl_subscribe_event(fh, sub);
         }
-        return -EINVAL;
 }
 
 
diff --git a/drivers/media/usb/gspca/konica.c b/drivers/media/usb/gspca/konica.c
index 71f273377f83..31b2117e8f1d 100644
--- a/drivers/media/usb/gspca/konica.c
+++ b/drivers/media/usb/gspca/konica.c
@@ -184,6 +184,9 @@ static int sd_start(struct gspca_dev *gspca_dev)
                 return -EIO;
         }
 
+        if (alt->desc.bNumEndpoints < 2)
+                return -ENODEV;
+
         packet_size = le16_to_cpu(alt->endpoint[0].desc.wMaxPacketSize);
 
         n = gspca_dev->cam.cam_mode[gspca_dev->curr_mode].priv;
diff --git a/drivers/media/usb/pulse8-cec/Kconfig b/drivers/media/usb/pulse8-cec/Kconfig
index 6ffc407de62f..8937f3986a01 100644
--- a/drivers/media/usb/pulse8-cec/Kconfig
+++ b/drivers/media/usb/pulse8-cec/Kconfig
@@ -1,6 +1,6 @@
 config USB_PULSE8_CEC
         tristate "Pulse Eight HDMI CEC"
-        depends on USB_ACM && MEDIA_CEC_SUPPORT
+        depends on USB_ACM && CEC_CORE
         select SERIO
         select SERIO_SERPORT
         ---help---
diff --git a/drivers/media/usb/pulse8-cec/pulse8-cec.c b/drivers/media/usb/pulse8-cec/pulse8-cec.c
index 7c18daeb0ade..1dfc2de1fe77 100644
--- a/drivers/media/usb/pulse8-cec/pulse8-cec.c
+++ b/drivers/media/usb/pulse8-cec/pulse8-cec.c
@@ -461,7 +461,7 @@ static int pulse8_apply_persistent_config(struct pulse8 *pulse8,
 
 static int pulse8_cec_adap_enable(struct cec_adapter *adap, bool enable)
 {
-        struct pulse8 *pulse8 = adap->priv;
+        struct pulse8 *pulse8 = cec_get_drvdata(adap);
         u8 cmd[16];
         int err;
 
@@ -474,7 +474,7 @@ static int pulse8_cec_adap_enable(struct cec_adapter *adap, bool enable)
 
 static int pulse8_cec_adap_log_addr(struct cec_adapter *adap, u8 log_addr)
 {
-        struct pulse8 *pulse8 = adap->priv;
+        struct pulse8 *pulse8 = cec_get_drvdata(adap);
         u16 mask = 0;
         u16 pa = adap->phys_addr;
         u8 cmd[16];
@@ -594,7 +594,7 @@ unlock:
 static int pulse8_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
                                     u32 signal_free_time, struct cec_msg *msg)
 {
-        struct pulse8 *pulse8 = adap->priv;
+        struct pulse8 *pulse8 = cec_get_drvdata(adap);
         u8 cmd[2];
         unsigned int i;
         int err;
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-eeprom.c b/drivers/media/usb/pvrusb2/pvrusb2-eeprom.c
index 4af2fb5c85d5..8b643d511a0b 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-eeprom.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-eeprom.c
@@ -118,15 +118,10 @@ int pvr2_eeprom_analyze(struct pvr2_hdw *hdw)
         memset(&tvdata,0,sizeof(tvdata));
 
         eeprom = pvr2_eeprom_fetch(hdw);
-        if (!eeprom) return -EINVAL;
-
-        {
-                struct i2c_client fake_client;
-                /* Newer version expects a useless client interface */
-                fake_client.addr = hdw->eeprom_addr;
-                fake_client.adapter = &hdw->i2c_adap;
-                tveeprom_hauppauge_analog(&fake_client,&tvdata,eeprom);
-        }
+        if (!eeprom)
+                return -EINVAL;
+
+        tveeprom_hauppauge_analog(&tvdata, eeprom);
 
         trace_eeprom("eeprom assumed v4l tveeprom module");
         trace_eeprom("eeprom direct call results:");
diff --git a/drivers/media/usb/rainshadow-cec/Kconfig b/drivers/media/usb/rainshadow-cec/Kconfig
new file mode 100644
index 000000000000..3eb86607efb8
--- /dev/null
+++ b/drivers/media/usb/rainshadow-cec/Kconfig
@@ -0,0 +1,10 @@
+config USB_RAINSHADOW_CEC
+        tristate "RainShadow Tech HDMI CEC"
+        depends on USB_ACM && CEC_CORE
+        select SERIO
+        select SERIO_SERPORT
+        ---help---
+          This is a cec driver for the RainShadow Tech HDMI CEC device.
+
+          To compile this driver as a module, choose M here: the
+          module will be called rainshadow-cec.
diff --git a/drivers/media/usb/rainshadow-cec/Makefile b/drivers/media/usb/rainshadow-cec/Makefile
new file mode 100644
index 000000000000..a79fbc77e1f7
--- /dev/null
+++ b/drivers/media/usb/rainshadow-cec/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_USB_RAINSHADOW_CEC) += rainshadow-cec.o
diff --git a/drivers/media/usb/rainshadow-cec/rainshadow-cec.c b/drivers/media/usb/rainshadow-cec/rainshadow-cec.c
new file mode 100644
index 000000000000..541ca543f71f
--- /dev/null
+++ b/drivers/media/usb/rainshadow-cec/rainshadow-cec.c
@@ -0,0 +1,388 @@
+/*
+ * RainShadow Tech HDMI CEC driver
+ *
+ * Copyright 2016 Hans Verkuil <hverkuil@xs4all.nl
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version of 2 of the License, or (at your
+ * option) any later version. See the file COPYING in the main directory of
+ * this archive for more details.
+ */
+
+/*
+ * Notes:
+ *
+ * The higher level protocols are currently disabled. This can be added
+ * later, similar to how this is done for the Pulse Eight CEC driver.
+ *
+ * Documentation of the protocol is available here:
+ *
+ * http://rainshadowtech.com/doc/HDMICECtoUSBandRS232v2.0.pdf
+ */
+
+#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/serio.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/time.h>
+#include <linux/workqueue.h>
+
+#include <media/cec.h>
+
+MODULE_AUTHOR("Hans Verkuil <hverkuil@xs4all.nl>");
+MODULE_DESCRIPTION("RainShadow Tech HDMI CEC driver");
+MODULE_LICENSE("GPL");
+
+#define DATA_SIZE 256
+
+struct rain {
+        struct device *dev;
+        struct serio *serio;
+        struct cec_adapter *adap;
+        struct completion cmd_done;
+        struct work_struct work;
+
+        /* Low-level ringbuffer, collecting incoming characters */
+        char buf[DATA_SIZE];
+        unsigned int buf_rd_idx;
+        unsigned int buf_wr_idx;
+        unsigned int buf_len;
+        spinlock_t buf_lock;
+
+        /* command buffer */
+        char cmd[DATA_SIZE];
+        unsigned int cmd_idx;
+        bool cmd_started;
+
+        /* reply to a command, only used to store the firmware version */
+        char cmd_reply[DATA_SIZE];
+
+        struct mutex write_lock;
+};
+
+static void rain_process_msg(struct rain *rain)
+{
+        struct cec_msg msg = {};
+        const char *cmd = rain->cmd + 3;
+        int stat = -1;
+
+        for (; *cmd; cmd++) {
+                if (!isxdigit(*cmd))
+                        continue;
+                if (isxdigit(cmd[0]) && isxdigit(cmd[1])) {
+                        if (msg.len == CEC_MAX_MSG_SIZE)
+                                break;
+                        if (hex2bin(msg.msg + msg.len, cmd, 1))
+                                continue;
+                        msg.len++;
+                        cmd++;
+                        continue;
+                }
+                if (!cmd[1])
+                        stat = hex_to_bin(cmd[0]);
+                break;
+        }
+
+        if (rain->cmd[0] == 'R') {
+                if (stat == 1 || stat == 2)
+                        cec_received_msg(rain->adap, &msg);
+                return;
+        }
+
+        switch (stat) {
+        case 1:
+                cec_transmit_done(rain->adap, CEC_TX_STATUS_OK,
+                                  0, 0, 0, 0);
+                break;
+        case 2:
+                cec_transmit_done(rain->adap, CEC_TX_STATUS_NACK,
+                                  0, 1, 0, 0);
+                break;
+        default:
+                cec_transmit_done(rain->adap, CEC_TX_STATUS_LOW_DRIVE,
+                                  0, 0, 0, 1);
+                break;
+        }
+}
+
+static void rain_irq_work_handler(struct work_struct *work)
+{
+        struct rain *rain =
+                container_of(work, struct rain, work);
+
+        while (true) {
+                unsigned long flags;
+                bool exit_loop;
+                char data;
+
+                spin_lock_irqsave(&rain->buf_lock, flags);
+                exit_loop = rain->buf_len == 0;
+                if (rain->buf_len) {
+                        data = rain->buf[rain->buf_rd_idx];
+                        rain->buf_len--;
+                        rain->buf_rd_idx = (rain->buf_rd_idx + 1) & 0xff;
+                }
+                spin_unlock_irqrestore(&rain->buf_lock, flags);
+
+                if (exit_loop)
+                        break;
+
+                if (!rain->cmd_started && data != '?')
+                        continue;
+
+                switch (data) {
+                case '\r':
+                        rain->cmd[rain->cmd_idx] = '\0';
+                        dev_dbg(rain->dev, "received: %s\n", rain->cmd);
+                        if (!memcmp(rain->cmd, "REC", 3) ||
+                            !memcmp(rain->cmd, "STA", 3)) {
+                                rain_process_msg(rain);
+                        } else {
+                                strcpy(rain->cmd_reply, rain->cmd);
+                                complete(&rain->cmd_done);
+                        }
+                        rain->cmd_idx = 0;
+                        rain->cmd_started = false;
+                        break;
+
+                case '\n':
+                        rain->cmd_idx = 0;
+                        rain->cmd_started = false;
+                        break;
+
+                case '?':
+                        rain->cmd_idx = 0;
+                        rain->cmd_started = true;
+                        break;
+
+                default:
+                        if (rain->cmd_idx >= DATA_SIZE - 1) {
+                                dev_dbg(rain->dev,
+                                        "throwing away %d bytes of garbage\n", rain->cmd_idx);
+                                rain->cmd_idx = 0;
+                        }
+                        rain->cmd[rain->cmd_idx++] = data;
+                        break;
+                }
+        }
+}
+
+static irqreturn_t rain_interrupt(struct serio *serio, unsigned char data,
+                                    unsigned int flags)
+{
+        struct rain *rain = serio_get_drvdata(serio);
+
+        if (rain->buf_len == DATA_SIZE) {
+                dev_warn_once(rain->dev, "buffer overflow\n");
+                return IRQ_HANDLED;
+        }
+        spin_lock(&rain->buf_lock);
+        rain->buf_len++;
+        rain->buf[rain->buf_wr_idx] = data;
+        rain->buf_wr_idx = (rain->buf_wr_idx + 1) & 0xff;
+        spin_unlock(&rain->buf_lock);
+        schedule_work(&rain->work);
+        return IRQ_HANDLED;
+}
+
+static void rain_disconnect(struct serio *serio)
+{
+        struct rain *rain = serio_get_drvdata(serio);
+
+        cancel_work_sync(&rain->work);
+        cec_unregister_adapter(rain->adap);
+        dev_info(&serio->dev, "disconnected\n");
+        serio_close(serio);
+        serio_set_drvdata(serio, NULL);
+        kfree(rain);
+}
+
+static int rain_send(struct rain *rain, const char *command)
+{
+        int err = serio_write(rain->serio, '!');
+
+        dev_dbg(rain->dev, "send: %s\n", command);
+        while (!err && *command)
+                err = serio_write(rain->serio, *command++);
+        if (!err)
+                err = serio_write(rain->serio, '~');
+
+        return err;
+}
+
+static int rain_send_and_wait(struct rain *rain,
+                              const char *cmd, const char *reply)
+{
+        int err;
+
+        init_completion(&rain->cmd_done);
+
+        mutex_lock(&rain->write_lock);
+        err = rain_send(rain, cmd);
+        if (err)
+                goto err;
+
+        if (!wait_for_completion_timeout(&rain->cmd_done, HZ)) {
+                err = -ETIMEDOUT;
+                goto err;
+        }
+        if (reply && strncmp(rain->cmd_reply, reply, strlen(reply))) {
+                dev_dbg(rain->dev,
+                         "transmit of '%s': received '%s' instead of '%s'\n",
+                         cmd, rain->cmd_reply, reply);
+                err = -EIO;
+        }
+err:
+        mutex_unlock(&rain->write_lock);
+        return err;
+}
+
+static int rain_setup(struct rain *rain, struct serio *serio,
+                        struct cec_log_addrs *log_addrs, u16 *pa)
+{
+        int err;
+
+        err = rain_send_and_wait(rain, "R", "REV");
+        if (err)
+                return err;
+        dev_info(rain->dev, "Firmware version %s\n", rain->cmd_reply + 4);
+
+        err = rain_send_and_wait(rain, "Q 1", "QTY");
+        if (err)
+                return err;
+        err = rain_send_and_wait(rain, "c0000", "CFG");
+        if (err)
+                return err;
+        return rain_send_and_wait(rain, "A F 0000", "ADR");
+}
+
+static int rain_cec_adap_enable(struct cec_adapter *adap, bool enable)
+{
+        return 0;
+}
+
+static int rain_cec_adap_log_addr(struct cec_adapter *adap, u8 log_addr)
+{
+        struct rain *rain = cec_get_drvdata(adap);
+        u8 cmd[16];
+
+        if (log_addr == CEC_LOG_ADDR_INVALID)
+                log_addr = CEC_LOG_ADDR_UNREGISTERED;
+        snprintf(cmd, sizeof(cmd), "A %x", log_addr);
+        return rain_send_and_wait(rain, cmd, "ADR");
+}
+
+static int rain_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
+                                    u32 signal_free_time, struct cec_msg *msg)
+{
+        struct rain *rain = cec_get_drvdata(adap);
+        char cmd[2 * CEC_MAX_MSG_SIZE + 16];
+        unsigned int i;
+        int err;
+
+        if (msg->len == 1) {
+                snprintf(cmd, sizeof(cmd), "x%x", cec_msg_destination(msg));
+        } else {
+                char hex[3];
+
+                snprintf(cmd, sizeof(cmd), "x%x %02x ",
+                         cec_msg_destination(msg), msg->msg[1]);
+                for (i = 2; i < msg->len; i++) {
+                        snprintf(hex, sizeof(hex), "%02x", msg->msg[i]);
+                        strncat(cmd, hex, sizeof(cmd));
+                }
+        }
+        mutex_lock(&rain->write_lock);
+        err = rain_send(rain, cmd);
+        mutex_unlock(&rain->write_lock);
+        return err;
+}
+
+static const struct cec_adap_ops rain_cec_adap_ops = {
+        .adap_enable = rain_cec_adap_enable,
+        .adap_log_addr = rain_cec_adap_log_addr,
+        .adap_transmit = rain_cec_adap_transmit,
+};
+
+static int rain_connect(struct serio *serio, struct serio_driver *drv)
+{
+        u32 caps = CEC_CAP_TRANSMIT | CEC_CAP_LOG_ADDRS | CEC_CAP_PHYS_ADDR |
+                CEC_CAP_PASSTHROUGH | CEC_CAP_RC | CEC_CAP_MONITOR_ALL;
+        struct rain *rain;
+        int err = -ENOMEM;
+        struct cec_log_addrs log_addrs = {};
+        u16 pa = CEC_PHYS_ADDR_INVALID;
+
+        rain = kzalloc(sizeof(*rain), GFP_KERNEL);
+
+        if (!rain)
+                return -ENOMEM;
+
+        rain->serio = serio;
+        rain->adap = cec_allocate_adapter(&rain_cec_adap_ops, rain,
+                "HDMI CEC", caps, 1);
+        err = PTR_ERR_OR_ZERO(rain->adap);
+        if (err < 0)
+                goto free_device;
+
+        rain->dev = &serio->dev;
+        serio_set_drvdata(serio, rain);
+        INIT_WORK(&rain->work, rain_irq_work_handler);
+        mutex_init(&rain->write_lock);
+
+        err = serio_open(serio, drv);
+        if (err)
+                goto delete_adap;
+
+        err = rain_setup(rain, serio, &log_addrs, &pa);
+        if (err)
+                goto close_serio;
+
+        err = cec_register_adapter(rain->adap, &serio->dev);
+        if (err < 0)
+                goto close_serio;
+
+        rain->dev = &rain->adap->devnode.dev;
+        return 0;
+
+close_serio:
+        serio_close(serio);
+delete_adap:
+        cec_delete_adapter(rain->adap);
+        serio_set_drvdata(serio, NULL);
+free_device:
+        kfree(rain);
+        return err;
+}
+
+static struct serio_device_id rain_serio_ids[] = {
+        {
+                .type   = SERIO_RS232,
+                .proto  = SERIO_RAINSHADOW_CEC,
+                .id     = SERIO_ANY,
+                .extra  = SERIO_ANY,
+        },
+        { 0 }
+};
+
+MODULE_DEVICE_TABLE(serio, rain_serio_ids);
+
+static struct serio_driver rain_drv = {
+        .driver         = {
+                .name   = "rainshadow-cec",
+        },
+        .description    = "RainShadow Tech HDMI CEC driver",
+        .id_table       = rain_serio_ids,
+        .interrupt      = rain_interrupt,
+        .connect        = rain_connect,
+        .disconnect     = rain_disconnect,
+};
+
+module_serio_driver(rain_drv);
diff --git a/drivers/media/usb/stk1160/Kconfig b/drivers/media/usb/stk1160/Kconfig
index 22dff4f3b921..425ed00e2599 100644
--- a/drivers/media/usb/stk1160/Kconfig
+++ b/drivers/media/usb/stk1160/Kconfig
@@ -6,7 +6,11 @@ config VIDEO_STK1160_COMMON
           This is a video4linux driver for STK1160 based video capture devices.
 
           To compile this driver as a module, choose M here: the
-          module will be called stk1160
+          module will be called stk1160.
+
+          This driver only provides support for video capture. For audio
+          capture, you need to select the snd-usb-audio driver (i.e.
+          CONFIG_SND_USB_AUDIO).
 
 config VIDEO_STK1160
         tristate
diff --git a/drivers/media/usb/tm6000/tm6000-video.c b/drivers/media/usb/tm6000/tm6000-video.c
index c4fdc1fa32ef..7e960d0a5b92 100644
--- a/drivers/media/usb/tm6000/tm6000-video.c
+++ b/drivers/media/usb/tm6000/tm6000-video.c
@@ -631,7 +631,7 @@ static int tm6000_prepare_isoc(struct tm6000_core *dev)
                 urb = usb_alloc_urb(max_packets, GFP_KERNEL);
                 if (!urb) {
                         tm6000_uninit_isoc(dev);
-                        usb_free_urb(urb);
+                        tm6000_free_urb_buffers(dev);
                         return -ENOMEM;
                 }
                 dev->isoc_ctl.urb[i] = urb;
diff --git a/drivers/media/usb/usbvision/usbvision-video.c b/drivers/media/usb/usbvision/usbvision-video.c
index f5c635a67d74..f9c3325aa4d4 100644
--- a/drivers/media/usb/usbvision/usbvision-video.c
+++ b/drivers/media/usb/usbvision/usbvision-video.c
@@ -1501,7 +1501,14 @@ static int usbvision_probe(struct usb_interface *intf,
         }
 
         for (i = 0; i < usbvision->num_alt; i++) {
-                u16 tmp = le16_to_cpu(uif->altsetting[i].endpoint[1].desc.
+                u16 tmp;
+
+                if (uif->altsetting[i].desc.bNumEndpoints < 2) {
+                        ret = -ENODEV;
+                        goto err_pkt;
+                }
+
+                tmp = le16_to_cpu(uif->altsetting[i].endpoint[1].desc.
                                       wMaxPacketSize);
                 usbvision->alt_max_pkt_size[i] =
                         (tmp & 0x07ff) * (((tmp & 0x1800) >> 11) + 1);
diff --git a/drivers/media/usb/uvc/uvc_driver.c b/drivers/media/usb/uvc/uvc_driver.c
index 04bf35063c4c..46d6be0bb316 100644
--- a/drivers/media/usb/uvc/uvc_driver.c
+++ b/drivers/media/usb/uvc/uvc_driver.c
@@ -188,6 +188,21 @@ static struct uvc_format_desc uvc_fmts[] = {
                 .guid           = UVC_GUID_FORMAT_GR16,
                 .fcc            = V4L2_PIX_FMT_SGRBG16,
         },
+        {
+                .name           = "Depth data 16-bit (Z16)",
+                .guid           = UVC_GUID_FORMAT_INVZ,
+                .fcc            = V4L2_PIX_FMT_Z16,
+        },
+        {
+                .name           = "Greyscale 10-bit (Y10 )",
+                .guid           = UVC_GUID_FORMAT_INVI,
+                .fcc            = V4L2_PIX_FMT_Y10,
+        },
+        {
+                .name           = "IR:Depth 26-bit (INZI)",
+                .guid           = UVC_GUID_FORMAT_INZI,
+                .fcc            = V4L2_PIX_FMT_INZI,
+        },
 };
 
 /* ------------------------------------------------------------------------
diff --git a/drivers/media/usb/uvc/uvc_video.c b/drivers/media/usb/uvc/uvc_video.c
index 07a6c833ef7b..47d93a938dde 100644
--- a/drivers/media/usb/uvc/uvc_video.c
+++ b/drivers/media/usb/uvc/uvc_video.c
@@ -818,7 +818,7 @@ static void uvc_video_stats_decode(struct uvc_streaming *stream,
 
         /* Update the packets counters. */
         stream->stats.frame.nb_packets++;
-        if (len > header_size)
+        if (len <= header_size)
                 stream->stats.frame.nb_empty++;
 
         if (data[1] & UVC_STREAM_ERR)
@@ -868,14 +868,8 @@ size_t uvc_video_stats_dump(struct uvc_streaming *stream, char *buf,
         struct timespec ts;
         size_t count = 0;
 
-        ts.tv_sec = stream->stats.stream.stop_ts.tv_sec
-                  - stream->stats.stream.start_ts.tv_sec;
-        ts.tv_nsec = stream->stats.stream.stop_ts.tv_nsec
-                   - stream->stats.stream.start_ts.tv_nsec;
-        if (ts.tv_nsec < 0) {
-                ts.tv_sec--;
-                ts.tv_nsec += 1000000000;
-        }
+        ts = timespec_sub(stream->stats.stream.stop_ts,
+                          stream->stats.stream.start_ts);
 
         /* Compute the SCR.SOF frequency estimate. At the nominal 1kHz SOF
          * frequency this will not overflow before more than 1h.
diff --git a/drivers/media/usb/uvc/uvcvideo.h b/drivers/media/usb/uvc/uvcvideo.h
index 4205e7a423f0..15e415e32c7f 100644
--- a/drivers/media/usb/uvc/uvcvideo.h
+++ b/drivers/media/usb/uvc/uvcvideo.h
@@ -143,6 +143,15 @@
 #define UVC_GUID_FORMAT_RW10 \
         { 'R',  'W',  '1',  '0', 0x00, 0x00, 0x10, 0x00, \
          0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71}
+#define UVC_GUID_FORMAT_INVZ \
+        { 'I',  'N',  'V',  'Z', 0x90, 0x2d, 0x58, 0x4a, \
+         0x92, 0x0b, 0x77, 0x3f, 0x1f, 0x2c, 0x55, 0x6b}
+#define UVC_GUID_FORMAT_INZI \
+        { 'I',  'N',  'Z',  'I', 0x66, 0x1a, 0x42, 0xa2, \
+         0x90, 0x65, 0xd0, 0x18, 0x14, 0xa8, 0xef, 0x8a}
+#define UVC_GUID_FORMAT_INVI \
+        { 'I',  'N',  'V',  'I', 0xdb, 0x57, 0x49, 0x5e, \
+         0x8e, 0x3f, 0xf4, 0x79, 0x53, 0x2b, 0x94, 0x6f}
 
 /* ------------------------------------------------------------------------
  * Driver specific constants.
diff --git a/drivers/media/usb/zr364xx/zr364xx.c b/drivers/media/usb/zr364xx/zr364xx.c
index f2d6fc03dda0..efdcd5bd6a4c 100644
--- a/drivers/media/usb/zr364xx/zr364xx.c
+++ b/drivers/media/usb/zr364xx/zr364xx.c
@@ -600,6 +600,14 @@ static int zr364xx_read_video_callback(struct zr364xx_camera *cam,
         ptr = pdest = frm->lpvbits;
 
         if (frm->ulState == ZR364XX_READ_IDLE) {
+                if (purb->actual_length < 128) {
+                        /* header incomplete */
+                        dev_info(&cam->udev->dev,
+                                 "%s: buffer (%d bytes) too small to hold jpeg header. Discarding.\n",
+                                 __func__, purb->actual_length);
+                        return -EINVAL;
+                }
+
                 frm->ulState = ZR364XX_READ_FRAME;
                 frm->cur_size = 0;
 
diff --git a/drivers/media/v4l2-core/v4l2-compat-ioctl32.c b/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
index eac9565dc3d8..6f52970f8b54 100644
--- a/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
+++ b/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
@@ -161,6 +161,20 @@ static inline int put_v4l2_sdr_format(struct v4l2_sdr_format *kp, struct v4l2_sd
         return 0;
 }
 
+static inline int get_v4l2_meta_format(struct v4l2_meta_format *kp, struct v4l2_meta_format __user *up)
+{
+        if (copy_from_user(kp, up, sizeof(struct v4l2_meta_format)))
+                return -EFAULT;
+        return 0;
+}
+
+static inline int put_v4l2_meta_format(struct v4l2_meta_format *kp, struct v4l2_meta_format __user *up)
+{
+        if (copy_to_user(up, kp, sizeof(struct v4l2_meta_format)))
+                return -EFAULT;
+        return 0;
+}
+
 struct v4l2_format32 {
         __u32   type;   /* enum v4l2_buf_type */
         union {
@@ -170,6 +184,7 @@ struct v4l2_format32 {
                 struct v4l2_vbi_format  vbi;
                 struct v4l2_sliced_vbi_format   sliced;
                 struct v4l2_sdr_format  sdr;
+                struct v4l2_meta_format meta;
                 __u8    raw_data[200];        /* user-defined */
         } fmt;
 };
@@ -216,6 +231,8 @@ static int __get_v4l2_format32(struct v4l2_format *kp, struct v4l2_format32 __us
         case V4L2_BUF_TYPE_SDR_CAPTURE:
         case V4L2_BUF_TYPE_SDR_OUTPUT:
                 return get_v4l2_sdr_format(&kp->fmt.sdr, &up->fmt.sdr);
+        case V4L2_BUF_TYPE_META_CAPTURE:
+                return get_v4l2_meta_format(&kp->fmt.meta, &up->fmt.meta);
         default:
                 pr_info("compat_ioctl32: unexpected VIDIOC_FMT type %d\n",
                                                                 kp->type);
@@ -263,6 +280,8 @@ static int __put_v4l2_format32(struct v4l2_format *kp, struct v4l2_format32 __us
         case V4L2_BUF_TYPE_SDR_CAPTURE:
         case V4L2_BUF_TYPE_SDR_OUTPUT:
                 return put_v4l2_sdr_format(&kp->fmt.sdr, &up->fmt.sdr);
+        case V4L2_BUF_TYPE_META_CAPTURE:
+                return put_v4l2_meta_format(&kp->fmt.meta, &up->fmt.meta);
         default:
                 pr_info("compat_ioctl32: unexpected VIDIOC_FMT type %d\n",
                                                                 kp->type);
@@ -990,6 +1009,10 @@ static long do_video_ioctl(struct file *file, unsigned int cmd, unsigned long ar
                 if (put_v4l2_ext_controls32(&karg.v2ecs, up))
                         err = -EFAULT;
                 break;
+        case VIDIOC_S_EDID:
+                if (put_v4l2_edid32(&karg.v2edid, up))
+                        err = -EFAULT;
+                break;
         }
         if (err)
                 return err;
@@ -1011,7 +1034,6 @@ static long do_video_ioctl(struct file *file, unsigned int cmd, unsigned long ar
                 break;
 
         case VIDIOC_G_EDID:
-        case VIDIOC_S_EDID:
                 err = put_v4l2_edid32(&karg.v2edid, up);
                 break;
 
diff --git a/drivers/media/v4l2-core/v4l2-ctrls.c b/drivers/media/v4l2-core/v4l2-ctrls.c
index b9e08e3d6e0e..ec42872d11cf 100644
--- a/drivers/media/v4l2-core/v4l2-ctrls.c
+++ b/drivers/media/v4l2-core/v4l2-ctrls.c
@@ -459,8 +459,8 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
         };
         static const char * const dv_rgb_range[] = {
                 "Automatic",
-                "RGB limited range (16-235)",
-                "RGB full range (0-255)",
+                "RGB Limited Range (16-235)",
+                "RGB Full Range (0-255)",
                 NULL,
         };
         static const char * const dv_it_content_type[] = {
@@ -997,6 +997,10 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
                 *min = 0;
                 *max = *step = 1;
                 break;
+        case V4L2_CID_ROTATE:
+                *type = V4L2_CTRL_TYPE_INTEGER;
+                *flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+                break;
         case V4L2_CID_MPEG_VIDEO_MV_H_SEARCH_RANGE:
         case V4L2_CID_MPEG_VIDEO_MV_V_SEARCH_RANGE:
                 *type = V4L2_CTRL_TYPE_INTEGER;
diff --git a/drivers/media/v4l2-core/v4l2-dev.c b/drivers/media/v4l2-core/v4l2-dev.c
index fa2124cb31bd..c647ba648805 100644
--- a/drivers/media/v4l2-core/v4l2-dev.c
+++ b/drivers/media/v4l2-core/v4l2-dev.c
@@ -575,30 +575,34 @@ static void determine_valid_ioctls(struct video_device *vdev)
                 set_bit(_IOC_NR(VIDIOC_ENUM_FREQ_BANDS), valid_ioctls);
 
         if (is_vid || is_tch) {
-                /* video specific ioctls */
+                /* video and metadata specific ioctls */
                 if ((is_rx && (ops->vidioc_enum_fmt_vid_cap ||
                                ops->vidioc_enum_fmt_vid_cap_mplane ||
-                               ops->vidioc_enum_fmt_vid_overlay)) ||
+                               ops->vidioc_enum_fmt_vid_overlay ||
+                               ops->vidioc_enum_fmt_meta_cap)) ||
                     (is_tx && (ops->vidioc_enum_fmt_vid_out ||
                                ops->vidioc_enum_fmt_vid_out_mplane)))
                         set_bit(_IOC_NR(VIDIOC_ENUM_FMT), valid_ioctls);
                 if ((is_rx && (ops->vidioc_g_fmt_vid_cap ||
                                ops->vidioc_g_fmt_vid_cap_mplane ||
-                               ops->vidioc_g_fmt_vid_overlay)) ||
+                               ops->vidioc_g_fmt_vid_overlay ||
+                               ops->vidioc_g_fmt_meta_cap)) ||
                     (is_tx && (ops->vidioc_g_fmt_vid_out ||
                                ops->vidioc_g_fmt_vid_out_mplane ||
                                ops->vidioc_g_fmt_vid_out_overlay)))
                          set_bit(_IOC_NR(VIDIOC_G_FMT), valid_ioctls);
                 if ((is_rx && (ops->vidioc_s_fmt_vid_cap ||
                                ops->vidioc_s_fmt_vid_cap_mplane ||
-                               ops->vidioc_s_fmt_vid_overlay)) ||
+                               ops->vidioc_s_fmt_vid_overlay ||
+                               ops->vidioc_s_fmt_meta_cap)) ||
                     (is_tx && (ops->vidioc_s_fmt_vid_out ||
                                ops->vidioc_s_fmt_vid_out_mplane ||
                                ops->vidioc_s_fmt_vid_out_overlay)))
                          set_bit(_IOC_NR(VIDIOC_S_FMT), valid_ioctls);
                 if ((is_rx && (ops->vidioc_try_fmt_vid_cap ||
                                ops->vidioc_try_fmt_vid_cap_mplane ||
-                               ops->vidioc_try_fmt_vid_overlay)) ||
+                               ops->vidioc_try_fmt_vid_overlay ||
+                               ops->vidioc_try_fmt_meta_cap)) ||
                     (is_tx && (ops->vidioc_try_fmt_vid_out ||
                                ops->vidioc_try_fmt_vid_out_mplane ||
                                ops->vidioc_try_fmt_vid_out_overlay)))
@@ -664,7 +668,7 @@ static void determine_valid_ioctls(struct video_device *vdev)
         }
 
         if (is_vid || is_vbi || is_sdr || is_tch) {
-                /* ioctls valid for video, vbi or sdr */
+                /* ioctls valid for video, metadata, vbi or sdr */
                 SET_VALID_IOCTL(ops, VIDIOC_REQBUFS, vidioc_reqbufs);
                 SET_VALID_IOCTL(ops, VIDIOC_QUERYBUF, vidioc_querybuf);
                 SET_VALID_IOCTL(ops, VIDIOC_QBUF, vidioc_qbuf);
diff --git a/drivers/media/v4l2-core/v4l2-device.c b/drivers/media/v4l2-core/v4l2-device.c
index f364cc1b521d..937c6de85606 100644
--- a/drivers/media/v4l2-core/v4l2-device.c
+++ b/drivers/media/v4l2-core/v4l2-device.c
@@ -235,6 +235,9 @@ int v4l2_device_register_subdev_nodes(struct v4l2_device *v4l2_dev)
                 if (!(sd->flags & V4L2_SUBDEV_FL_HAS_DEVNODE))
                         continue;
 
+                if (sd->devnode)
+                        continue;
+
                 vdev = kzalloc(sizeof(*vdev), GFP_KERNEL);
                 if (!vdev) {
                         err = -ENOMEM;
diff --git a/drivers/media/v4l2-core/v4l2-ioctl.c b/drivers/media/v4l2-core/v4l2-ioctl.c
index 0c3f238a2e76..e5a2187381db 100644
--- a/drivers/media/v4l2-core/v4l2-ioctl.c
+++ b/drivers/media/v4l2-core/v4l2-ioctl.c
@@ -155,6 +155,7 @@ const char *v4l2_type_names[] = {
         [V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE] = "vid-out-mplane",
         [V4L2_BUF_TYPE_SDR_CAPTURE]        = "sdr-cap",
         [V4L2_BUF_TYPE_SDR_OUTPUT]         = "sdr-out",
+        [V4L2_BUF_TYPE_META_CAPTURE]       = "meta-cap",
 };
 EXPORT_SYMBOL(v4l2_type_names);
 
@@ -246,6 +247,7 @@ static void v4l_print_format(const void *arg, bool write_only)
         const struct v4l2_sliced_vbi_format *sliced;
         const struct v4l2_window *win;
         const struct v4l2_sdr_format *sdr;
+        const struct v4l2_meta_format *meta;
         unsigned i;
 
         pr_cont("type=%s", prt_names(p->type, v4l2_type_names));
@@ -325,6 +327,15 @@ static void v4l_print_format(const void *arg, bool write_only)
                         (sdr->pixelformat >> 16) & 0xff,
                         (sdr->pixelformat >> 24) & 0xff);
                 break;
+        case V4L2_BUF_TYPE_META_CAPTURE:
+                meta = &p->fmt.meta;
+                pr_cont(", dataformat=%c%c%c%c, buffersize=%u\n",
+                        (meta->dataformat >>  0) & 0xff,
+                        (meta->dataformat >>  8) & 0xff,
+                        (meta->dataformat >> 16) & 0xff,
+                        (meta->dataformat >> 24) & 0xff,
+                        meta->buffersize);
+                break;
         }
 }
 
@@ -943,6 +954,10 @@ static int check_fmt(struct file *file, enum v4l2_buf_type type)
                 if (is_sdr && is_tx && ops->vidioc_g_fmt_sdr_out)
                         return 0;
                 break;
+        case V4L2_BUF_TYPE_META_CAPTURE:
+                if (is_vid && is_rx && ops->vidioc_g_fmt_meta_cap)
+                        return 0;
+                break;
         default:
                 break;
         }
@@ -1131,6 +1146,7 @@ static void v4l_fill_fmtdesc(struct v4l2_fmtdesc *fmt)
         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_Z16:          descr = "16-bit Depth"; break;
+        case V4L2_PIX_FMT_INZI:         descr = "Planar 10:16 Greyscale Depth"; break;
         case V4L2_PIX_FMT_PAL8:         descr = "8-bit Palette"; break;
         case V4L2_PIX_FMT_UV8:          descr = "8-bit Chrominance UV 4-4"; break;
         case V4L2_PIX_FMT_YVU410:       descr = "Planar YVU 4:1:0"; break;
@@ -1217,6 +1233,8 @@ static void v4l_fill_fmtdesc(struct v4l2_fmtdesc *fmt)
         case V4L2_TCH_FMT_DELTA_TD08:   descr = "8-bit signed deltas"; break;
         case V4L2_TCH_FMT_TU16:         descr = "16-bit unsigned touch data"; break;
         case V4L2_TCH_FMT_TU08:         descr = "8-bit unsigned touch data"; break;
+        case V4L2_META_FMT_VSP1_HGO:    descr = "R-Car VSP1 1-D Histogram"; break;
+        case V4L2_META_FMT_VSP1_HGT:    descr = "R-Car VSP1 2-D Histogram"; break;
 
         default:
                 /* Compressed formats */
@@ -1326,6 +1344,11 @@ static int v4l_enum_fmt(const struct v4l2_ioctl_ops *ops,
                         break;
                 ret = ops->vidioc_enum_fmt_sdr_out(file, fh, arg);
                 break;
+        case V4L2_BUF_TYPE_META_CAPTURE:
+                if (unlikely(!is_rx || !is_vid || !ops->vidioc_enum_fmt_meta_cap))
+                        break;
+                ret = ops->vidioc_enum_fmt_meta_cap(file, fh, arg);
+                break;
         }
         if (ret == 0)
                 v4l_fill_fmtdesc(p);
@@ -1425,6 +1448,10 @@ static int v4l_g_fmt(const struct v4l2_ioctl_ops *ops,
                 if (unlikely(!is_tx || !is_sdr || !ops->vidioc_g_fmt_sdr_out))
                         break;
                 return ops->vidioc_g_fmt_sdr_out(file, fh, arg);
+        case V4L2_BUF_TYPE_META_CAPTURE:
+                if (unlikely(!is_rx || !is_vid || !ops->vidioc_g_fmt_meta_cap))
+                        break;
+                return ops->vidioc_g_fmt_meta_cap(file, fh, arg);
         }
         return -EINVAL;
 }
@@ -1530,6 +1557,11 @@ static int v4l_s_fmt(const struct v4l2_ioctl_ops *ops,
                         break;
                 CLEAR_AFTER_FIELD(p, fmt.sdr);
                 return ops->vidioc_s_fmt_sdr_out(file, fh, arg);
+        case V4L2_BUF_TYPE_META_CAPTURE:
+                if (unlikely(!is_rx || !is_vid || !ops->vidioc_s_fmt_meta_cap))
+                        break;
+                CLEAR_AFTER_FIELD(p, fmt.meta);
+                return ops->vidioc_s_fmt_meta_cap(file, fh, arg);
         }
         return -EINVAL;
 }
@@ -1615,6 +1647,11 @@ static int v4l_try_fmt(const struct v4l2_ioctl_ops *ops,
                         break;
                 CLEAR_AFTER_FIELD(p, fmt.sdr);
                 return ops->vidioc_try_fmt_sdr_out(file, fh, arg);
+        case V4L2_BUF_TYPE_META_CAPTURE:
+                if (unlikely(!is_rx || !is_vid || !ops->vidioc_try_fmt_meta_cap))
+                        break;
+                CLEAR_AFTER_FIELD(p, fmt.meta);
+                return ops->vidioc_try_fmt_meta_cap(file, fh, arg);
         }
         return -EINVAL;
 }
diff --git a/drivers/media/v4l2-core/videobuf2-core.c b/drivers/media/v4l2-core/videobuf2-core.c
index 7c1d390ea438..94afbbf92807 100644
--- a/drivers/media/v4l2-core/videobuf2-core.c
+++ b/drivers/media/v4l2-core/videobuf2-core.c
@@ -984,11 +984,12 @@ static int __qbuf_userptr(struct vb2_buffer *vb, const void *pb)
 
         memset(planes, 0, sizeof(planes[0]) * vb->num_planes);
         /* Copy relevant information provided by the userspace */
-        if (pb)
+        if (pb) {
                 ret = call_bufop(vb->vb2_queue, fill_vb2_buffer,
                                  vb, pb, planes);
-        if (ret)
-                return ret;
+                if (ret)
+                        return ret;
+        }
 
         for (plane = 0; plane < vb->num_planes; ++plane) {
                 /* Skip the plane if already verified */
@@ -1101,11 +1102,12 @@ static int __qbuf_dmabuf(struct vb2_buffer *vb, const void *pb)
 
         memset(planes, 0, sizeof(planes[0]) * vb->num_planes);
         /* Copy relevant information provided by the userspace */
-        if (pb)
+        if (pb) {
                 ret = call_bufop(vb->vb2_queue, fill_vb2_buffer,
                                  vb, pb, planes);
-        if (ret)
-                return ret;
+                if (ret)
+                        return ret;
+        }
 
         for (plane = 0; plane < vb->num_planes; ++plane) {
                 struct dma_buf *dbuf = dma_buf_get(planes[plane].m.fd);
diff --git a/drivers/media/v4l2-core/videobuf2-dma-contig.c b/drivers/media/v4l2-core/videobuf2-dma-contig.c
index 2db0413f5d57..4f246d166111 100644
--- a/drivers/media/v4l2-core/videobuf2-dma-contig.c
+++ b/drivers/media/v4l2-core/videobuf2-dma-contig.c
@@ -12,6 +12,7 @@
 
 #include <linux/dma-buf.h>
 #include <linux/module.h>
+#include <linux/refcount.h>
 #include <linux/scatterlist.h>
 #include <linux/sched.h>
 #include <linux/slab.h>
@@ -34,7 +35,7 @@ struct vb2_dc_buf {
 
         /* MMAP related */
         struct vb2_vmarea_handler       handler;
-        atomic_t                        refcount;
+        refcount_t                      refcount;
         struct sg_table                 *sgt_base;
 
         /* DMABUF related */
@@ -86,7 +87,7 @@ static unsigned int vb2_dc_num_users(void *buf_priv)
 {
         struct vb2_dc_buf *buf = buf_priv;
 
-        return atomic_read(&buf->refcount);
+        return refcount_read(&buf->refcount);
 }
 
 static void vb2_dc_prepare(void *buf_priv)
@@ -122,7 +123,7 @@ static void vb2_dc_put(void *buf_priv)
 {
         struct vb2_dc_buf *buf = buf_priv;
 
-        if (!atomic_dec_and_test(&buf->refcount))
+        if (!refcount_dec_and_test(&buf->refcount))
                 return;
 
         if (buf->sgt_base) {
@@ -170,7 +171,7 @@ static void *vb2_dc_alloc(struct device *dev, unsigned long attrs,
         buf->handler.put = vb2_dc_put;
         buf->handler.arg = buf;
 
-        atomic_inc(&buf->refcount);
+        refcount_set(&buf->refcount, 1);
 
         return buf;
 }
@@ -407,7 +408,7 @@ static struct dma_buf *vb2_dc_get_dmabuf(void *buf_priv, unsigned long flags)
                 return NULL;
 
         /* dmabuf keeps reference to vb2 buffer */
-        atomic_inc(&buf->refcount);
+        refcount_inc(&buf->refcount);
 
         return dbuf;
 }
diff --git a/drivers/media/v4l2-core/videobuf2-dma-sg.c b/drivers/media/v4l2-core/videobuf2-dma-sg.c
index 6fd1343b7c13..8e8798a74760 100644
--- a/drivers/media/v4l2-core/videobuf2-dma-sg.c
+++ b/drivers/media/v4l2-core/videobuf2-dma-sg.c
@@ -12,6 +12,7 @@
 
 #include <linux/module.h>
 #include <linux/mm.h>
+#include <linux/refcount.h>
 #include <linux/scatterlist.h>
 #include <linux/sched.h>
 #include <linux/slab.h>
@@ -46,7 +47,7 @@ struct vb2_dma_sg_buf {
         struct sg_table                 *dma_sgt;
         size_t                          size;
         unsigned int                    num_pages;
-        atomic_t                        refcount;
+        refcount_t                      refcount;
         struct vb2_vmarea_handler       handler;
 
         struct dma_buf_attachment       *db_attach;
@@ -150,7 +151,7 @@ static void *vb2_dma_sg_alloc(struct device *dev, unsigned long dma_attrs,
         buf->handler.put = vb2_dma_sg_put;
         buf->handler.arg = buf;
 
-        atomic_inc(&buf->refcount);
+        refcount_set(&buf->refcount, 1);
 
         dprintk(1, "%s: Allocated buffer of %d pages\n",
                 __func__, buf->num_pages);
@@ -176,7 +177,7 @@ static void vb2_dma_sg_put(void *buf_priv)
         struct sg_table *sgt = &buf->sg_table;
         int i = buf->num_pages;
 
-        if (atomic_dec_and_test(&buf->refcount)) {
+        if (refcount_dec_and_test(&buf->refcount)) {
                 dprintk(1, "%s: Freeing buffer of %d pages\n", __func__,
                         buf->num_pages);
                 dma_unmap_sg_attrs(buf->dev, sgt->sgl, sgt->orig_nents,
@@ -320,7 +321,7 @@ static unsigned int vb2_dma_sg_num_users(void *buf_priv)
 {
         struct vb2_dma_sg_buf *buf = buf_priv;
 
-        return atomic_read(&buf->refcount);
+        return refcount_read(&buf->refcount);
 }
 
 static int vb2_dma_sg_mmap(void *buf_priv, struct vm_area_struct *vma)
@@ -530,7 +531,7 @@ static struct dma_buf *vb2_dma_sg_get_dmabuf(void *buf_priv, unsigned long flags
                 return NULL;
 
         /* dmabuf keeps reference to vb2 buffer */
-        atomic_inc(&buf->refcount);
+        refcount_inc(&buf->refcount);
 
         return dbuf;
 }
diff --git a/drivers/media/v4l2-core/videobuf2-memops.c b/drivers/media/v4l2-core/videobuf2-memops.c
index 1cd322e939c7..4bb8424114ce 100644
--- a/drivers/media/v4l2-core/videobuf2-memops.c
+++ b/drivers/media/v4l2-core/videobuf2-memops.c
@@ -96,10 +96,10 @@ static void vb2_common_vm_open(struct vm_area_struct *vma)
         struct vb2_vmarea_handler *h = vma->vm_private_data;
 
         pr_debug("%s: %p, refcount: %d, vma: %08lx-%08lx\n",
-               __func__, h, atomic_read(h->refcount), vma->vm_start,
+               __func__, h, refcount_read(h->refcount), vma->vm_start,
                vma->vm_end);
 
-        atomic_inc(h->refcount);
+        refcount_inc(h->refcount);
 }
 
 /**
@@ -114,7 +114,7 @@ static void vb2_common_vm_close(struct vm_area_struct *vma)
         struct vb2_vmarea_handler *h = vma->vm_private_data;
 
         pr_debug("%s: %p, refcount: %d, vma: %08lx-%08lx\n",
-               __func__, h, atomic_read(h->refcount), vma->vm_start,
+               __func__, h, refcount_read(h->refcount), vma->vm_start,
                vma->vm_end);
 
         h->put(h->arg);
diff --git a/drivers/media/v4l2-core/videobuf2-v4l2.c b/drivers/media/v4l2-core/videobuf2-v4l2.c
index 3529849d2218..0c0669976bdc 100644
--- a/drivers/media/v4l2-core/videobuf2-v4l2.c
+++ b/drivers/media/v4l2-core/videobuf2-v4l2.c
@@ -544,6 +544,9 @@ int vb2_create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create)
         case V4L2_BUF_TYPE_SDR_OUTPUT:
                 requested_sizes[0] = f->fmt.sdr.buffersize;
                 break;
+        case V4L2_BUF_TYPE_META_CAPTURE:
+                requested_sizes[0] = f->fmt.meta.buffersize;
+                break;
         default:
                 return -EINVAL;
         }
diff --git a/drivers/media/v4l2-core/videobuf2-vmalloc.c b/drivers/media/v4l2-core/videobuf2-vmalloc.c
index 27d1db3bb8cf..b337d780844c 100644
--- a/drivers/media/v4l2-core/videobuf2-vmalloc.c
+++ b/drivers/media/v4l2-core/videobuf2-vmalloc.c
@@ -13,6 +13,7 @@
 #include <linux/io.h>
 #include <linux/module.h>
 #include <linux/mm.h>
+#include <linux/refcount.h>
 #include <linux/sched.h>
 #include <linux/slab.h>
 #include <linux/vmalloc.h>
@@ -26,7 +27,7 @@ struct vb2_vmalloc_buf {
         struct frame_vector             *vec;
         enum dma_data_direction         dma_dir;
         unsigned long                   size;
-        atomic_t                        refcount;
+        refcount_t                      refcount;
         struct vb2_vmarea_handler       handler;
         struct dma_buf                  *dbuf;
 };
@@ -56,7 +57,7 @@ static void *vb2_vmalloc_alloc(struct device *dev, unsigned long attrs,
                 return ERR_PTR(-ENOMEM);
         }
 
-        atomic_inc(&buf->refcount);
+        refcount_set(&buf->refcount, 1);
         return buf;
 }
 
@@ -64,7 +65,7 @@ static void vb2_vmalloc_put(void *buf_priv)
 {
         struct vb2_vmalloc_buf *buf = buf_priv;
 
-        if (atomic_dec_and_test(&buf->refcount)) {
+        if (refcount_dec_and_test(&buf->refcount)) {
                 vfree(buf->vaddr);
                 kfree(buf);
         }
@@ -161,7 +162,7 @@ static void *vb2_vmalloc_vaddr(void *buf_priv)
 static unsigned int vb2_vmalloc_num_users(void *buf_priv)
 {
         struct vb2_vmalloc_buf *buf = buf_priv;
-        return atomic_read(&buf->refcount);
+        return refcount_read(&buf->refcount);
 }
 
 static int vb2_vmalloc_mmap(void *buf_priv, struct vm_area_struct *vma)
@@ -368,7 +369,7 @@ static struct dma_buf *vb2_vmalloc_get_dmabuf(void *buf_priv, unsigned long flag
                 return NULL;
 
         /* dmabuf keeps reference to vb2 buffer */
-        atomic_inc(&buf->refcount);
+        refcount_inc(&buf->refcount);
 
         return dbuf;
 }