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 ...
author: Linus Torvalds <torvalds@linux-foundation.org> 2017-05-06 03:34:57 +0300
committer: Linus Torvalds <torvalds@linux-foundation.org> 2017-05-06 03:34:57 +0300
commit: e87d51ac61f88ae44fe14b34abe08566032d726b (patch)
tree: fc418d2e29fbf8a06f1ed0b6eaff8ba03e0543d7 /drivers
parent: bdc713bf5674bc6a881bd05c85e2a0f811b409b3 (diff)
parent: 3622d3e77ecef090b5111e3c5423313f11711dfa (diff)
download: linux-e87d51ac61f88ae44fe14b34abe08566032d726b.tar.xz
280 files changed, 13392 insertions, 4784 deletions
diff --git a/drivers/gpu/drm/exynos/exynos_hdmi.c b/drivers/gpu/drm/exynos/exynos_hdmi.c
index 52438404c8c9..1ff6ab6371e8 100644
--- a/drivers/gpu/drm/exynos/exynos_hdmi.c
+++ b/drivers/gpu/drm/exynos/exynos_hdmi.c
@@ -43,6 +43,8 @@
 
 #include <drm/exynos_drm.h>
 
+#include <media/cec-notifier.h>
+
 #include "exynos_drm_crtc.h"
 
 #define HOTPLUG_DEBOUNCE_MS		1100
@@ -118,6 +120,7 @@ struct hdmi_context {
 	bool				dvi_mode;
 	struct delayed_work		hotplug_work;
 	struct drm_display_mode		current_mode;
+	struct cec_notifier		*notifier;
 	const struct hdmi_driver_data	*drv_data;
 
 	void __iomem			*regs;
@@ -821,6 +824,7 @@ static enum drm_connector_status hdmi_detect(struct drm_connector *connector,
 	if (gpiod_get_value(hdata->hpd_gpio))
 		return connector_status_connected;
 
+	cec_notifier_set_phys_addr(hdata->notifier, CEC_PHYS_ADDR_INVALID);
 	return connector_status_disconnected;
 }
 
@@ -859,6 +863,7 @@ static int hdmi_get_modes(struct drm_connector *connector)
 		edid->width_cm, edid->height_cm);
 
 	drm_mode_connector_update_edid_property(connector, edid);
+	cec_notifier_set_phys_addr_from_edid(hdata->notifier, edid);
 
 	ret = drm_add_edid_modes(connector, edid);
 
@@ -1501,6 +1506,7 @@ static void hdmi_disable(struct drm_encoder *encoder)
 	if (funcs && funcs->disable)
 		(*funcs->disable)(crtc);
 
+	cec_notifier_set_phys_addr(hdata->notifier, CEC_PHYS_ADDR_INVALID);
 	cancel_delayed_work(&hdata->hotplug_work);
 
 	hdmiphy_disable(hdata);
@@ -1880,15 +1886,22 @@ static int hdmi_probe(struct platform_device *pdev)
 		}
 	}
 
+	hdata->notifier = cec_notifier_get(&pdev->dev);
+	if (hdata->notifier == NULL) {
+		ret = -ENOMEM;
+		goto err_hdmiphy;
+	}
+
 	pm_runtime_enable(dev);
 
 	ret = component_add(&pdev->dev, &hdmi_component_ops);
 	if (ret)
-		goto err_disable_pm_runtime;
+		goto err_notifier_put;
 
 	return ret;
 
-err_disable_pm_runtime:
+err_notifier_put:
+	cec_notifier_put(hdata->notifier);
 	pm_runtime_disable(dev);
 
 err_hdmiphy:
@@ -1907,9 +1920,11 @@ static int hdmi_remove(struct platform_device *pdev)
 	struct hdmi_context *hdata = platform_get_drvdata(pdev);
 
 	cancel_delayed_work_sync(&hdata->hotplug_work);
+	cec_notifier_set_phys_addr(hdata->notifier, CEC_PHYS_ADDR_INVALID);
 
 	component_del(&pdev->dev, &hdmi_component_ops);
 
+	cec_notifier_put(hdata->notifier);
 	pm_runtime_disable(&pdev->dev);
 
 	if (!IS_ERR(hdata->reg_hdmi_en))
diff --git a/drivers/gpu/drm/sti/sti_hdmi.c b/drivers/gpu/drm/sti/sti_hdmi.c
index ce2dcba679d5..243905b6ae59 100644
--- a/drivers/gpu/drm/sti/sti_hdmi.c
+++ b/drivers/gpu/drm/sti/sti_hdmi.c
@@ -771,6 +771,8 @@ static void sti_hdmi_disable(struct drm_bridge *bridge)
 	clk_disable_unprepare(hdmi->clk_pix);
 
 	hdmi->enabled = false;
+
+	cec_notifier_set_phys_addr(hdmi->notifier, CEC_PHYS_ADDR_INVALID);
 }
 
 /**
@@ -973,6 +975,7 @@ static int sti_hdmi_connector_get_modes(struct drm_connector *connector)
 	DRM_DEBUG_KMS("%s : %dx%d cm\n",
 		      (hdmi->hdmi_monitor ? "hdmi monitor" : "dvi monitor"),
 		      edid->width_cm, edid->height_cm);
+	cec_notifier_set_phys_addr_from_edid(hdmi->notifier, edid);
 
 	count = drm_add_edid_modes(connector, edid);
 	drm_mode_connector_update_edid_property(connector, edid);
@@ -1035,6 +1038,7 @@ sti_hdmi_connector_detect(struct drm_connector *connector, bool force)
 	}
 
 	DRM_DEBUG_DRIVER("hdmi cable disconnected\n");
+	cec_notifier_set_phys_addr(hdmi->notifier, CEC_PHYS_ADDR_INVALID);
 	return connector_status_disconnected;
 }
 
@@ -1423,6 +1427,10 @@ static int sti_hdmi_probe(struct platform_device *pdev)
 		goto release_adapter;
 	}
 
+	hdmi->notifier = cec_notifier_get(&pdev->dev);
+	if (!hdmi->notifier)
+		goto release_adapter;
+
 	hdmi->reset = devm_reset_control_get(dev, "hdmi");
 	/* Take hdmi out of reset */
 	if (!IS_ERR(hdmi->reset))
@@ -1442,11 +1450,14 @@ static int sti_hdmi_remove(struct platform_device *pdev)
 {
 	struct sti_hdmi *hdmi = dev_get_drvdata(&pdev->dev);
 
+	cec_notifier_set_phys_addr(hdmi->notifier, CEC_PHYS_ADDR_INVALID);
+
 	i2c_put_adapter(hdmi->ddc_adapt);
 	if (hdmi->audio_pdev)
 		platform_device_unregister(hdmi->audio_pdev);
 	component_del(&pdev->dev, &sti_hdmi_ops);
 
+	cec_notifier_put(hdmi->notifier);
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/sti/sti_hdmi.h b/drivers/gpu/drm/sti/sti_hdmi.h
index 407012350f1a..c6469b56ce7e 100644
--- a/drivers/gpu/drm/sti/sti_hdmi.h
+++ b/drivers/gpu/drm/sti/sti_hdmi.h
@@ -11,6 +11,7 @@
 #include <linux/platform_device.h>
 
 #include <drm/drmP.h>
+#include <media/cec-notifier.h>
 
 #define HDMI_STA           0x0010
 #define HDMI_STA_DLL_LCK   BIT(5)
@@ -64,6 +65,7 @@ static const struct drm_prop_enum_list colorspace_mode_names[] = {
  * @audio_pdev: ASoC hdmi-codec platform device
  * @audio: hdmi audio parameters.
  * @drm_connector: hdmi connector
+ * @notifier: hotplug detect notifier
  */
 struct sti_hdmi {
 	struct device dev;
@@ -89,6 +91,7 @@ struct sti_hdmi {
 	struct platform_device *audio_pdev;
 	struct hdmi_audio_params audio;
 	struct drm_connector *drm_connector;
+	struct cec_notifier *notifier;
 };
 
 u32 hdmi_read(struct sti_hdmi *hdmi, int offset);
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/staging/media/s5p-cec/Makefile b/drivers/media/platform/s5p-cec/Makefile
index 0e2cf457825a..0e2cf457825a 100644
--- a/drivers/staging/media/s5p-cec/Makefile
+++ b/drivers/media/platform/s5p-cec/Makefile
diff --git a/drivers/staging/media/s5p-cec/exynos_hdmi_cec.h b/drivers/media/platform/s5p-cec/exynos_hdmi_cec.h
index 7d9453505dce..7d9453505dce 100644
--- a/drivers/staging/media/s5p-cec/exynos_hdmi_cec.h
+++ b/drivers/media/platform/s5p-cec/exynos_hdmi_cec.h
diff --git a/drivers/staging/media/s5p-cec/exynos_hdmi_cecctrl.c b/drivers/media/platform/s5p-cec/exynos_hdmi_cecctrl.c
index 1edf667d562a..1edf667d562a 100644
--- a/drivers/staging/media/s5p-cec/exynos_hdmi_cecctrl.c
+++ b/drivers/media/platform/s5p-cec/exynos_hdmi_cecctrl.c
diff --git a/drivers/staging/media/s5p-cec/regs-cec.h b/drivers/media/platform/s5p-cec/regs-cec.h
index b2e7e129920e..b2e7e129920e 100644
--- a/drivers/staging/media/s5p-cec/regs-cec.h
+++ b/drivers/media/platform/s5p-cec/regs-cec.h
diff --git a/drivers/staging/media/s5p-cec/s5p_cec.c b/drivers/media/platform/s5p-cec/s5p_cec.c
index 2a07968b5ac6..664937b61fa4 100644
--- a/drivers/staging/media/s5p-cec/s5p_cec.c
+++ b/drivers/media/platform/s5p-cec/s5p_cec.c
@@ -19,11 +19,13 @@
 #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"
@@ -37,7 +39,7 @@ 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 = adap->priv;
+	struct s5p_cec_dev *cec = cec_get_drvdata(adap);
 
 	if (enable) {
 		pm_runtime_get_sync(cec->dev);
@@ -61,7 +63,7 @@ static int s5p_cec_adap_enable(struct cec_adapter *adap, bool enable)
 
 static int s5p_cec_adap_log_addr(struct cec_adapter *adap, u8 addr)
 {
-	struct s5p_cec_dev *cec = adap->priv;
+	struct s5p_cec_dev *cec = cec_get_drvdata(adap);
 
 	s5p_cec_set_addr(cec, addr);
 	return 0;
@@ -70,7 +72,7 @@ static int s5p_cec_adap_log_addr(struct cec_adapter *adap, u8 addr)
 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 = adap->priv;
+	struct s5p_cec_dev *cec = cec_get_drvdata(adap);
 
 	/*
 	 * Unclear if 0 retries are allowed by the hardware, so have 1 as
@@ -167,10 +169,22 @@ static const struct cec_adap_ops s5p_cec_adap_ops = {
 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;
@@ -200,24 +214,33 @@ static int s5p_cec_probe(struct platform_device *pdev)
 	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_PHYS_ADDR | CEC_CAP_LOG_ADDRS | CEC_CAP_TRANSMIT |
+		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) {
-		cec_delete_adapter(cec->adap);
-		return ret;
-	}
+	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)
@@ -225,6 +248,7 @@ 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;
 }
diff --git a/drivers/staging/media/s5p-cec/s5p_cec.h b/drivers/media/platform/s5p-cec/s5p_cec.h
index 03732c13d19f..7015845c1caa 100644
--- a/drivers/staging/media/s5p-cec/s5p_cec.h
+++ b/drivers/media/platform/s5p-cec/s5p_cec.h
@@ -59,12 +59,15 @@ enum cec_state {
 	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;
 
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/staging/media/st-cec/Makefile b/drivers/media/platform/sti/cec/Makefile
index f07905e1448a..f07905e1448a 100644
--- a/drivers/staging/media/st-cec/Makefile
+++ b/drivers/media/platform/sti/cec/Makefile
diff --git a/drivers/staging/media/st-cec/stih-cec.c b/drivers/media/platform/sti/cec/stih-cec.c
index 3c25638a9610..39ff55145a82 100644
--- a/drivers/staging/media/st-cec/stih-cec.c
+++ b/drivers/media/platform/sti/cec/stih-cec.c
@@ -1,6 +1,4 @@
 /*
- * drivers/staging/media/st-cec/stih-cec.c
- *
  * STIH4xx CEC driver
  * Copyright (C) STMicroelectronic SA 2016
  *
@@ -15,9 +13,11 @@
 #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"
 
@@ -129,11 +129,12 @@ struct stih_cec {
 	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 = adap->priv;
+	struct stih_cec *cec = cec_get_drvdata(adap);
 
 	if (enable) {
 		/* The doc says (input TCLK_PERIOD * CEC_CLK_DIV) = 0.1ms */
@@ -189,7 +190,7 @@ static int stih_cec_adap_enable(struct cec_adapter *adap, bool enable)
 
 static int stih_cec_adap_log_addr(struct cec_adapter *adap, u8 logical_addr)
 {
-	struct stih_cec *cec = adap->priv;
+	struct stih_cec *cec = cec_get_drvdata(adap);
 	u32 reg = readl(cec->regs + CEC_ADDR_TABLE);
 
 	reg |= 1 << logical_addr;
@@ -205,7 +206,7 @@ static int stih_cec_adap_log_addr(struct cec_adapter *adap, u8 logical_addr)
 static int stih_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
 				  u32 signal_free_time, struct cec_msg *msg)
 {
-	struct stih_cec *cec = adap->priv;
+	struct stih_cec *cec = cec_get_drvdata(adap);
 	int i;
 
 	/* Copy message into registers */
@@ -303,12 +304,29 @@ 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);
@@ -335,7 +353,7 @@ static int stih_cec_probe(struct platform_device *pdev)
 	cec->adap = cec_allocate_adapter(&sti_cec_adap_ops, cec,
 			CEC_NAME,
 			CEC_CAP_LOG_ADDRS | CEC_CAP_PASSTHROUGH |
-			CEC_CAP_PHYS_ADDR | CEC_CAP_TRANSMIT, 1);
+			CEC_CAP_TRANSMIT, 1);
 	ret = PTR_ERR_OR_ZERO(cec->adap);
 	if (ret)
 		return ret;
@@ -346,12 +364,19 @@ static int stih_cec_probe(struct platform_device *pdev)
 		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;
 }
 
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;
 }
diff --git a/drivers/staging/media/Kconfig b/drivers/staging/media/Kconfig
index abd0e2d57c20..8ed8202da57a 100644
--- a/drivers/staging/media/Kconfig
+++ b/drivers/staging/media/Kconfig
@@ -29,11 +29,7 @@ source "drivers/staging/media/omap4iss/Kconfig"
 
 source "drivers/staging/media/platform/bcm2835/Kconfig"
 
-source "drivers/staging/media/s5p-cec/Kconfig"
-
 # Keep LIRC at the end, as it has sub-menus
 source "drivers/staging/media/lirc/Kconfig"
 
-source "drivers/staging/media/st-cec/Kconfig"
-
 endif
diff --git a/drivers/staging/media/Makefile b/drivers/staging/media/Makefile
index dc89325c463d..3a6adeabede1 100644
--- a/drivers/staging/media/Makefile
+++ b/drivers/staging/media/Makefile
@@ -1,8 +1,6 @@
 obj-$(CONFIG_I2C_BCM2048)	+= bcm2048/
-obj-$(CONFIG_VIDEO_SAMSUNG_S5P_CEC) += s5p-cec/
 obj-$(CONFIG_DVB_CXD2099)	+= cxd2099/
 obj-$(CONFIG_LIRC_STAGING)	+= lirc/
 obj-$(CONFIG_VIDEO_BCM2835)	+= platform/bcm2835/
 obj-$(CONFIG_VIDEO_DM365_VPFE)	+= davinci_vpfe/
 obj-$(CONFIG_VIDEO_OMAP4)	+= omap4iss/
-obj-$(CONFIG_VIDEO_STI_HDMI_CEC) += st-cec/
diff --git a/drivers/staging/media/bcm2048/radio-bcm2048.c b/drivers/staging/media/bcm2048/radio-bcm2048.c
index d605c41d0424..38f72d069e27 100644
--- a/drivers/staging/media/bcm2048/radio-bcm2048.c
+++ b/drivers/staging/media/bcm2048/radio-bcm2048.c
@@ -177,12 +177,12 @@
 
 #define BCM2048_FREQDEV_UNIT		10000
 #define BCM2048_FREQV4L2_MULTI		625
-#define dev_to_v4l2(f)	((f * BCM2048_FREQDEV_UNIT) / BCM2048_FREQV4L2_MULTI)
-#define v4l2_to_dev(f)	((f * BCM2048_FREQV4L2_MULTI) / BCM2048_FREQDEV_UNIT)
+#define dev_to_v4l2(f)	(((f) * BCM2048_FREQDEV_UNIT) / BCM2048_FREQV4L2_MULTI)
+#define v4l2_to_dev(f)	(((f) * BCM2048_FREQV4L2_MULTI) / BCM2048_FREQDEV_UNIT)
 
-#define msb(x)                  ((u8)((u16)x >> 8))
-#define lsb(x)                  ((u8)((u16)x &  0x00FF))
-#define compose_u16(msb, lsb)	(((u16)msb << 8) | lsb)
+#define msb(x)                  ((u8)((u16)(x) >> 8))
+#define lsb(x)                  ((u8)((u16)(x) &  0x00FF))
+#define compose_u16(msb, lsb)	(((u16)(msb) << 8) | (lsb))
 
 #define BCM2048_DEFAULT_POWERING_DELAY	20
 #define BCM2048_DEFAULT_REGION		0x02
@@ -1534,7 +1534,11 @@ static int bcm2048_parse_rt_match_c(struct bcm2048_device *bdev, int i,
 	if (crc == BCM2048_RDS_CRC_UNRECOVARABLE)
 		return 0;
 
-	BUG_ON((index+2) >= BCM2048_MAX_RDS_RT);
+	if ((index + 2) >= BCM2048_MAX_RDS_RT) {
+		dev_err(&bdev->client->dev,
+			"Incorrect index = %d\n", index);
+		return 0;
+	}
 
 	if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK) ==
 		BCM2048_RDS_BLOCK_C) {
@@ -1557,7 +1561,11 @@ static void bcm2048_parse_rt_match_d(struct bcm2048_device *bdev, int i,
 	if (crc == BCM2048_RDS_CRC_UNRECOVARABLE)
 		return;
 
-	BUG_ON((index+4) >= BCM2048_MAX_RDS_RT);
+	if ((index + 4) >= BCM2048_MAX_RDS_RT) {
+		dev_err(&bdev->client->dev,
+			"Incorrect index = %d\n", index);
+		return;
+	}
 
 	if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK) ==
 	    BCM2048_RDS_BLOCK_D)
@@ -2008,7 +2016,7 @@ static ssize_t bcm2048_##prop##_read(struct device *dev,		\
 	if (!bdev)							\
 		return -ENODEV;						\
 									\
-	out = kzalloc(size + 1, GFP_KERNEL);				\
+	out = kzalloc((size) + 1, GFP_KERNEL);				\
 	if (!out)							\
 		return -ENOMEM;						\
 									\
@@ -2634,7 +2642,7 @@ exit:
 	return err;
 }
 
-static int __exit bcm2048_i2c_driver_remove(struct i2c_client *client)
+static int bcm2048_i2c_driver_remove(struct i2c_client *client)
 {
 	struct bcm2048_device *bdev = i2c_get_clientdata(client);
 
@@ -2673,7 +2681,7 @@ static struct i2c_driver bcm2048_i2c_driver = {
 		.name	= BCM2048_DRIVER_NAME,
 	},
 	.probe		= bcm2048_i2c_driver_probe,
-	.remove		= __exit_p(bcm2048_i2c_driver_remove),
+	.remove		= bcm2048_i2c_driver_remove,
 	.id_table	= bcm2048_id,
 };
 
diff --git a/drivers/staging/media/lirc/Kconfig b/drivers/staging/media/lirc/Kconfig
index bc67da254262..3e350a9922de 100644
--- a/drivers/staging/media/lirc/Kconfig
+++ b/drivers/staging/media/lirc/Kconfig
@@ -12,18 +12,6 @@ menuconfig LIRC_STAGING
 
 if LIRC_STAGING
 
-config LIRC_SASEM
-	tristate "Sasem USB IR Remote"
-	depends on LIRC && USB
-	help
-	  Driver for the Sasem OnAir Remocon-V or Dign HV5 HTPC IR/VFD Module
-
-config LIRC_SIR
-	tristate "Built-in SIR IrDA port"
-	depends on RC_CORE
-	help
-	  Driver for the SIR IrDA port
-
 config LIRC_ZILOG
 	tristate "Zilog/Hauppauge IR Transmitter"
 	depends on LIRC && I2C
diff --git a/drivers/staging/media/lirc/Makefile b/drivers/staging/media/lirc/Makefile
index 28740c94349c..665562436e30 100644
--- a/drivers/staging/media/lirc/Makefile
+++ b/drivers/staging/media/lirc/Makefile
@@ -3,6 +3,4 @@
 
 # Each configuration option enables a list of files.
 
-obj-$(CONFIG_LIRC_SASEM)	+= lirc_sasem.o
-obj-$(CONFIG_LIRC_SIR)		+= lirc_sir.o
 obj-$(CONFIG_LIRC_ZILOG)	+= lirc_zilog.o
diff --git a/drivers/staging/media/lirc/lirc_sasem.c b/drivers/staging/media/lirc/lirc_sasem.c
deleted file mode 100644
index b0c176e14b6b..000000000000
--- a/drivers/staging/media/lirc/lirc_sasem.c
+++ /dev/null
@@ -1,899 +0,0 @@
-/*
- * lirc_sasem.c - USB remote support for LIRC
- * Version 0.5
- *
- * Copyright (C) 2004-2005 Oliver Stabel <oliver.stabel@gmx.de>
- *			 Tim Davies <tim@opensystems.net.au>
- *
- * This driver was derived from:
- *   Venky Raju <dev@venky.ws>
- *      "lirc_imon - "LIRC/VFD driver for Ahanix/Soundgraph IMON IR/VFD"
- *   Paul Miller <pmiller9@users.sourceforge.net>'s 2003-2004
- *      "lirc_atiusb - USB remote support for LIRC"
- *   Culver Consulting Services <henry@culcon.com>'s 2003
- *      "Sasem OnAir VFD/IR USB driver"
- *
- *
- * NOTE - The LCDproc iMon driver should work with this module.  More info at
- *	http://www.frogstorm.info/sasem
- */
-
-/*
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; either version 2 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/uaccess.h>
-#include <linux/usb.h>
-#include <linux/ktime.h>
-
-#include <media/lirc.h>
-#include <media/lirc_dev.h>
-
-#define MOD_AUTHOR	"Oliver Stabel <oliver.stabel@gmx.de>, " \
-			"Tim Davies <tim@opensystems.net.au>"
-#define MOD_DESC	"USB Driver for Sasem Remote Controller V1.1"
-#define MOD_NAME	"lirc_sasem"
-#define MOD_VERSION	"0.5"
-
-#define VFD_MINOR_BASE	144	/* Same as LCD */
-#define DEVICE_NAME	"lcd%d"
-
-#define BUF_CHUNK_SIZE	8
-#define BUF_SIZE	128
-
-#define IOCTL_LCD_CONTRAST 1
-
-/*** P R O T O T Y P E S ***/
-
-/* USB Callback prototypes */
-static int sasem_probe(struct usb_interface *interface,
-			const struct usb_device_id *id);
-static void sasem_disconnect(struct usb_interface *interface);
-static void usb_rx_callback(struct urb *urb);
-static void usb_tx_callback(struct urb *urb);
-
-/* VFD file_operations function prototypes */
-static int vfd_open(struct inode *inode, struct file *file);
-static long vfd_ioctl(struct file *file, unsigned int cmd, unsigned long arg);
-static int vfd_close(struct inode *inode, struct file *file);
-static ssize_t vfd_write(struct file *file, const char __user *buf,
-				size_t n_bytes, loff_t *pos);
-
-/* LIRC driver function prototypes */
-static int ir_open(void *data);
-static void ir_close(void *data);
-
-/*** G L O B A L S ***/
-#define SASEM_DATA_BUF_SZ	32
-
-struct sasem_context {
-	struct usb_device *dev;
-	int vfd_isopen;			/* VFD port has been opened */
-	unsigned int vfd_contrast;	/* VFD contrast */
-	int ir_isopen;			/* IR port has been opened */
-	int dev_present;		/* USB device presence */
-	struct mutex ctx_lock;		/* to lock this object */
-	wait_queue_head_t remove_ok;	/* For unexpected USB disconnects */
-
-	struct lirc_driver *driver;
-	struct usb_endpoint_descriptor *rx_endpoint;
-	struct usb_endpoint_descriptor *tx_endpoint;
-	struct urb *rx_urb;
-	struct urb *tx_urb;
-	unsigned char usb_rx_buf[8];
-	unsigned char usb_tx_buf[8];
-
-	struct tx_t {
-		unsigned char data_buf[SASEM_DATA_BUF_SZ]; /* user data
-							    * buffer
-							    */
-		struct completion finished;  /* wait for write to finish  */
-		atomic_t busy;		     /* write in progress */
-		int status;		     /* status of tx completion */
-	} tx;
-
-	/* for dealing with repeat codes (wish there was a toggle bit!) */
-	ktime_t presstime;
-	char lastcode[8];
-	int codesaved;
-};
-
-/* VFD file operations */
-static const struct file_operations vfd_fops = {
-	.owner		= THIS_MODULE,
-	.open		= &vfd_open,
-	.write		= vfd_write,
-	.unlocked_ioctl	= &vfd_ioctl,
-	.release	= &vfd_close,
-	.llseek		= noop_llseek,
-};
-
-/* USB Device ID for Sasem USB Control Board */
-static struct usb_device_id sasem_usb_id_table[] = {
-	/* Sasem USB Control Board */
-	{ USB_DEVICE(0x11ba, 0x0101) },
-	/* Terminating entry */
-	{}
-};
-
-/* USB Device data */
-static struct usb_driver sasem_driver = {
-	.name		= MOD_NAME,
-	.probe		= sasem_probe,
-	.disconnect	= sasem_disconnect,
-	.id_table	= sasem_usb_id_table,
-};
-
-static struct usb_class_driver sasem_class = {
-	.name		= DEVICE_NAME,
-	.fops		= &vfd_fops,
-	.minor_base	= VFD_MINOR_BASE,
-};
-
-/* to prevent races between open() and disconnect() */
-static DEFINE_MUTEX(disconnect_lock);
-
-static int debug;
-
-
-/*** M O D U L E   C O D E ***/
-MODULE_AUTHOR(MOD_AUTHOR);
-MODULE_DESCRIPTION(MOD_DESC);
-MODULE_LICENSE("GPL");
-module_param(debug, int, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(debug, "Debug messages: 0=no, 1=yes (default: no)");
-
-static void delete_context(struct sasem_context *context)
-{
-	usb_free_urb(context->tx_urb);  /* VFD */
-	usb_free_urb(context->rx_urb);  /* IR */
-	lirc_buffer_free(context->driver->rbuf);
-	kfree(context->driver->rbuf);
-	kfree(context->driver);
-	kfree(context);
-}
-
-static void deregister_from_lirc(struct sasem_context *context)
-{
-	int retval;
-	int minor = context->driver->minor;
-
-	retval = lirc_unregister_driver(minor);
-	if (retval)
-		dev_err(&context->dev->dev,
-			"%s: unable to deregister from lirc (%d)\n",
-			__func__, retval);
-	else
-		dev_info(&context->dev->dev,
-			 "Deregistered Sasem driver (minor:%d)\n", minor);
-}
-
-/**
- * Called when the VFD device (e.g. /dev/usb/lcd)
- * is opened by the application.
- */
-static int vfd_open(struct inode *inode, struct file *file)
-{
-	struct usb_interface *interface;
-	struct sasem_context *context = NULL;
-	int subminor;
-	int retval = 0;
-
-	/* prevent races with disconnect */
-	mutex_lock(&disconnect_lock);
-
-	subminor = iminor(inode);
-	interface = usb_find_interface(&sasem_driver, subminor);
-	if (!interface) {
-		pr_err("%s: could not find interface for minor %d\n",
-		       __func__, subminor);
-		retval = -ENODEV;
-		goto exit;
-	}
-	context = usb_get_intfdata(interface);
-
-	if (!context) {
-		dev_err(&interface->dev, "no context found for minor %d\n",
-			subminor);
-		retval = -ENODEV;
-		goto exit;
-	}
-
-	mutex_lock(&context->ctx_lock);
-
-	if (context->vfd_isopen) {
-		dev_err(&interface->dev,
-			"%s: VFD port is already open", __func__);
-		retval = -EBUSY;
-	} else {
-		context->vfd_isopen = 1;
-		file->private_data = context;
-		dev_info(&interface->dev, "VFD port opened\n");
-	}
-
-	mutex_unlock(&context->ctx_lock);
-
-exit:
-	mutex_unlock(&disconnect_lock);
-	return retval;
-}
-
-/**
- * Called when the VFD device (e.g. /dev/usb/lcd)
- * is closed by the application.
- */
-static long vfd_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
-{
-	struct sasem_context *context;
-
-	context = (struct sasem_context *) file->private_data;
-
-	if (!context) {
-		pr_err("%s: no context for device\n", __func__);
-		return -ENODEV;
-	}
-
-	mutex_lock(&context->ctx_lock);
-
-	switch (cmd) {
-	case IOCTL_LCD_CONTRAST:
-		if (arg > 1000)
-			arg = 1000;
-		context->vfd_contrast = (unsigned int)arg;
-		break;
-	default:
-		pr_info("Unknown IOCTL command\n");
-		mutex_unlock(&context->ctx_lock);
-		return -ENOIOCTLCMD;  /* not supported */
-	}
-
-	mutex_unlock(&context->ctx_lock);
-	return 0;
-}
-
-/**
- * Called when the VFD device (e.g. /dev/usb/lcd)
- * is closed by the application.
- */
-static int vfd_close(struct inode *inode, struct file *file)
-{
-	struct sasem_context *context = NULL;
-	int retval = 0;
-
-	context = (struct sasem_context *) file->private_data;
-
-	if (!context) {
-		pr_err("%s: no context for device\n", __func__);
-		return -ENODEV;
-	}
-
-	mutex_lock(&context->ctx_lock);
-
-	if (!context->vfd_isopen) {
-		dev_err(&context->dev->dev, "%s: VFD is not open\n", __func__);
-		retval = -EIO;
-	} else {
-		context->vfd_isopen = 0;
-		dev_info(&context->dev->dev, "VFD port closed\n");
-		if (!context->dev_present && !context->ir_isopen) {
-			/* Device disconnected before close and IR port is
-			 * not open. If IR port is open, context will be
-			 * deleted by ir_close.
-			 */
-			mutex_unlock(&context->ctx_lock);
-			delete_context(context);
-			return retval;
-		}
-	}
-
-	mutex_unlock(&context->ctx_lock);
-	return retval;
-}
-
-/**
- * Sends a packet to the VFD.
- */
-static int send_packet(struct sasem_context *context)
-{
-	unsigned int pipe;
-	int interval = 0;
-	int retval = 0;
-
-	pipe = usb_sndintpipe(context->dev,
-			context->tx_endpoint->bEndpointAddress);
-	interval = context->tx_endpoint->bInterval;
-
-	usb_fill_int_urb(context->tx_urb, context->dev, pipe,
-		context->usb_tx_buf, sizeof(context->usb_tx_buf),
-		usb_tx_callback, context, interval);
-
-	context->tx_urb->actual_length = 0;
-
-	init_completion(&context->tx.finished);
-	atomic_set(&context->tx.busy, 1);
-
-	retval =  usb_submit_urb(context->tx_urb, GFP_KERNEL);
-	if (retval) {
-		atomic_set(&context->tx.busy, 0);
-		dev_err(&context->dev->dev, "error submitting urb (%d)\n",
-			retval);
-	} else {
-		/* Wait for transmission to complete (or abort) */
-		mutex_unlock(&context->ctx_lock);
-		wait_for_completion(&context->tx.finished);
-		mutex_lock(&context->ctx_lock);
-
-		retval = context->tx.status;
-		if (retval)
-			dev_err(&context->dev->dev,
-				"packet tx failed (%d)\n", retval);
-	}
-
-	return retval;
-}
-
-/**
- * Writes data to the VFD.  The Sasem VFD is 2x16 characters
- * and requires data in 9 consecutive USB interrupt packets,
- * each packet carrying 8 bytes.
- */
-static ssize_t vfd_write(struct file *file, const char __user *buf,
-				size_t n_bytes, loff_t *pos)
-{
-	int i;
-	int retval = 0;
-	struct sasem_context *context;
-	int *data_buf = NULL;
-
-	context = (struct sasem_context *) file->private_data;
-	if (!context) {
-		pr_err("%s: no context for device\n", __func__);
-		return -ENODEV;
-	}
-
-	mutex_lock(&context->ctx_lock);
-
-	if (!context->dev_present) {
-		pr_err("%s: no Sasem device present\n", __func__);
-		retval = -ENODEV;
-		goto exit;
-	}
-
-	if (n_bytes <= 0 || n_bytes > SASEM_DATA_BUF_SZ) {
-		dev_err(&context->dev->dev, "%s: invalid payload size\n",
-			__func__);
-		retval = -EINVAL;
-		goto exit;
-	}
-
-	data_buf = memdup_user(buf, n_bytes);
-	if (IS_ERR(data_buf)) {
-		mutex_unlock(&context->ctx_lock);
-		return PTR_ERR(data_buf);
-	}
-
-	memcpy(context->tx.data_buf, data_buf, n_bytes);
-
-	/* Pad with spaces */
-	for (i = n_bytes; i < SASEM_DATA_BUF_SZ; ++i)
-		context->tx.data_buf[i] = ' ';
-
-	/* Nine 8 byte packets to be sent */
-	/* NOTE: "\x07\x01\0\0\0\0\0\0" or "\x0c\0\0\0\0\0\0\0"
-	 *       will clear the VFD
-	 */
-	for (i = 0; i < 9; i++) {
-		switch (i) {
-		case 0:
-			memcpy(context->usb_tx_buf, "\x07\0\0\0\0\0\0\0", 8);
-			context->usb_tx_buf[1] = (context->vfd_contrast) ?
-				(0x2B - (context->vfd_contrast - 1) / 250)
-				: 0x2B;
-			break;
-		case 1:
-			memcpy(context->usb_tx_buf, "\x09\x01\0\0\0\0\0\0", 8);
-			break;
-		case 2:
-			memcpy(context->usb_tx_buf, "\x0b\x01\0\0\0\0\0\0", 8);
-			break;
-		case 3:
-			memcpy(context->usb_tx_buf, context->tx.data_buf, 8);
-			break;
-		case 4:
-			memcpy(context->usb_tx_buf,
-			       context->tx.data_buf + 8, 8);
-			break;
-		case 5:
-			memcpy(context->usb_tx_buf, "\x09\x01\0\0\0\0\0\0", 8);
-			break;
-		case 6:
-			memcpy(context->usb_tx_buf, "\x0b\x02\0\0\0\0\0\0", 8);
-			break;
-		case 7:
-			memcpy(context->usb_tx_buf,
-			       context->tx.data_buf + 16, 8);
-			break;
-		case 8:
-			memcpy(context->usb_tx_buf,
-			       context->tx.data_buf + 24, 8);
-			break;
-		}
-		retval = send_packet(context);
-		if (retval) {
-			dev_err(&context->dev->dev,
-				"send packet failed for packet #%d\n", i);
-			goto exit;
-		}
-	}
-exit:
-
-	mutex_unlock(&context->ctx_lock);
-	kfree(data_buf);
-
-	return (!retval) ? n_bytes : retval;
-}
-
-/**
- * Callback function for USB core API: transmit data
- */
-static void usb_tx_callback(struct urb *urb)
-{
-	struct sasem_context *context;
-
-	if (!urb)
-		return;
-	context = (struct sasem_context *) urb->context;
-	if (!context)
-		return;
-
-	context->tx.status = urb->status;
-
-	/* notify waiters that write has finished */
-	atomic_set(&context->tx.busy, 0);
-	complete(&context->tx.finished);
-}
-
-/**
- * Called by lirc_dev when the application opens /dev/lirc
- */
-static int ir_open(void *data)
-{
-	int retval = 0;
-	struct sasem_context *context;
-
-	/* prevent races with disconnect */
-	mutex_lock(&disconnect_lock);
-
-	context = data;
-
-	mutex_lock(&context->ctx_lock);
-
-	if (context->ir_isopen) {
-		dev_err(&context->dev->dev, "%s: IR port is already open\n",
-			__func__);
-		retval = -EBUSY;
-		goto exit;
-	}
-
-	usb_fill_int_urb(context->rx_urb, context->dev,
-		usb_rcvintpipe(context->dev,
-				context->rx_endpoint->bEndpointAddress),
-		context->usb_rx_buf, sizeof(context->usb_rx_buf),
-		usb_rx_callback, context, context->rx_endpoint->bInterval);
-
-	retval = usb_submit_urb(context->rx_urb, GFP_KERNEL);
-
-	if (retval)
-		dev_err(&context->dev->dev,
-			"usb_submit_urb failed for ir_open (%d)\n", retval);
-	else {
-		context->ir_isopen = 1;
-		dev_info(&context->dev->dev, "IR port opened\n");
-	}
-
-exit:
-	mutex_unlock(&context->ctx_lock);
-
-	mutex_unlock(&disconnect_lock);
-	return retval;
-}
-
-/**
- * Called by lirc_dev when the application closes /dev/lirc
- */
-static void ir_close(void *data)
-{
-	struct sasem_context *context;
-
-	context = data;
-	if (!context) {
-		pr_err("%s: no context for device\n", __func__);
-		return;
-	}
-
-	mutex_lock(&context->ctx_lock);
-
-	usb_kill_urb(context->rx_urb);
-	context->ir_isopen = 0;
-	pr_info("IR port closed\n");
-
-	if (!context->dev_present) {
-
-		/*
-		 * Device disconnected while IR port was
-		 * still open. Driver was not deregistered
-		 * at disconnect time, so do it now.
-		 */
-		deregister_from_lirc(context);
-		if (!context->vfd_isopen) {
-			mutex_unlock(&context->ctx_lock);
-			delete_context(context);
-			return;
-		}
-		/* If VFD port is open, context will be deleted by vfd_close */
-	}
-
-	mutex_unlock(&context->ctx_lock);
-}
-
-/**
- * Process the incoming packet
- */
-static void incoming_packet(struct sasem_context *context,
-				   struct urb *urb)
-{
-	int len = urb->actual_length;
-	unsigned char *buf = urb->transfer_buffer;
-	u64 ns;
-	ktime_t kt;
-
-	if (len != 8) {
-		dev_warn(&context->dev->dev,
-			 "%s: invalid incoming packet size (%d)\n",
-			 __func__, len);
-		return;
-	}
-
-	if (debug)
-		dev_info(&context->dev->dev, "Incoming data: %*ph\n", len, buf);
-	/*
-	 * Lirc could deal with the repeat code, but we really need to block it
-	 * if it arrives too late.  Otherwise we could repeat the wrong code.
-	 */
-
-	/* get the time since the last button press */
-	kt = ktime_get();
-	ns = ktime_to_ns(ktime_sub(kt, context->presstime));
-
-	if (memcmp(buf, "\x08\0\0\0\0\0\0\0", 8) == 0) {
-		/*
-		 * the repeat code is being sent, so we copy
-		 * the old code to LIRC
-		 */
-
-		/*
-		 * NOTE: Only if the last code was less than 250ms ago
-		 * - no one should be able to push another (undetected) button
-		 *   in that time and then get a false repeat of the previous
-		 *   press but it is long enough for a genuine repeat
-		 */
-		if ((ns < 250 * NSEC_PER_MSEC) && (context->codesaved != 0)) {
-			memcpy(buf, &context->lastcode, 8);
-			context->presstime = kt;
-		}
-	} else {
-		/* save the current valid code for repeats */
-		memcpy(&context->lastcode, buf, 8);
-		/*
-		 * set flag to signal a valid code was save;
-		 * just for safety reasons
-		 */
-		context->codesaved = 1;
-		context->presstime = kt;
-	}
-
-	lirc_buffer_write(context->driver->rbuf, buf);
-	wake_up(&context->driver->rbuf->wait_poll);
-}
-
-/**
- * Callback function for USB core API: receive data
- */
-static void usb_rx_callback(struct urb *urb)
-{
-	struct sasem_context *context;
-
-	if (!urb)
-		return;
-	context = (struct sasem_context *) urb->context;
-	if (!context)
-		return;
-
-	switch (urb->status) {
-	case -ENOENT:		/* usbcore unlink successful! */
-		return;
-
-	case 0:
-		if (context->ir_isopen)
-			incoming_packet(context, urb);
-		break;
-
-	default:
-		dev_warn(&urb->dev->dev, "%s: status (%d): ignored",
-			 __func__, urb->status);
-		break;
-	}
-
-	usb_submit_urb(context->rx_urb, GFP_ATOMIC);
-}
-
-/**
- * Callback function for USB core API: Probe
- */
-static int sasem_probe(struct usb_interface *interface,
-			const struct usb_device_id *id)
-{
-	struct usb_device *dev = NULL;
-	struct usb_host_interface *iface_desc = NULL;
-	struct usb_endpoint_descriptor *rx_endpoint = NULL;
-	struct usb_endpoint_descriptor *tx_endpoint = NULL;
-	struct urb *rx_urb = NULL;
-	struct urb *tx_urb = NULL;
-	struct lirc_driver *driver = NULL;
-	struct lirc_buffer *rbuf = NULL;
-	int lirc_minor = 0;
-	int num_endpoints;
-	int retval = 0;
-	int vfd_ep_found;
-	int ir_ep_found;
-	int alloc_status;
-	struct sasem_context *context = NULL;
-	int i;
-
-	dev_info(&interface->dev, "%s: found Sasem device\n", __func__);
-
-
-	dev = usb_get_dev(interface_to_usbdev(interface));
-	iface_desc = interface->cur_altsetting;
-	num_endpoints = iface_desc->desc.bNumEndpoints;
-
-	/*
-	 * Scan the endpoint list and set:
-	 *	first input endpoint = IR endpoint
-	 *	first output endpoint = VFD endpoint
-	 */
-
-	ir_ep_found = 0;
-	vfd_ep_found = 0;
-
-	for (i = 0; i < num_endpoints && !(ir_ep_found && vfd_ep_found); ++i) {
-
-		struct usb_endpoint_descriptor *ep;
-
-		ep = &iface_desc->endpoint [i].desc;
-
-		if (!ir_ep_found &&
-			usb_endpoint_is_int_in(ep)) {
-
-			rx_endpoint = ep;
-			ir_ep_found = 1;
-			if (debug)
-				dev_info(&interface->dev,
-					"%s: found IR endpoint\n", __func__);
-
-		} else if (!vfd_ep_found &&
-			usb_endpoint_is_int_out(ep)) {
-			tx_endpoint = ep;
-			vfd_ep_found = 1;
-			if (debug)
-				dev_info(&interface->dev,
-					"%s: found VFD endpoint\n", __func__);
-		}
-	}
-
-	/* Input endpoint is mandatory */
-	if (!ir_ep_found) {
-		dev_err(&interface->dev,
-			"%s: no valid input (IR) endpoint found.\n", __func__);
-		retval = -ENODEV;
-		goto exit;
-	}
-
-	if (!vfd_ep_found)
-		dev_info(&interface->dev,
-			"%s: no valid output (VFD) endpoint found.\n",
-			__func__);
-
-
-	/* Allocate memory */
-	alloc_status = 0;
-
-	context = kzalloc(sizeof(*context), GFP_KERNEL);
-	if (!context) {
-		alloc_status = 1;
-		goto alloc_status_switch;
-	}
-	driver = kzalloc(sizeof(*driver), GFP_KERNEL);
-	if (!driver) {
-		alloc_status = 2;
-		goto alloc_status_switch;
-	}
-	rbuf = kmalloc(sizeof(*rbuf), GFP_KERNEL);
-	if (!rbuf) {
-		alloc_status = 3;
-		goto alloc_status_switch;
-	}
-	if (lirc_buffer_init(rbuf, BUF_CHUNK_SIZE, BUF_SIZE)) {
-		dev_err(&interface->dev,
-			"%s: lirc_buffer_init failed\n", __func__);
-		alloc_status = 4;
-		goto alloc_status_switch;
-	}
-	rx_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!rx_urb) {
-		alloc_status = 5;
-		goto alloc_status_switch;
-	}
-	if (vfd_ep_found) {
-		tx_urb = usb_alloc_urb(0, GFP_KERNEL);
-		if (!tx_urb) {
-			alloc_status = 6;
-			goto alloc_status_switch;
-		}
-	}
-
-	mutex_init(&context->ctx_lock);
-
-	strcpy(driver->name, MOD_NAME);
-	driver->minor = -1;
-	driver->code_length = 64;
-	driver->sample_rate = 0;
-	driver->features = LIRC_CAN_REC_LIRCCODE;
-	driver->data = context;
-	driver->rbuf = rbuf;
-	driver->set_use_inc = ir_open;
-	driver->set_use_dec = ir_close;
-	driver->dev   = &interface->dev;
-	driver->owner = THIS_MODULE;
-
-	mutex_lock(&context->ctx_lock);
-
-	lirc_minor = lirc_register_driver(driver);
-	if (lirc_minor < 0) {
-		dev_err(&interface->dev,
-			"%s: lirc_register_driver failed\n", __func__);
-		alloc_status = 7;
-		retval = lirc_minor;
-		goto unlock;
-	} else
-		dev_info(&interface->dev,
-			 "%s: Registered Sasem driver (minor:%d)\n",
-			 __func__, lirc_minor);
-
-	/* Needed while unregistering! */
-	driver->minor = lirc_minor;
-
-	context->dev = dev;
-	context->dev_present = 1;
-	context->rx_endpoint = rx_endpoint;
-	context->rx_urb = rx_urb;
-	if (vfd_ep_found) {
-		context->tx_endpoint = tx_endpoint;
-		context->tx_urb = tx_urb;
-		context->vfd_contrast = 1000;   /* range 0 - 1000 */
-	}
-	context->driver = driver;
-
-	usb_set_intfdata(interface, context);
-
-	if (vfd_ep_found) {
-
-		if (debug)
-			dev_info(&interface->dev,
-				 "Registering VFD with sysfs\n");
-		if (usb_register_dev(interface, &sasem_class))
-			/* Not a fatal error, so ignore */
-			dev_info(&interface->dev,
-				 "%s: could not get a minor number for VFD\n",
-				 __func__);
-	}
-
-	dev_info(&interface->dev,
-		 "%s: Sasem device on usb<%d:%d> initialized\n",
-		 __func__, dev->bus->busnum, dev->devnum);
-unlock:
-	mutex_unlock(&context->ctx_lock);
-
-alloc_status_switch:
-	switch (alloc_status) {
-
-	case 7:
-		if (vfd_ep_found)
-			usb_free_urb(tx_urb);
-	case 6:
-		usb_free_urb(rx_urb);
-		/* fall-through */
-	case 5:
-		lirc_buffer_free(rbuf);
-		/* fall-through */
-	case 4:
-		kfree(rbuf);
-		/* fall-through */
-	case 3:
-		kfree(driver);
-		/* fall-through */
-	case 2:
-		kfree(context);
-		context = NULL;
-		/* fall-through */
-	case 1:
-		if (retval == 0)
-			retval = -ENOMEM;
-	}
-
-exit:
-	return retval;
-}
-
-/**
- * Callback function for USB core API: disconnect
- */
-static void sasem_disconnect(struct usb_interface *interface)
-{
-	struct sasem_context *context;
-
-	/* prevent races with ir_open()/vfd_open() */
-	mutex_lock(&disconnect_lock);
-
-	context = usb_get_intfdata(interface);
-	mutex_lock(&context->ctx_lock);
-
-	dev_info(&interface->dev, "%s: Sasem device disconnected\n",
-		 __func__);
-
-	usb_set_intfdata(interface, NULL);
-	context->dev_present = 0;
-
-	/* Stop reception */
-	usb_kill_urb(context->rx_urb);
-
-	/* Abort ongoing write */
-	if (atomic_read(&context->tx.busy)) {
-
-		usb_kill_urb(context->tx_urb);
-		wait_for_completion(&context->tx.finished);
-	}
-
-	/* De-register from lirc_dev if IR port is not open */
-	if (!context->ir_isopen)
-		deregister_from_lirc(context);
-
-	usb_deregister_dev(interface, &sasem_class);
-
-	mutex_unlock(&context->ctx_lock);
-
-	if (!context->ir_isopen && !context->vfd_isopen)
-		delete_context(context);
-
-	mutex_unlock(&disconnect_lock);
-}
-
-module_usb_driver(sasem_driver);
diff --git a/drivers/staging/media/lirc/lirc_sir.c b/drivers/staging/media/lirc/lirc_sir.c
deleted file mode 100644
index c6c3de94adaa..000000000000
--- a/drivers/staging/media/lirc/lirc_sir.c
+++ /dev/null
@@ -1,839 +0,0 @@
-/*
- * LIRC 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.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- *
- *
- * 2000/09/16 Frank Przybylski <mail@frankprzybylski.de> :
- *  added timeout and relaxed pulse detection, removed gap bug
- *
- * 2000/12/15 Christoph Bartelmus <lirc@bartelmus.de> :
- *   added support for Tekram Irmate 210 (sending does not work yet,
- *   kind of disappointing that nobody was able to implement that
- *   before),
- *   major clean-up
- *
- * 2001/02/27 Christoph Bartelmus <lirc@bartelmus.de> :
- *   added support for StrongARM SA1100 embedded microprocessor
- *   parts cut'n'pasted from sa1100_ir.c (C) 2000 Russell King
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/module.h>
-#include <linux/sched/signal.h>
-#include <linux/errno.h>
-#include <linux/signal.h>
-#include <linux/fs.h>
-#include <linux/interrupt.h>
-#include <linux/ioport.h>
-#include <linux/kernel.h>
-#include <linux/serial_reg.h>
-#include <linux/ktime.h>
-#include <linux/string.h>
-#include <linux/types.h>
-#include <linux/wait.h>
-#include <linux/mm.h>
-#include <linux/delay.h>
-#include <linux/poll.h>
-#include <linux/io.h>
-#include <asm/irq.h>
-#include <linux/fcntl.h>
-#include <linux/platform_device.h>
-
-#include <linux/timer.h>
-
-#include <media/rc-core.h>
-
-/* SECTION: Definitions */
-
-/*** Tekram dongle ***/
-#ifdef LIRC_SIR_TEKRAM
-/* stolen from kernel source */
-/* definitions for Tekram dongle */
-#define TEKRAM_115200 0x00
-#define TEKRAM_57600  0x01
-#define TEKRAM_38400  0x02
-#define TEKRAM_19200  0x03
-#define TEKRAM_9600   0x04
-#define TEKRAM_2400   0x08
-
-#define TEKRAM_PW 0x10 /* Pulse select bit */
-
-/* 10bit * 1s/115200bit in milliseconds = 87ms*/
-#define TIME_CONST (10000000ul/115200ul)
-
-#endif
-
-#ifdef LIRC_SIR_ACTISYS_ACT200L
-static void init_act200(void);
-#elif defined(LIRC_SIR_ACTISYS_ACT220L)
-static void init_act220(void);
-#endif
-
-#define PULSE '['
-
-#ifndef LIRC_SIR_TEKRAM
-/* 9bit * 1s/115200bit in milli seconds = 78.125ms*/
-#define TIME_CONST (9000000ul/115200ul)
-#endif
-
-
-/* timeout for sequences in jiffies (=5/100s), must be longer than TIME_CONST */
-#define SIR_TIMEOUT	(HZ*5/100)
-
-#ifndef LIRC_ON_SA1100
-#ifndef LIRC_IRQ
-#define LIRC_IRQ 4
-#endif
-#ifndef LIRC_PORT
-/* for external dongles, default to com1 */
-#if defined(LIRC_SIR_ACTISYS_ACT200L)         || \
-	    defined(LIRC_SIR_ACTISYS_ACT220L) || \
-	    defined(LIRC_SIR_TEKRAM)
-#define LIRC_PORT 0x3f8
-#else
-/* onboard sir ports are typically com3 */
-#define LIRC_PORT 0x3e8
-#endif
-#endif
-
-static int io = LIRC_PORT;
-static int irq = LIRC_IRQ;
-static int threshold = 3;
-#endif
-
-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);
-
-static bool debug;
-
-/* 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);
-}
-
-#ifndef MAX_UDELAY_MS
-#define MAX_UDELAY_US 5000
-#else
-#define MAX_UDELAY_US (MAX_UDELAY_MS*1000)
-#endif
-
-static void safe_udelay(unsigned long usecs)
-{
-	while (usecs > MAX_UDELAY_US) {
-		udelay(MAX_UDELAY_US);
-		usecs -= MAX_UDELAY_US;
-	}
-	udelay(usecs);
-}
-
-/* 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_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->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:
-			(void) inb(io + UART_LSR);
-			break;
-		case UART_IIR_THRI:
-#if 0
-			if (lsr & UART_LSR_THRE) /* FIFO is empty */
-				outb(data, io + UART_TX)
-#endif
-			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)
-{
-	safe_udelay(len);
-}
-
-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 */
-#if defined(LIRC_SIR_TEKRAM)
-	/* disable FIFO */
-	soutp(UART_FCR,
-	      UART_FCR_CLEAR_RCVR|
-	      UART_FCR_CLEAR_XMIT|
-	      UART_FCR_TRIGGER_1);
-
-	/* Set DLAB 0. */
-	soutp(UART_LCR, sinp(UART_LCR) & (~UART_LCR_DLAB));
-
-	/* First of all, disable all interrupts */
-	soutp(UART_IER, sinp(UART_IER) &
-	      (~(UART_IER_MSI|UART_IER_RLSI|UART_IER_THRI|UART_IER_RDI)));
-
-	/* Set DLAB 1. */
-	soutp(UART_LCR, sinp(UART_LCR) | UART_LCR_DLAB);
-
-	/* Set divisor to 12 => 9600 Baud */
-	soutp(UART_DLM, 0);
-	soutp(UART_DLL, 12);
-
-	/* Set DLAB 0. */
-	soutp(UART_LCR, sinp(UART_LCR) & (~UART_LCR_DLAB));
-
-	/* power supply */
-	soutp(UART_MCR, UART_MCR_RTS|UART_MCR_DTR|UART_MCR_OUT2);
-	safe_udelay(50*1000);
-
-	/* -DTR low -> reset PIC */
-	soutp(UART_MCR, UART_MCR_RTS|UART_MCR_OUT2);
-	udelay(1*1000);
-
-	soutp(UART_MCR, UART_MCR_RTS|UART_MCR_DTR|UART_MCR_OUT2);
-	udelay(100);
-
-
-	/* -RTS low -> send control byte */
-	soutp(UART_MCR, UART_MCR_DTR|UART_MCR_OUT2);
-	udelay(7);
-	soutp(UART_TX, TEKRAM_115200|TEKRAM_PW);
-
-	/* one byte takes ~1042 usec to transmit at 9600,8N1 */
-	udelay(1500);
-
-	/* back to normal operation */
-	soutp(UART_MCR, UART_MCR_RTS|UART_MCR_DTR|UART_MCR_OUT2);
-	udelay(50);
-
-	udelay(1500);
-
-	/* read previous control byte */
-	pr_info("0x%02x\n", sinp(UART_RX));
-
-	/* Set DLAB 1. */
-	soutp(UART_LCR, sinp(UART_LCR) | UART_LCR_DLAB);
-
-	/* Set divisor to 1 => 115200 Baud */
-	soutp(UART_DLM, 0);
-	soutp(UART_DLL, 1);
-
-	/* Set DLAB 0, 8 Bit */
-	soutp(UART_LCR, UART_LCR_WLEN8);
-	/* enable interrupts */
-	soutp(UART_IER, sinp(UART_IER)|UART_IER_RDI);
-#else
-	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);
-#ifdef LIRC_SIR_ACTISYS_ACT200L
-	init_act200();
-#elif defined(LIRC_SIR_ACTISYS_ACT220L)
-	init_act220();
-#endif
-#endif
-	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;
-
-	/* 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);
-
-	setup_timer(&timerlist, sir_timeout, 0);
-
-	return 0;
-}
-
-static void drop_port(void)
-{
-	free_irq(irq, NULL);
-	del_timer_sync(&timerlist);
-	release_region(io, 8);
-}
-
-#ifdef LIRC_SIR_ACTISYS_ACT200L
-/* Crystal/Cirrus CS8130 IR transceiver, used in Actisys Act200L dongle */
-/* some code borrowed from Linux IRDA driver */
-
-/* Register 0: Control register #1 */
-#define ACT200L_REG0    0x00
-#define ACT200L_TXEN    0x01 /* Enable transmitter */
-#define ACT200L_RXEN    0x02 /* Enable receiver */
-#define ACT200L_ECHO    0x08 /* Echo control chars */
-
-/* Register 1: Control register #2 */
-#define ACT200L_REG1    0x10
-#define ACT200L_LODB    0x01 /* Load new baud rate count value */
-#define ACT200L_WIDE    0x04 /* Expand the maximum allowable pulse */
-
-/* Register 3: Transmit mode register #2 */
-#define ACT200L_REG3    0x30
-#define ACT200L_B0      0x01 /* DataBits, 0=6, 1=7, 2=8, 3=9(8P)  */
-#define ACT200L_B1      0x02 /* DataBits, 0=6, 1=7, 2=8, 3=9(8P)  */
-#define ACT200L_CHSY    0x04 /* StartBit Synced 0=bittime, 1=startbit */
-
-/* Register 4: Output Power register */
-#define ACT200L_REG4    0x40
-#define ACT200L_OP0     0x01 /* Enable LED1C output */
-#define ACT200L_OP1     0x02 /* Enable LED2C output */
-#define ACT200L_BLKR    0x04
-
-/* Register 5: Receive Mode register */
-#define ACT200L_REG5    0x50
-#define ACT200L_RWIDL   0x01 /* fixed 1.6us pulse mode */
-    /*.. other various IRDA bit modes, and TV remote modes..*/
-
-/* Register 6: Receive Sensitivity register #1 */
-#define ACT200L_REG6    0x60
-#define ACT200L_RS0     0x01 /* receive threshold bit 0 */
-#define ACT200L_RS1     0x02 /* receive threshold bit 1 */
-
-/* Register 7: Receive Sensitivity register #2 */
-#define ACT200L_REG7    0x70
-#define ACT200L_ENPOS   0x04 /* Ignore the falling edge */
-
-/* Register 8,9: Baud Rate Divider register #1,#2 */
-#define ACT200L_REG8    0x80
-#define ACT200L_REG9    0x90
-
-#define ACT200L_2400    0x5f
-#define ACT200L_9600    0x17
-#define ACT200L_19200   0x0b
-#define ACT200L_38400   0x05
-#define ACT200L_57600   0x03
-#define ACT200L_115200  0x01
-
-/* Register 13: Control register #3 */
-#define ACT200L_REG13   0xd0
-#define ACT200L_SHDW    0x01 /* Enable access to shadow registers */
-
-/* Register 15: Status register */
-#define ACT200L_REG15   0xf0
-
-/* Register 21: Control register #4 */
-#define ACT200L_REG21   0x50
-#define ACT200L_EXCK    0x02 /* Disable clock output driver */
-#define ACT200L_OSCL    0x04 /* oscillator in low power, medium accuracy mode */
-
-static void init_act200(void)
-{
-	int i;
-	__u8 control[] = {
-		ACT200L_REG15,
-		ACT200L_REG13 | ACT200L_SHDW,
-		ACT200L_REG21 | ACT200L_EXCK | ACT200L_OSCL,
-		ACT200L_REG13,
-		ACT200L_REG7  | ACT200L_ENPOS,
-		ACT200L_REG6  | ACT200L_RS0  | ACT200L_RS1,
-		ACT200L_REG5  | ACT200L_RWIDL,
-		ACT200L_REG4  | ACT200L_OP0  | ACT200L_OP1 | ACT200L_BLKR,
-		ACT200L_REG3  | ACT200L_B0,
-		ACT200L_REG0  | ACT200L_TXEN | ACT200L_RXEN,
-		ACT200L_REG8 |  (ACT200L_115200       & 0x0f),
-		ACT200L_REG9 | ((ACT200L_115200 >> 4) & 0x0f),
-		ACT200L_REG1 | ACT200L_LODB | ACT200L_WIDE
-	};
-
-	/* Set DLAB 1. */
-	soutp(UART_LCR, UART_LCR_DLAB | UART_LCR_WLEN8);
-
-	/* Set divisor to 12 => 9600 Baud */
-	soutp(UART_DLM, 0);
-	soutp(UART_DLL, 12);
-
-	/* Set DLAB 0. */
-	soutp(UART_LCR, UART_LCR_WLEN8);
-	/* Set divisor to 12 => 9600 Baud */
-
-	/* power supply */
-	soutp(UART_MCR, UART_MCR_RTS|UART_MCR_DTR|UART_MCR_OUT2);
-	for (i = 0; i < 50; i++)
-		safe_udelay(1000);
-
-		/* Reset the dongle : set RTS low for 25 ms */
-	soutp(UART_MCR, UART_MCR_DTR|UART_MCR_OUT2);
-	for (i = 0; i < 25; i++)
-		udelay(1000);
-
-	soutp(UART_MCR, UART_MCR_RTS|UART_MCR_DTR|UART_MCR_OUT2);
-	udelay(100);
-
-	/* Clear DTR and set RTS to enter command mode */
-	soutp(UART_MCR, UART_MCR_RTS|UART_MCR_OUT2);
-	udelay(7);
-
-	/* send out the control register settings for 115K 7N1 SIR operation */
-	for (i = 0; i < sizeof(control); i++) {
-		soutp(UART_TX, control[i]);
-		/* one byte takes ~1042 usec to transmit at 9600,8N1 */
-		udelay(1500);
-	}
-
-	/* back to normal operation */
-	soutp(UART_MCR, UART_MCR_RTS|UART_MCR_DTR|UART_MCR_OUT2);
-	udelay(50);
-
-	udelay(1500);
-	soutp(UART_LCR, sinp(UART_LCR) | UART_LCR_DLAB);
-
-	/* Set DLAB 1. */
-	soutp(UART_LCR, UART_LCR_DLAB | UART_LCR_WLEN7);
-
-	/* Set divisor to 1 => 115200 Baud */
-	soutp(UART_DLM, 0);
-	soutp(UART_DLL, 1);
-
-	/* Set DLAB 0. */
-	soutp(UART_LCR, sinp(UART_LCR) & (~UART_LCR_DLAB));
-
-	/* Set DLAB 0, 7 Bit */
-	soutp(UART_LCR, UART_LCR_WLEN7);
-
-	/* enable interrupts */
-	soutp(UART_IER, sinp(UART_IER)|UART_IER_RDI);
-}
-#endif
-
-#ifdef LIRC_SIR_ACTISYS_ACT220L
-/*
- * Derived from linux IrDA driver (net/irda/actisys.c)
- * Drop me a mail for any kind of comment: maxx@spaceboyz.net
- */
-
-void init_act220(void)
-{
-	int i;
-
-	/* DLAB 1 */
-	soutp(UART_LCR, UART_LCR_DLAB|UART_LCR_WLEN7);
-
-	/* 9600 baud */
-	soutp(UART_DLM, 0);
-	soutp(UART_DLL, 12);
-
-	/* DLAB 0 */
-	soutp(UART_LCR, UART_LCR_WLEN7);
-
-	/* reset the dongle, set DTR low for 10us */
-	soutp(UART_MCR, UART_MCR_RTS|UART_MCR_OUT2);
-	udelay(10);
-
-	/* back to normal (still 9600) */
-	soutp(UART_MCR, UART_MCR_DTR|UART_MCR_RTS|UART_MCR_OUT2);
-
-	/*
-	 * send RTS pulses until we reach 115200
-	 * i hope this is really the same for act220l/act220l+
-	 */
-	for (i = 0; i < 3; i++) {
-		udelay(10);
-		/* set RTS low for 10 us */
-		soutp(UART_MCR, UART_MCR_DTR|UART_MCR_OUT2);
-		udelay(10);
-		/* set RTS high for 10 us */
-		soutp(UART_MCR, UART_MCR_RTS|UART_MCR_DTR|UART_MCR_OUT2);
-	}
-
-	/* back to normal operation */
-	udelay(1500); /* better safe than sorry ;) */
-
-	/* Set DLAB 1. */
-	soutp(UART_LCR, UART_LCR_DLAB | UART_LCR_WLEN7);
-
-	/* Set divisor to 1 => 115200 Baud */
-	soutp(UART_DLM, 0);
-	soutp(UART_DLL, 1);
-
-	/* Set DLAB 0, 7 Bit */
-	/* The dongle doesn't seem to have any problems with operation at 7N1 */
-	soutp(UART_LCR, UART_LCR_WLEN7);
-
-	/* enable interrupts */
-	soutp(UART_IER, UART_IER_RDI);
-}
-#endif
-
-static int init_sir_ir(void)
-{
-	int retval;
-
-	retval = init_port();
-	if (retval < 0)
-		return retval;
-	init_hardware();
-	pr_info("Installed.\n");
-	return 0;
-}
-
-static int sir_ir_probe(struct platform_device *dev)
-{
-	return 0;
-}
-
-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) {
-		pr_err("Platform driver register failed!\n");
-		return -ENODEV;
-	}
-
-	sir_ir_dev = platform_device_alloc("sir_ir", 0);
-	if (!sir_ir_dev) {
-		pr_err("Platform device alloc failed!\n");
-		retval = -ENOMEM;
-		goto pdev_alloc_fail;
-	}
-
-	retval = platform_device_add(sir_ir_dev);
-	if (retval) {
-		pr_err("Platform device add failed!\n");
-		retval = -ENODEV;
-		goto pdev_add_fail;
-	}
-
-	retval = init_chrdev();
-	if (retval < 0)
-		goto fail;
-
-	retval = init_sir_ir();
-	if (retval)
-		goto fail;
-
-	return 0;
-
-fail:
-	platform_device_del(sir_ir_dev);
-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);
-	pr_info("Uninstalled.\n");
-}
-
-module_init(sir_ir_init);
-module_exit(sir_ir_exit);
-
-#ifdef LIRC_SIR_TEKRAM
-MODULE_DESCRIPTION("Infrared receiver driver for Tekram Irmate 210");
-MODULE_AUTHOR("Christoph Bartelmus");
-#elif defined(LIRC_SIR_ACTISYS_ACT200L)
-MODULE_DESCRIPTION("LIRC driver for Actisys Act200L");
-MODULE_AUTHOR("Karl Bongers");
-#elif defined(LIRC_SIR_ACTISYS_ACT220L)
-MODULE_DESCRIPTION("LIRC driver for Actisys Act220L(+)");
-MODULE_AUTHOR("Jan Roemisch");
-#else
-MODULE_DESCRIPTION("Infrared receiver driver for SIR type serial ports");
-MODULE_AUTHOR("Milan Pikula");
-#endif
-MODULE_LICENSE("GPL");
-
-module_param(io, int, S_IRUGO);
-MODULE_PARM_DESC(io, "I/O address base (0x3f8 or 0x2f8)");
-
-module_param(irq, int, S_IRUGO);
-MODULE_PARM_DESC(irq, "Interrupt (4 or 3)");
-
-module_param(threshold, int, S_IRUGO);
-MODULE_PARM_DESC(threshold, "space detection threshold (3)");
-
-module_param(debug, bool, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(debug, "Enable debugging messages");
diff --git a/drivers/staging/media/lirc/lirc_zilog.c b/drivers/staging/media/lirc/lirc_zilog.c
index e4a533b6beb3..8ce1db04414a 100644
--- a/drivers/staging/media/lirc/lirc_zilog.c
+++ b/drivers/staging/media/lirc/lirc_zilog.c
@@ -1475,7 +1475,7 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	ir = get_ir_device_by_adapter(adap);
 	if (ir == NULL) {
 		ir = kzalloc(sizeof(struct IR), GFP_KERNEL);
-		if (ir == NULL) {
+		if (!ir) {
 			ret = -ENOMEM;
 			goto out_no_ir;
 		}
@@ -1515,7 +1515,7 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 
 		/* Set up a struct IR_tx instance */
 		tx = kzalloc(sizeof(struct IR_tx), GFP_KERNEL);
-		if (tx == NULL) {
+		if (!tx) {
 			ret = -ENOMEM;
 			goto out_put_xx;
 		}
@@ -1559,7 +1559,7 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 
 		/* Set up a struct IR_rx instance */
 		rx = kzalloc(sizeof(struct IR_rx), GFP_KERNEL);
-		if (rx == NULL) {
+		if (!rx) {
 			ret = -ENOMEM;
 			goto out_put_xx;
 		}
@@ -1597,7 +1597,7 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 			i2c_set_clientdata(client, NULL);
 			put_ir_rx(rx, true);
 			ir->l.features &= ~LIRC_CAN_REC_LIRCCODE;
-			goto out_put_xx;
+			goto out_put_tx;
 		}
 
 		/* Proceed only if the Tx client is also ready */
@@ -1637,6 +1637,7 @@ out_ok:
 out_put_xx:
 	if (rx != NULL)
 		put_ir_rx(rx, true);
+out_put_tx:
 	if (tx != NULL)
 		put_ir_tx(tx, true);
 out_put_ir:
diff --git a/drivers/staging/media/omap4iss/iss_csi2.c b/drivers/staging/media/omap4iss/iss_csi2.c
index f71d5f2f179f..f6acc541e8a2 100644
--- a/drivers/staging/media/omap4iss/iss_csi2.c
+++ b/drivers/staging/media/omap4iss/iss_csi2.c
@@ -1268,7 +1268,7 @@ static int csi2_init_entities(struct iss_csi2_device *csi2, const char *subname)
 	snprintf(name, sizeof(name), "CSI2%s", subname);
 	snprintf(sd->name, sizeof(sd->name), "OMAP4 ISS %s", name);
 
-	sd->grp_id = 1 << 16;	/* group ID for iss subdevs */
+	sd->grp_id = BIT(16);	/* group ID for iss subdevs */
 	v4l2_set_subdevdata(sd, csi2);
 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 
diff --git a/drivers/staging/media/omap4iss/iss_ipipe.c b/drivers/staging/media/omap4iss/iss_ipipe.c
index d38782e8e84c..d86ef8a031f2 100644
--- a/drivers/staging/media/omap4iss/iss_ipipe.c
+++ b/drivers/staging/media/omap4iss/iss_ipipe.c
@@ -508,7 +508,7 @@ static int ipipe_init_entities(struct iss_ipipe_device *ipipe)
 	v4l2_subdev_init(sd, &ipipe_v4l2_ops);
 	sd->internal_ops = &ipipe_v4l2_internal_ops;
 	strlcpy(sd->name, "OMAP4 ISS ISP IPIPE", sizeof(sd->name));
-	sd->grp_id = 1 << 16;	/* group ID for iss subdevs */
+	sd->grp_id = BIT(16);	/* group ID for iss subdevs */
 	v4l2_set_subdevdata(sd, ipipe);
 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 
diff --git a/drivers/staging/media/omap4iss/iss_ipipeif.c b/drivers/staging/media/omap4iss/iss_ipipeif.c
index 23de8330731d..cb88b2bd0d82 100644
--- a/drivers/staging/media/omap4iss/iss_ipipeif.c
+++ b/drivers/staging/media/omap4iss/iss_ipipeif.c
@@ -739,7 +739,7 @@ static int ipipeif_init_entities(struct iss_ipipeif_device *ipipeif)
 	v4l2_subdev_init(sd, &ipipeif_v4l2_ops);
 	sd->internal_ops = &ipipeif_v4l2_internal_ops;
 	strlcpy(sd->name, "OMAP4 ISS ISP IPIPEIF", sizeof(sd->name));
-	sd->grp_id = 1 << 16;	/* group ID for iss subdevs */
+	sd->grp_id = BIT(16);	/* group ID for iss subdevs */
 	v4l2_set_subdevdata(sd, ipipeif);
 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 
diff --git a/drivers/staging/media/omap4iss/iss_resizer.c b/drivers/staging/media/omap4iss/iss_resizer.c
index f1d352c711d5..4bbfa20b3c38 100644
--- a/drivers/staging/media/omap4iss/iss_resizer.c
+++ b/drivers/staging/media/omap4iss/iss_resizer.c
@@ -782,7 +782,7 @@ static int resizer_init_entities(struct iss_resizer_device *resizer)
 	v4l2_subdev_init(sd, &resizer_v4l2_ops);
 	sd->internal_ops = &resizer_v4l2_internal_ops;
 	strlcpy(sd->name, "OMAP4 ISS ISP resizer", sizeof(sd->name));
-	sd->grp_id = 1 << 16;	/* group ID for iss subdevs */
+	sd->grp_id = BIT(16);	/* group ID for iss subdevs */
 	v4l2_set_subdevdata(sd, resizer);
 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 
diff --git a/drivers/staging/media/omap4iss/iss_video.c b/drivers/staging/media/omap4iss/iss_video.c
index bb0e3b4a4558..0bac58241a22 100644
--- a/drivers/staging/media/omap4iss/iss_video.c
+++ b/drivers/staging/media/omap4iss/iss_video.c
@@ -128,7 +128,8 @@ static unsigned int iss_video_mbus_to_pix(const struct iss_video *video,
 	pix->width = mbus->width;
 	pix->height = mbus->height;
 
-	/* Skip the last format in the loop so that it will be selected if no
+	/*
+	 * Skip the last format in the loop so that it will be selected if no
 	 * match is found.
 	 */
 	for (i = 0; i < ARRAY_SIZE(formats) - 1; ++i) {
@@ -138,7 +139,8 @@ static unsigned int iss_video_mbus_to_pix(const struct iss_video *video,
 
 	min_bpl = pix->width * ALIGN(formats[i].bpp, 8) / 8;
 
-	/* Clamp the requested bytes per line value. If the maximum bytes per
+	/*
+	 * Clamp the requested bytes per line value. If the maximum bytes per
 	 * line value is zero, the module doesn't support user configurable line
 	 * sizes. Override the requested value with the minimum in that case.
 	 */
@@ -172,7 +174,8 @@ static void iss_video_pix_to_mbus(const struct v4l2_pix_format *pix,
 	mbus->width = pix->width;
 	mbus->height = pix->height;
 
-	/* Skip the last format in the loop so that it will be selected if no
+	/*
+	 * Skip the last format in the loop so that it will be selected if no
 	 * match is found.
 	 */
 	for (i = 0; i < ARRAY_SIZE(formats) - 1; ++i) {
@@ -298,7 +301,8 @@ iss_video_check_format(struct iss_video *video, struct iss_video_fh *vfh)
 
 static int iss_video_queue_setup(struct vb2_queue *vq,
 				 unsigned int *count, unsigned int *num_planes,
-				 unsigned int sizes[], struct device *alloc_devs[])
+				 unsigned int sizes[],
+				 struct device *alloc_devs[])
 {
 	struct iss_video_fh *vfh = vb2_get_drv_priv(vq);
 	struct iss_video *video = vfh->video;
@@ -360,7 +364,8 @@ static void iss_video_buf_queue(struct vb2_buffer *vb)
 
 	spin_lock_irqsave(&video->qlock, flags);
 
-	/* Mark the buffer is faulty and give it back to the queue immediately
+	/*
+	 * Mark the buffer is faulty and give it back to the queue immediately
 	 * if the video node has registered an error. vb2 will perform the same
 	 * check when preparing the buffer, but that is inherently racy, so we
 	 * need to handle the race condition with an authoritative check here.
@@ -443,7 +448,8 @@ struct iss_buffer *omap4iss_video_buffer_next(struct iss_video *video)
 
 	buf->vb.vb2_buf.timestamp = ktime_get_ns();
 
-	/* Do frame number propagation only if this is the output video node.
+	/*
+	 * Do frame number propagation only if this is the output video node.
 	 * Frame number either comes from the CSI receivers or it gets
 	 * incremented here if H3A is not active.
 	 * Note: There is no guarantee that the output buffer will finish
@@ -605,7 +611,8 @@ iss_video_set_format(struct file *file, void *fh, struct v4l2_format *format)
 
 	mutex_lock(&video->mutex);
 
-	/* Fill the bytesperline and sizeimage fields by converting to media bus
+	/*
+	 * Fill the bytesperline and sizeimage fields by converting to media bus
 	 * format and back to pixel format.
 	 */
 	iss_video_pix_to_mbus(&format->fmt.pix, &fmt);
@@ -678,8 +685,9 @@ iss_video_get_selection(struct file *file, void *fh, struct v4l2_selection *sel)
 	if (subdev == NULL)
 		return -EINVAL;
 
-	/* Try the get selection operation first and fallback to get format if not
-	 * implemented.
+	/*
+	 * Try the get selection operation first and fallback to get format if
+	 * not implemented.
 	 */
 	sdsel.pad = pad;
 	ret = v4l2_subdev_call(subdev, pad, get_selection, NULL, &sdsel);
@@ -867,7 +875,8 @@ iss_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 
 	mutex_lock(&video->stream_lock);
 
-	/* Start streaming on the pipeline. No link touching an entity in the
+	/*
+	 * Start streaming on the pipeline. No link touching an entity in the
 	 * pipeline can be activated or deactivated once streaming is started.
 	 */
 	pipe = entity->pipe
@@ -895,7 +904,8 @@ iss_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 	while ((entity = media_graph_walk_next(&graph)))
 		media_entity_enum_set(&pipe->ent_enum, entity);
 
-	/* Verify that the currently configured format matches the output of
+	/*
+	 * Verify that the currently configured format matches the output of
 	 * the connected subdev.
 	 */
 	ret = iss_video_check_format(video, vfh);
@@ -905,7 +915,8 @@ iss_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 	video->bpl_padding = ret;
 	video->bpl_value = vfh->format.fmt.pix.bytesperline;
 
-	/* Find the ISS video node connected at the far end of the pipeline and
+	/*
+	 * Find the ISS video node connected at the far end of the pipeline and
 	 * update the pipeline.
 	 */
 	far_end = iss_video_far_end(video);
@@ -930,7 +941,8 @@ iss_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 	pipe->state |= state;
 	spin_unlock_irqrestore(&pipe->lock, flags);
 
-	/* Set the maximum time per frame as the value requested by userspace.
+	/*
+	 * Set the maximum time per frame as the value requested by userspace.
 	 * This is a soft limit that can be overridden if the hardware doesn't
 	 * support the request limit.
 	 */
@@ -946,7 +958,8 @@ iss_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 	if (ret < 0)
 		goto err_iss_video_check_format;
 
-	/* In sensor-to-memory mode, the stream can be started synchronously
+	/*
+	 * In sensor-to-memory mode, the stream can be started synchronously
 	 * to the stream on command. In memory-to-memory mode, it will be
 	 * started when buffers are queued on both the input and output.
 	 */
diff --git a/drivers/staging/media/s5p-cec/Kconfig b/drivers/staging/media/s5p-cec/Kconfig
deleted file mode 100644
index 7a3489df3e70..000000000000
--- a/drivers/staging/media/s5p-cec/Kconfig
+++ /dev/null
@@ -1,9 +0,0 @@
-config VIDEO_SAMSUNG_S5P_CEC
-       tristate "Samsung S5P CEC driver"
-       depends on VIDEO_DEV && MEDIA_CEC_SUPPORT && (ARCH_EXYNOS || COMPILE_TEST)
-       ---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.
-
diff --git a/drivers/staging/media/s5p-cec/TODO b/drivers/staging/media/s5p-cec/TODO
deleted file mode 100644
index 64f21bab38f5..000000000000
--- a/drivers/staging/media/s5p-cec/TODO
+++ /dev/null
@@ -1,7 +0,0 @@
-This driver requires that userspace sets the physical address.
-However, this should be passed on from the corresponding
-Samsung HDMI driver.
-
-We have to wait until the HDMI notifier framework has been merged
-in order to handle this gracefully, until that time this driver
-has to remain in staging.
diff --git a/drivers/staging/media/st-cec/Kconfig b/drivers/staging/media/st-cec/Kconfig
deleted file mode 100644
index c04283db58d6..000000000000
--- a/drivers/staging/media/st-cec/Kconfig
+++ /dev/null
@@ -1,8 +0,0 @@
-config VIDEO_STI_HDMI_CEC
-       tristate "STMicroelectronics STiH4xx HDMI CEC driver"
-       depends on VIDEO_DEV && MEDIA_CEC_SUPPORT && (ARCH_STI || COMPILE_TEST)
-       ---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.
diff --git a/drivers/staging/media/st-cec/TODO b/drivers/staging/media/st-cec/TODO
deleted file mode 100644
index c61289742c5c..000000000000
--- a/drivers/staging/media/st-cec/TODO
+++ /dev/null
@@ -1,7 +0,0 @@
-This driver requires that userspace sets the physical address.
-However, this should be passed on from the corresponding
-ST HDMI driver.
-
-We have to wait until the HDMI notifier framework has been merged
-in order to handle this gracefully, until that time this driver
-has to remain in staging.
author	Linus Torvalds <torvalds@linux-foundation.org>	2017-05-06 03:34:57 +0300
committer	Linus Torvalds <torvalds@linux-foundation.org>	2017-05-06 03:34:57 +0300
commit	e87d51ac61f88ae44fe14b34abe08566032d726b (patch)
tree	fc418d2e29fbf8a06f1ed0b6eaff8ba03e0543d7 /drivers
parent	bdc713bf5674bc6a881bd05c85e2a0f811b409b3 (diff)
parent	3622d3e77ecef090b5111e3c5423313f11711dfa (diff)
download	linux-e87d51ac61f88ae44fe14b34abe08566032d726b.tar.xz