Merge branch 'v4l_for_linus' of git://git.kernel.org/pub/scm/linux/kernel/git/mchehab/linux-2.6

* 'v4l_for_linus' of git://git.kernel.org/pub/scm/linux/kernel/git/mchehab/linux-2.6: (430 commits)
  [media] ir-mce_kbd-decoder: include module.h for its facilities
  [media] ov5642: include module.h for its facilities
  [media] em28xx: Fix DVB-C maxsize for em2884
  [media] tda18271c2dd: Fix saw filter configuration for DVB-C @6MHz
  [media] v4l: mt9v032: Fix Bayer pattern
  [media] V4L: mt9m111: rewrite set_pixfmt
  [media] V4L: mt9m111: fix missing return value check mt9m111_reg_clear
  [media] V4L: initial driver for ov5642 CMOS sensor
  [media] V4L: sh_mobile_ceu_camera: fix Oops when USERPTR mapping fails
  [media] V4L: soc-camera: remove soc-camera bus and devices on it
  [media] V4L: soc-camera: un-export the soc-camera bus
  [media] V4L: sh_mobile_csi2: switch away from using the soc-camera bus notifier
  [media] V4L: add media bus configuration subdev operations
  [media] V4L: soc-camera: group struct field initialisations together
  [media] V4L: soc-camera: remove now unused soc-camera specific PM hooks
  [media] V4L: pxa-camera: switch to using standard PM hooks
  [media] NetUP Dual DVB-T/C CI RF: force card hardware revision by module param
  [media] Don't OOPS if videobuf_dvb_get_frontend return NULL
  [media] NetUP Dual DVB-T/C CI RF: load firmware according card revision
  [media] omap3isp: Support configurable HS/VS polarities
  ...

Fix up conflicts:
 - arch/arm/mach-omap2/board-rx51-peripherals.c:
     cleanup regulator supply definitions in mach-omap2
   vs
     OMAP3: RX-51: define vdds_csib regulator supply
 - drivers/staging/tm6000/tm6000-alsa.c (trivial)

234 files changed, 29954 insertions, 7685 deletions

diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
index bb53de7fe40..f574dc012ca 100644
--- a/drivers/media/video/Kconfig
+++ b/drivers/media/video/Kconfig
@@ -489,6 +489,15 @@ config VIDEO_TCM825X
           This is a driver for the Toshiba TCM825x VGA camera sensor.
           It is used for example in Nokia N800.
 
+comment "Flash devices"
+
+config VIDEO_ADP1653
+        tristate "ADP1653 flash support"
+        depends on I2C && VIDEO_V4L2 && MEDIA_CONTROLLER
+        ---help---
+          This is a driver for the ADP1653 flash controller. It is used for
+          example in Nokia N900.
+
 comment "Video improvement chips"
 
 config VIDEO_UPD64031A
@@ -707,6 +716,8 @@ source "drivers/media/video/cx18/Kconfig"
 
 source "drivers/media/video/saa7164/Kconfig"
 
+source "drivers/media/video/marvell-ccic/Kconfig"
+
 config VIDEO_M32R_AR
         tristate "AR devices"
         depends on M32R && VIDEO_V4L2
@@ -726,15 +737,6 @@ config VIDEO_M32R_AR_M64278
           To compile this driver as a module, choose M here: the
           module will be called arv.
 
-config VIDEO_CAFE_CCIC
-        tristate "Marvell 88ALP01 (Cafe) CMOS Camera Controller support"
-        depends on PCI && I2C && VIDEO_V4L2
-        select VIDEO_OV7670
-        ---help---
-          This is a video4linux2 driver for the Marvell 88ALP01 integrated
-          CMOS camera controller.  This is the controller found on first-
-          generation OLPC systems.
-
 config VIDEO_SR030PC30
         tristate "SR030PC30 VGA camera sensor support"
         depends on I2C && VIDEO_V4L2
@@ -846,6 +848,12 @@ config SOC_CAMERA_OV2640
         help
           This is a ov2640 camera driver
 
+config SOC_CAMERA_OV5642
+        tristate "ov5642 camera support"
+        depends on SOC_CAMERA && I2C
+        help
+          This is a V4L2 camera driver for the OmniVision OV5642 sensor
+
 config SOC_CAMERA_OV6650
         tristate "ov6650 sensor support"
         depends on SOC_CAMERA && I2C
@@ -952,6 +960,14 @@ config  VIDEO_SAMSUNG_S5P_FIMC
           To compile this driver as a module, choose M here: the
           module will be called s5p-fimc.
 
+config VIDEO_ATMEL_ISI
+        tristate "ATMEL Image Sensor Interface (ISI) support"
+        depends on VIDEO_DEV && SOC_CAMERA && ARCH_AT91
+        select VIDEOBUF2_DMA_CONTIG
+        ---help---
+          This module makes the ATMEL Image Sensor Interface available
+          as a v4l2 device.
+
 config VIDEO_S5P_MIPI_CSIS
         tristate "Samsung S5P and EXYNOS4 MIPI CSI receiver driver"
         depends on VIDEO_V4L2 && PM_RUNTIME && PLAT_S5P && VIDEO_V4L2_SUBDEV_API
@@ -961,6 +977,8 @@ config VIDEO_S5P_MIPI_CSIS
           To compile this driver as a module, choose M here: the
           module will be called s5p-csis.
 
+source "drivers/media/video/s5p-tv/Kconfig"
+
 #
 # USB Multimedia device configuration
 #
@@ -1056,4 +1074,12 @@ config VIDEO_MEM2MEM_TESTDEV
           framework.
 
 
+config VIDEO_SAMSUNG_S5P_MFC
+        tristate "Samsung S5P MFC 5.1 Video Codec"
+        depends on VIDEO_DEV && VIDEO_V4L2 && PLAT_S5P
+        select VIDEOBUF2_DMA_CONTIG
+        default n
+        help
+            MFC 5.1 driver for V4L2.
+
 endif # V4L_MEM2MEM_DRIVERS
diff --git a/drivers/media/video/Makefile b/drivers/media/video/Makefile
index f0fecd6f6a3..272390072ae 100644
--- a/drivers/media/video/Makefile
+++ b/drivers/media/video/Makefile
@@ -70,6 +70,7 @@ obj-$(CONFIG_VIDEO_MT9V032) += mt9v032.o
 obj-$(CONFIG_VIDEO_SR030PC30)   += sr030pc30.o
 obj-$(CONFIG_VIDEO_NOON010PC30) += noon010pc30.o
 obj-$(CONFIG_VIDEO_M5MOLS)      += m5mols/
+obj-$(CONFIG_VIDEO_ADP1653)     += adp1653.o
 
 obj-$(CONFIG_SOC_CAMERA_IMX074)         += imx074.o
 obj-$(CONFIG_SOC_CAMERA_MT9M001)        += mt9m001.o
@@ -78,6 +79,7 @@ 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
 obj-$(CONFIG_SOC_CAMERA_OV9640)         += ov9640.o
@@ -127,7 +129,8 @@ obj-$(CONFIG_VIDEO_M32R_AR_M64278) += arv.o
 
 obj-$(CONFIG_VIDEO_CX2341X) += cx2341x.o
 
-obj-$(CONFIG_VIDEO_CAFE_CCIC) += cafe_ccic.o
+obj-$(CONFIG_VIDEO_CAFE_CCIC) += marvell-ccic/
+obj-$(CONFIG_VIDEO_MMP_CAMERA) += marvell-ccic/
 
 obj-$(CONFIG_VIDEO_VIA_CAMERA) += via-camera.o
 
@@ -166,8 +169,11 @@ obj-$(CONFIG_VIDEO_PXA27x)		+= pxa_camera.o
 obj-$(CONFIG_VIDEO_SH_MOBILE_CSI2)      += sh_mobile_csi2.o
 obj-$(CONFIG_VIDEO_SH_MOBILE_CEU)       += sh_mobile_ceu_camera.o
 obj-$(CONFIG_VIDEO_OMAP1)               += omap1_camera.o
+obj-$(CONFIG_VIDEO_ATMEL_ISI)           += atmel-isi.o
 
 obj-$(CONFIG_VIDEO_SAMSUNG_S5P_FIMC)    += s5p-fimc/
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_MFC)     += s5p-mfc/
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_TV)      += s5p-tv/
 
 obj-$(CONFIG_ARCH_DAVINCI)              += davinci/
 
diff --git a/drivers/media/video/adp1653.c b/drivers/media/video/adp1653.c
new file mode 100644
index 00000000000..be7befd6094
--- /dev/null
+++ b/drivers/media/video/adp1653.c
@@ -0,0 +1,491 @@
+/*
+ * drivers/media/video/adp1653.c
+ *
+ * Copyright (C) 2008--2011 Nokia Corporation
+ *
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * Contributors:
+ *      Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *      Tuukka Toivonen <tuukkat76@gmail.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.
+ *
+ * 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., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ * TODO:
+ * - fault interrupt handling
+ * - hardware strobe
+ * - power doesn't need to be ON if all lights are off
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/version.h>
+#include <media/adp1653.h>
+#include <media/v4l2-device.h>
+
+#define TIMEOUT_MAX             820000
+#define TIMEOUT_STEP            54600
+#define TIMEOUT_MIN             (TIMEOUT_MAX - ADP1653_REG_CONFIG_TMR_SET_MAX \
+                                 * TIMEOUT_STEP)
+#define TIMEOUT_US_TO_CODE(t)   ((TIMEOUT_MAX + (TIMEOUT_STEP / 2) - (t)) \
+                                 / TIMEOUT_STEP)
+#define TIMEOUT_CODE_TO_US(c)   (TIMEOUT_MAX - (c) * TIMEOUT_STEP)
+
+/* Write values into ADP1653 registers. */
+static int adp1653_update_hw(struct adp1653_flash *flash)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(&flash->subdev);
+        u8 out_sel;
+        u8 config = 0;
+        int rval;
+
+        out_sel = ADP1653_INDICATOR_INTENSITY_uA_TO_REG(
+                flash->indicator_intensity->val)
+                << ADP1653_REG_OUT_SEL_ILED_SHIFT;
+
+        switch (flash->led_mode->val) {
+        case V4L2_FLASH_LED_MODE_NONE:
+                break;
+        case V4L2_FLASH_LED_MODE_FLASH:
+                /* Flash mode, light on with strobe, duration from timer */
+                config = ADP1653_REG_CONFIG_TMR_CFG;
+                config |= TIMEOUT_US_TO_CODE(flash->flash_timeout->val)
+                          << ADP1653_REG_CONFIG_TMR_SET_SHIFT;
+                break;
+        case V4L2_FLASH_LED_MODE_TORCH:
+                /* Torch mode, light immediately on, duration indefinite */
+                out_sel |= ADP1653_FLASH_INTENSITY_mA_TO_REG(
+                        flash->torch_intensity->val)
+                        << ADP1653_REG_OUT_SEL_HPLED_SHIFT;
+                break;
+        }
+
+        rval = i2c_smbus_write_byte_data(client, ADP1653_REG_OUT_SEL, out_sel);
+        if (rval < 0)
+                return rval;
+
+        rval = i2c_smbus_write_byte_data(client, ADP1653_REG_CONFIG, config);
+        if (rval < 0)
+                return rval;
+
+        return 0;
+}
+
+static int adp1653_get_fault(struct adp1653_flash *flash)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(&flash->subdev);
+        int fault;
+        int rval;
+
+        fault = i2c_smbus_read_byte_data(client, ADP1653_REG_FAULT);
+        if (IS_ERR_VALUE(fault))
+                return fault;
+
+        flash->fault |= fault;
+
+        if (!flash->fault)
+                return 0;
+
+        /* Clear faults. */
+        rval = i2c_smbus_write_byte_data(client, ADP1653_REG_OUT_SEL, 0);
+        if (IS_ERR_VALUE(rval))
+                return rval;
+
+        flash->led_mode->val = V4L2_FLASH_LED_MODE_NONE;
+
+        rval = adp1653_update_hw(flash);
+        if (IS_ERR_VALUE(rval))
+                return rval;
+
+        return flash->fault;
+}
+
+static int adp1653_strobe(struct adp1653_flash *flash, int enable)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(&flash->subdev);
+        u8 out_sel = ADP1653_INDICATOR_INTENSITY_uA_TO_REG(
+                flash->indicator_intensity->val)
+                << ADP1653_REG_OUT_SEL_ILED_SHIFT;
+        int rval;
+
+        if (flash->led_mode->val != V4L2_FLASH_LED_MODE_FLASH)
+                return -EBUSY;
+
+        if (!enable)
+                return i2c_smbus_write_byte_data(client, ADP1653_REG_OUT_SEL,
+                                                 out_sel);
+
+        out_sel |= ADP1653_FLASH_INTENSITY_mA_TO_REG(
+                flash->flash_intensity->val)
+                << ADP1653_REG_OUT_SEL_HPLED_SHIFT;
+        rval = i2c_smbus_write_byte_data(client, ADP1653_REG_OUT_SEL, out_sel);
+        if (rval)
+                return rval;
+
+        /* Software strobe using i2c */
+        rval = i2c_smbus_write_byte_data(client, ADP1653_REG_SW_STROBE,
+                ADP1653_REG_SW_STROBE_SW_STROBE);
+        if (rval)
+                return rval;
+        return i2c_smbus_write_byte_data(client, ADP1653_REG_SW_STROBE, 0);
+}
+
+/* --------------------------------------------------------------------------
+ * V4L2 controls
+ */
+
+static int adp1653_get_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct adp1653_flash *flash =
+                container_of(ctrl->handler, struct adp1653_flash, ctrls);
+        int rval;
+
+        rval = adp1653_get_fault(flash);
+        if (IS_ERR_VALUE(rval))
+                return rval;
+
+        ctrl->cur.val = 0;
+
+        if (flash->fault & ADP1653_REG_FAULT_FLT_SCP)
+                ctrl->cur.val |= V4L2_FLASH_FAULT_SHORT_CIRCUIT;
+        if (flash->fault & ADP1653_REG_FAULT_FLT_OT)
+                ctrl->cur.val |= V4L2_FLASH_FAULT_OVER_TEMPERATURE;
+        if (flash->fault & ADP1653_REG_FAULT_FLT_TMR)
+                ctrl->cur.val |= V4L2_FLASH_FAULT_TIMEOUT;
+        if (flash->fault & ADP1653_REG_FAULT_FLT_OV)
+                ctrl->cur.val |= V4L2_FLASH_FAULT_OVER_VOLTAGE;
+
+        flash->fault = 0;
+
+        return 0;
+}
+
+static int adp1653_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct adp1653_flash *flash =
+                container_of(ctrl->handler, struct adp1653_flash, ctrls);
+        int rval;
+
+        rval = adp1653_get_fault(flash);
+        if (IS_ERR_VALUE(rval))
+                return rval;
+        if ((rval & (ADP1653_REG_FAULT_FLT_SCP |
+                     ADP1653_REG_FAULT_FLT_OT |
+                     ADP1653_REG_FAULT_FLT_OV)) &&
+            (ctrl->id == V4L2_CID_FLASH_STROBE ||
+             ctrl->id == V4L2_CID_FLASH_TORCH_INTENSITY ||
+             ctrl->id == V4L2_CID_FLASH_LED_MODE))
+                return -EBUSY;
+
+        switch (ctrl->id) {
+        case V4L2_CID_FLASH_STROBE:
+                return adp1653_strobe(flash, 1);
+        case V4L2_CID_FLASH_STROBE_STOP:
+                return adp1653_strobe(flash, 0);
+        }
+
+        return adp1653_update_hw(flash);
+}
+
+static const struct v4l2_ctrl_ops adp1653_ctrl_ops = {
+        .g_volatile_ctrl = adp1653_get_ctrl,
+        .s_ctrl = adp1653_set_ctrl,
+};
+
+static int adp1653_init_controls(struct adp1653_flash *flash)
+{
+        struct v4l2_ctrl *fault;
+
+        v4l2_ctrl_handler_init(&flash->ctrls, 9);
+
+        flash->led_mode =
+                v4l2_ctrl_new_std_menu(&flash->ctrls, &adp1653_ctrl_ops,
+                                       V4L2_CID_FLASH_LED_MODE,
+                                       V4L2_FLASH_LED_MODE_TORCH, ~0x7, 0);
+        v4l2_ctrl_new_std_menu(&flash->ctrls, &adp1653_ctrl_ops,
+                               V4L2_CID_FLASH_STROBE_SOURCE,
+                               V4L2_FLASH_STROBE_SOURCE_SOFTWARE, ~0x1, 0);
+        v4l2_ctrl_new_std(&flash->ctrls, &adp1653_ctrl_ops,
+                          V4L2_CID_FLASH_STROBE, 0, 0, 0, 0);
+        v4l2_ctrl_new_std(&flash->ctrls, &adp1653_ctrl_ops,
+                          V4L2_CID_FLASH_STROBE_STOP, 0, 0, 0, 0);
+        flash->flash_timeout =
+                v4l2_ctrl_new_std(&flash->ctrls, &adp1653_ctrl_ops,
+                                  V4L2_CID_FLASH_TIMEOUT, TIMEOUT_MIN,
+                                  flash->platform_data->max_flash_timeout,
+                                  TIMEOUT_STEP,
+                                  flash->platform_data->max_flash_timeout);
+        flash->flash_intensity =
+                v4l2_ctrl_new_std(&flash->ctrls, &adp1653_ctrl_ops,
+                                  V4L2_CID_FLASH_INTENSITY,
+                                  ADP1653_FLASH_INTENSITY_MIN,
+                                  flash->platform_data->max_flash_intensity,
+                                  1, flash->platform_data->max_flash_intensity);
+        flash->torch_intensity =
+                v4l2_ctrl_new_std(&flash->ctrls, &adp1653_ctrl_ops,
+                                  V4L2_CID_FLASH_TORCH_INTENSITY,
+                                  ADP1653_TORCH_INTENSITY_MIN,
+                                  flash->platform_data->max_torch_intensity,
+                                  ADP1653_FLASH_INTENSITY_STEP,
+                                  flash->platform_data->max_torch_intensity);
+        flash->indicator_intensity =
+                v4l2_ctrl_new_std(&flash->ctrls, &adp1653_ctrl_ops,
+                                  V4L2_CID_FLASH_INDICATOR_INTENSITY,
+                                  ADP1653_INDICATOR_INTENSITY_MIN,
+                                  flash->platform_data->max_indicator_intensity,
+                                  ADP1653_INDICATOR_INTENSITY_STEP,
+                                  ADP1653_INDICATOR_INTENSITY_MIN);
+        fault = v4l2_ctrl_new_std(&flash->ctrls, &adp1653_ctrl_ops,
+                                  V4L2_CID_FLASH_FAULT, 0,
+                                  V4L2_FLASH_FAULT_OVER_VOLTAGE
+                                  | V4L2_FLASH_FAULT_OVER_TEMPERATURE
+                                  | V4L2_FLASH_FAULT_SHORT_CIRCUIT, 0, 0);
+
+        if (flash->ctrls.error)
+                return flash->ctrls.error;
+
+        fault->is_volatile = 1;
+
+        flash->subdev.ctrl_handler = &flash->ctrls;
+        return 0;
+}
+
+/* --------------------------------------------------------------------------
+ * V4L2 subdev operations
+ */
+
+static int
+adp1653_init_device(struct adp1653_flash *flash)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(&flash->subdev);
+        int rval;
+
+        /* Clear FAULT register by writing zero to OUT_SEL */
+        rval = i2c_smbus_write_byte_data(client, ADP1653_REG_OUT_SEL, 0);
+        if (rval < 0) {
+                dev_err(&client->dev, "failed writing fault register\n");
+                return -EIO;
+        }
+
+        mutex_lock(&flash->ctrls.lock);
+        /* Reset faults before reading new ones. */
+        flash->fault = 0;
+        rval = adp1653_get_fault(flash);
+        mutex_unlock(&flash->ctrls.lock);
+        if (rval > 0) {
+                dev_err(&client->dev, "faults detected: 0x%1.1x\n", rval);
+                return -EIO;
+        }
+
+        mutex_lock(&flash->ctrls.lock);
+        rval = adp1653_update_hw(flash);
+        mutex_unlock(&flash->ctrls.lock);
+        if (rval) {
+                dev_err(&client->dev,
+                        "adp1653_update_hw failed at %s\n", __func__);
+                return -EIO;
+        }
+
+        return 0;
+}
+
+static int
+__adp1653_set_power(struct adp1653_flash *flash, int on)
+{
+        int ret;
+
+        ret = flash->platform_data->power(&flash->subdev, on);
+        if (ret < 0)
+                return ret;
+
+        if (!on)
+                return 0;
+
+        ret = adp1653_init_device(flash);
+        if (ret < 0)
+                flash->platform_data->power(&flash->subdev, 0);
+
+        return ret;
+}
+
+static int
+adp1653_set_power(struct v4l2_subdev *subdev, int on)
+{
+        struct adp1653_flash *flash = to_adp1653_flash(subdev);
+        int ret = 0;
+
+        mutex_lock(&flash->power_lock);
+
+        /* If the power count is modified from 0 to != 0 or from != 0 to 0,
+         * update the power state.
+         */
+        if (flash->power_count == !on) {
+                ret = __adp1653_set_power(flash, !!on);
+                if (ret < 0)
+                        goto done;
+        }
+
+        /* Update the power count. */
+        flash->power_count += on ? 1 : -1;
+        WARN_ON(flash->power_count < 0);
+
+done:
+        mutex_unlock(&flash->power_lock);
+        return ret;
+}
+
+static int adp1653_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+        return adp1653_set_power(sd, 1);
+}
+
+static int adp1653_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+        return adp1653_set_power(sd, 0);
+}
+
+static const struct v4l2_subdev_core_ops adp1653_core_ops = {
+        .s_power = adp1653_set_power,
+};
+
+static const struct v4l2_subdev_ops adp1653_ops = {
+        .core = &adp1653_core_ops,
+};
+
+static const struct v4l2_subdev_internal_ops adp1653_internal_ops = {
+        .open = adp1653_open,
+        .close = adp1653_close,
+};
+
+/* --------------------------------------------------------------------------
+ * I2C driver
+ */
+#ifdef CONFIG_PM
+
+static int adp1653_suspend(struct device *dev)
+{
+        struct i2c_client *client = to_i2c_client(dev);
+        struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+        struct adp1653_flash *flash = to_adp1653_flash(subdev);
+
+        if (!flash->power_count)
+                return 0;
+
+        return __adp1653_set_power(flash, 0);
+}
+
+static int adp1653_resume(struct device *dev)
+{
+        struct i2c_client *client = to_i2c_client(dev);
+        struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+        struct adp1653_flash *flash = to_adp1653_flash(subdev);
+
+        if (!flash->power_count)
+                return 0;
+
+        return __adp1653_set_power(flash, 1);
+}
+
+#else
+
+#define adp1653_suspend NULL
+#define adp1653_resume  NULL
+
+#endif /* CONFIG_PM */
+
+static int adp1653_probe(struct i2c_client *client,
+                         const struct i2c_device_id *devid)
+{
+        struct adp1653_flash *flash;
+        int ret;
+
+        flash = kzalloc(sizeof(*flash), GFP_KERNEL);
+        if (flash == NULL)
+                return -ENOMEM;
+
+        flash->platform_data = client->dev.platform_data;
+
+        mutex_init(&flash->power_lock);
+
+        v4l2_i2c_subdev_init(&flash->subdev, client, &adp1653_ops);
+        flash->subdev.internal_ops = &adp1653_internal_ops;
+        flash->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+        adp1653_init_controls(flash);
+
+        ret = media_entity_init(&flash->subdev.entity, 0, NULL, 0);
+        if (ret < 0)
+                kfree(flash);
+
+        return ret;
+}
+
+static int __exit adp1653_remove(struct i2c_client *client)
+{
+        struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+        struct adp1653_flash *flash = to_adp1653_flash(subdev);
+
+        v4l2_device_unregister_subdev(&flash->subdev);
+        v4l2_ctrl_handler_free(&flash->ctrls);
+        media_entity_cleanup(&flash->subdev.entity);
+        kfree(flash);
+        return 0;
+}
+
+static const struct i2c_device_id adp1653_id_table[] = {
+        { ADP1653_NAME, 0 },
+        { }
+};
+MODULE_DEVICE_TABLE(i2c, adp1653_id_table);
+
+static struct dev_pm_ops adp1653_pm_ops = {
+        .suspend        = adp1653_suspend,
+        .resume         = adp1653_resume,
+};
+
+static struct i2c_driver adp1653_i2c_driver = {
+        .driver         = {
+                .name   = ADP1653_NAME,
+                .pm     = &adp1653_pm_ops,
+        },
+        .probe          = adp1653_probe,
+        .remove         = __exit_p(adp1653_remove),
+        .id_table       = adp1653_id_table,
+};
+
+static int __init adp1653_init(void)
+{
+        int rval;
+
+        rval = i2c_add_driver(&adp1653_i2c_driver);
+        if (rval)
+                printk(KERN_ALERT "%s: failed at i2c_add_driver\n", __func__);
+
+        return rval;
+}
+
+static void __exit adp1653_exit(void)
+{
+        i2c_del_driver(&adp1653_i2c_driver);
+}
+
+module_init(adp1653_init);
+module_exit(adp1653_exit);
+
+MODULE_AUTHOR("Sakari Ailus <sakari.ailus@nokia.com>");
+MODULE_DESCRIPTION("Analog Devices ADP1653 LED flash driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/arv.c b/drivers/media/video/arv.c
index f989f2820d8..b6ed44aebe3 100644
--- a/drivers/media/video/arv.c
+++ b/drivers/media/video/arv.c
@@ -27,7 +27,6 @@
 #include <linux/slab.h>
 #include <linux/mm.h>
 #include <linux/sched.h>
-#include <linux/version.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
@@ -54,7 +53,7 @@
  */
 #define USE_INT         0       /* Don't modify */
 
-#define VERSION "0.04"
+#define VERSION "0.0.5"
 
 #define ar_inl(addr)            inl((unsigned long)(addr))
 #define ar_outl(val, addr)      outl((unsigned long)(val), (unsigned long)(addr))
@@ -404,7 +403,6 @@ static int ar_querycap(struct file *file, void  *priv,
         strlcpy(vcap->driver, ar->vdev.name, sizeof(vcap->driver));
         strlcpy(vcap->card, "Colour AR VGA", sizeof(vcap->card));
         strlcpy(vcap->bus_info, "Platform", sizeof(vcap->bus_info));
-        vcap->version = KERNEL_VERSION(0, 0, 4);
         vcap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
         return 0;
 }
@@ -879,3 +877,4 @@ module_exit(ar_cleanup_module);
 MODULE_AUTHOR("Takeo Takahashi <takahashi.takeo@renesas.com>");
 MODULE_DESCRIPTION("Colour AR M64278(VGA) for Video4Linux");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(VERSION);
diff --git a/drivers/media/video/atmel-isi.c b/drivers/media/video/atmel-isi.c
new file mode 100644
index 00000000000..7b89f00501b
--- /dev/null
+++ b/drivers/media/video/atmel-isi.c
@@ -0,0 +1,1048 @@
+/*
+ * 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/slab.h>
+
+#include <media/atmel-isi.h>
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
+#include <media/videobuf2-dma-contig.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)
+
+/* ISI states */
+enum {
+        ISI_STATE_IDLE = 0,
+        ISI_STATE_READY,
+        ISI_STATE_WAIT_SOF,
+};
+
+/* 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;
+        u32 fbd_phys;
+};
+
+/* Frame buffer data */
+struct frame_buffer {
+        struct vb2_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;
+        /* State of the ISI module in capturing mode */
+        int                             state;
+
+        /* Wait queue for waiting for SOF */
+        wait_queue_head_t               vsync_wq;
+
+        struct vb2_alloc_ctx            *alloc_ctx;
+
+        /* Allocate descriptors for dma buffer use */
+        struct fbd                      *p_fb_descriptors;
+        u32                             fb_descriptors_phys;
+        struct                          list_head dma_desc_head;
+        struct isi_dma_desc             dma_desc[MAX_BUFFER_NUM];
+
+        struct completion               complete;
+        struct clk                      *pclk;
+        unsigned int                    irq;
+
+        struct isi_platform_data        *pdata;
+
+        struct list_head                video_buffer_list;
+        struct frame_buffer             *active;
+
+        struct soc_camera_device        *icd;
+        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 int configure_geometry(struct atmel_isi *isi, u32 width,
+                        u32 height, enum v4l2_mbus_pixelcode code)
+{
+        u32 cfg2, cr;
+
+        switch (code) {
+        /* YUV, including grey */
+        case V4L2_MBUS_FMT_Y8_1X8:
+                cr = ISI_CFG2_GRAYSCALE;
+                break;
+        case V4L2_MBUS_FMT_UYVY8_2X8:
+                cr = ISI_CFG2_YCC_SWAP_MODE_3;
+                break;
+        case V4L2_MBUS_FMT_VYUY8_2X8:
+                cr = ISI_CFG2_YCC_SWAP_MODE_2;
+                break;
+        case V4L2_MBUS_FMT_YUYV8_2X8:
+                cr = ISI_CFG2_YCC_SWAP_MODE_1;
+                break;
+        case V4L2_MBUS_FMT_YVYU8_2X8:
+                cr = ISI_CFG2_YCC_SWAP_DEFAULT;
+                break;
+        /* RGB, TODO */
+        default:
+                return -EINVAL;
+        }
+
+        isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
+
+        cfg2 = isi_readl(isi, ISI_CFG2);
+        cfg2 |= cr;
+        /* Set width */
+        cfg2 &= ~(ISI_CFG2_IM_HSIZE_MASK);
+        cfg2 |= ((width - 1) << ISI_CFG2_IM_HSIZE_OFFSET) &
+                        ISI_CFG2_IM_HSIZE_MASK;
+        /* Set height */
+        cfg2 &= ~(ISI_CFG2_IM_VSIZE_MASK);
+        cfg2 |= ((height - 1) << ISI_CFG2_IM_VSIZE_OFFSET)
+                        & ISI_CFG2_IM_VSIZE_MASK;
+        isi_writel(isi, ISI_CFG2, cfg2);
+
+        return 0;
+}
+
+static irqreturn_t atmel_isi_handle_streaming(struct atmel_isi *isi)
+{
+        if (isi->active) {
+                struct vb2_buffer *vb = &isi->active->vb;
+                struct frame_buffer *buf = isi->active;
+
+                list_del_init(&buf->list);
+                do_gettimeofday(&vb->v4l2_buf.timestamp);
+                vb->v4l2_buf.sequence = isi->sequence++;
+                vb2_buffer_done(vb, 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);
+                isi_writel(isi, ISI_DMA_C_DSCR,
+                        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);
+        }
+        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 ((pending & ISI_SR_VSYNC) &&
+                                (isi->state == ISI_STATE_IDLE)) {
+                        isi->state = ISI_STATE_READY;
+                        wake_up_interruptible(&isi->vsync_wq);
+                        ret = IRQ_HANDLED;
+                }
+                if (likely(pending & ISI_SR_CXFR_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(100));
+        if (timeout == 0)
+                return -ETIMEDOUT;
+
+        return 0;
+}
+
+/* ------------------------------------------------------------------
+        Videobuf operations
+   ------------------------------------------------------------------*/
+static int queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+                                unsigned int *nplanes, unsigned long sizes[],
+                                void *alloc_ctxs[])
+{
+        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;
+        int ret, bytes_per_line;
+
+        /* Reset ISI */
+        ret = atmel_isi_wait_status(isi, WAIT_ISI_RESET);
+        if (ret < 0) {
+                dev_err(icd->parent, "Reset ISI timed out\n");
+                return ret;
+        }
+        /* Disable all interrupts */
+        isi_writel(isi, ISI_INTDIS, ~0UL);
+
+        bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+                                                icd->current_fmt->host_fmt);
+
+        if (bytes_per_line < 0)
+                return bytes_per_line;
+
+        size = bytes_per_line * icd->user_height;
+
+        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;
+        alloc_ctxs[0] = isi->alloc_ctx;
+
+        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 frame_buffer *buf = container_of(vb, 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 soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
+        struct frame_buffer *buf = container_of(vb, 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;
+        int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+                                                icd->current_fmt->host_fmt);
+
+        if (bytes_per_line < 0)
+                return bytes_per_line;
+
+        size = bytes_per_line * icd->user_height;
+
+        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(&buf->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_paddr(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 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(vb, 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_readl(isi, ISI_STATUS) & ISI_CTRL_CDC) {
+                dev_err(isi->icd->parent, "Already in frame handling.\n");
+                return;
+        }
+
+        isi_writel(isi, ISI_DMA_C_DSCR, 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);
+
+        /* Enable linked list */
+        cfg1 |= isi->pdata->frate | ISI_CFG1_DISCR;
+
+        /* Enable codec path and ISI */
+        ctrl = ISI_CTRL_CDC | ISI_CTRL_EN;
+        isi_writel(isi, ISI_CTRL, ctrl);
+        isi_writel(isi, ISI_CFG1, cfg1);
+}
+
+static void buffer_queue(struct vb2_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(vb, 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;
+                start_dma(isi, buf);
+        }
+        spin_unlock_irqrestore(&isi->lock, flags);
+}
+
+static int start_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;
+
+        u32 sr = 0;
+        int ret;
+
+        spin_lock_irq(&isi->lock);
+        isi->state = ISI_STATE_IDLE;
+        /* Clear any pending SOF interrupt */
+        sr = isi_readl(isi, ISI_STATUS);
+        /* Enable VSYNC interrupt for SOF */
+        isi_writel(isi, ISI_INTEN, ISI_SR_VSYNC);
+        isi_writel(isi, ISI_CTRL, ISI_CTRL_EN);
+        spin_unlock_irq(&isi->lock);
+
+        dev_dbg(icd->parent, "Waiting for SOF\n");
+        ret = wait_event_interruptible(isi->vsync_wq,
+                                       isi->state != ISI_STATE_IDLE);
+        if (ret)
+                return ret;
+
+        if (isi->state != ISI_STATE_READY)
+                return -EIO;
+
+        spin_lock_irq(&isi->lock);
+        isi->state = ISI_STATE_WAIT_SOF;
+        isi_writel(isi, ISI_INTDIS, ISI_SR_VSYNC);
+        spin_unlock_irq(&isi->lock);
+
+        return 0;
+}
+
+/* abort streaming and wait for last buffer */
+static int 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_STATE_ERROR);
+        }
+        spin_unlock_irq(&isi->lock);
+
+        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");
+                return -ETIMEDOUT;
+        }
+
+        /* 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");
+
+        return ret;
+}
+
+static 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           = soc_camera_unlock,
+        .wait_finish            = soc_camera_lock,
+};
+
+/* ------------------------------------------------------------------
+        SOC camera operations for the device
+   ------------------------------------------------------------------*/
+static int isi_camera_init_videobuf(struct vb2_queue *q,
+                                     struct soc_camera_device *icd)
+{
+        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;
+
+        return vb2_queue_init(q);
+}
+
+static int isi_camera_set_fmt(struct soc_camera_device *icd,
+                              struct v4l2_format *f)
+{
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+        struct atmel_isi *isi = ici->priv;
+        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_mbus_framefmt mf;
+        int ret;
+
+        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, video, s_mbus_fmt, &mf);
+        if (ret < 0)
+                return ret;
+
+        if (mf.code != xlate->code)
+                return -EINVAL;
+
+        ret = configure_geometry(isi, pix->width, pix->height, xlate->code);
+        if (ret < 0)
+                return ret;
+
+        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_mbus_framefmt mf;
+        u32 pixfmt = pix->pixelformat;
+        int ret;
+
+        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, video, try_mbus_fmt, &mf);
+        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,
+        },
+};
+
+/* 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);
+}
+
+static unsigned long make_bus_param(struct atmel_isi *isi)
+{
+        unsigned long flags;
+        /*
+         * Platform specified synchronization and pixel clock polarities are
+         * only a recommendation and are only used during probing. Atmel ISI
+         * camera interface only works in master mode, i.e., uses HSYNC and
+         * VSYNC signals from the sensor
+         */
+        flags = SOCAM_MASTER |
+                SOCAM_HSYNC_ACTIVE_HIGH |
+                SOCAM_HSYNC_ACTIVE_LOW |
+                SOCAM_VSYNC_ACTIVE_HIGH |
+                SOCAM_VSYNC_ACTIVE_LOW |
+                SOCAM_PCLK_SAMPLE_RISING |
+                SOCAM_PCLK_SAMPLE_FALLING |
+                SOCAM_DATA_ACTIVE_HIGH;
+
+        if (isi->pdata->data_width_flags & ISI_DATAWIDTH_10)
+                flags |= SOCAM_DATAWIDTH_10;
+
+        if (isi->pdata->data_width_flags & ISI_DATAWIDTH_8)
+                flags |= SOCAM_DATAWIDTH_8;
+
+        if (flags & SOCAM_DATAWIDTH_MASK)
+                return flags;
+
+        return 0;
+}
+
+static int isi_camera_try_bus_param(struct soc_camera_device *icd,
+                                    unsigned char buswidth)
+{
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+        struct atmel_isi *isi = ici->priv;
+        unsigned long camera_flags;
+        int ret;
+
+        camera_flags = icd->ops->query_bus_param(icd);
+        ret = soc_camera_bus_param_compatible(camera_flags,
+                                        make_bus_param(isi));
+        if (!ret)
+                return -EINVAL;
+        return 0;
+}
+
+
+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;
+        /* sensor format */
+        enum v4l2_mbus_pixelcode code;
+        /* soc camera host format */
+        const struct soc_mbus_pixelfmt *fmt;
+
+        ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
+        if (ret < 0)
+                /* No more formats */
+                return 0;
+
+        fmt = soc_mbus_get_fmtdesc(code);
+        if (!fmt) {
+                dev_err(icd->parent,
+                        "Invalid format code #%u: %d\n", idx, 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) {
+        case V4L2_MBUS_FMT_UYVY8_2X8:
+        case V4L2_MBUS_FMT_VYUY8_2X8:
+        case V4L2_MBUS_FMT_YUYV8_2X8:
+        case V4L2_MBUS_FMT_YVYU8_2X8:
+                formats++;
+                if (xlate) {
+                        xlate->host_fmt = &isi_camera_formats[0];
+                        xlate->code     = code;
+                        xlate++;
+                        dev_dbg(icd->parent, "Providing format %s using code %d\n",
+                                isi_camera_formats[0].name, 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;
+                xlate++;
+        }
+
+        return formats;
+}
+
+/* Called with .video_lock held */
+static int isi_camera_add_device(struct soc_camera_device *icd)
+{
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+        struct atmel_isi *isi = ici->priv;
+        int ret;
+
+        if (isi->icd)
+                return -EBUSY;
+
+        ret = clk_enable(isi->pclk);
+        if (ret)
+                return ret;
+
+        isi->icd = icd;
+        dev_dbg(icd->parent, "Atmel ISI Camera driver attached to camera %d\n",
+                 icd->devnum);
+        return 0;
+}
+/* Called with .video_lock held */
+static void isi_camera_remove_device(struct soc_camera_device *icd)
+{
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+        struct atmel_isi *isi = ici->priv;
+
+        BUG_ON(icd != isi->icd);
+
+        clk_disable(isi->pclk);
+        isi->icd = NULL;
+
+        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->capabilities = (V4L2_CAP_VIDEO_CAPTURE |
+                                V4L2_CAP_STREAMING);
+        return 0;
+}
+
+static int isi_camera_set_bus_param(struct soc_camera_device *icd, u32 pixfmt)
+{
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+        struct atmel_isi *isi = ici->priv;
+        unsigned long bus_flags, camera_flags, common_flags;
+        int ret;
+        u32 cfg1 = 0;
+
+        camera_flags = icd->ops->query_bus_param(icd);
+
+        bus_flags = make_bus_param(isi);
+        common_flags = soc_camera_bus_param_compatible(camera_flags, bus_flags);
+        dev_dbg(icd->parent, "Flags cam: 0x%lx host: 0x%lx common: 0x%lx\n",
+                camera_flags, bus_flags, common_flags);
+        if (!common_flags)
+                return -EINVAL;
+
+        /* Make choises, based on platform preferences */
+        if ((common_flags & SOCAM_HSYNC_ACTIVE_HIGH) &&
+            (common_flags & SOCAM_HSYNC_ACTIVE_LOW)) {
+                if (isi->pdata->hsync_act_low)
+                        common_flags &= ~SOCAM_HSYNC_ACTIVE_HIGH;
+                else
+                        common_flags &= ~SOCAM_HSYNC_ACTIVE_LOW;
+        }
+
+        if ((common_flags & SOCAM_VSYNC_ACTIVE_HIGH) &&
+            (common_flags & SOCAM_VSYNC_ACTIVE_LOW)) {
+                if (isi->pdata->vsync_act_low)
+                        common_flags &= ~SOCAM_VSYNC_ACTIVE_HIGH;
+                else
+                        common_flags &= ~SOCAM_VSYNC_ACTIVE_LOW;
+        }
+
+        if ((common_flags & SOCAM_PCLK_SAMPLE_RISING) &&
+            (common_flags & SOCAM_PCLK_SAMPLE_FALLING)) {
+                if (isi->pdata->pclk_act_falling)
+                        common_flags &= ~SOCAM_PCLK_SAMPLE_RISING;
+                else
+                        common_flags &= ~SOCAM_PCLK_SAMPLE_FALLING;
+        }
+
+        ret = icd->ops->set_bus_param(icd, common_flags);
+        if (ret < 0) {
+                dev_dbg(icd->parent, "Camera set_bus_param(%lx) returned %d\n",
+                        common_flags, ret);
+                return ret;
+        }
+
+        /* set bus param for ISI */
+        if (common_flags & SOCAM_HSYNC_ACTIVE_LOW)
+                cfg1 |= ISI_CFG1_HSYNC_POL_ACTIVE_LOW;
+        if (common_flags & SOCAM_VSYNC_ACTIVE_LOW)
+                cfg1 |= ISI_CFG1_VSYNC_POL_ACTIVE_LOW;
+        if (common_flags & SOCAM_PCLK_SAMPLE_FALLING)
+                cfg1 |= ISI_CFG1_PIXCLK_POL_ACTIVE_FALLING;
+
+        if (isi->pdata->has_emb_sync)
+                cfg1 |= ISI_CFG1_EMB_SYNC;
+        if (isi->pdata->isi_full_mode)
+                cfg1 |= ISI_CFG1_FULL_MODE;
+
+        isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
+        isi_writel(isi, ISI_CFG1, cfg1);
+
+        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 __devexit 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);
+
+        free_irq(isi->irq, isi);
+        soc_camera_host_unregister(soc_host);
+        vb2_dma_contig_cleanup_ctx(isi->alloc_ctx);
+        dma_free_coherent(&pdev->dev,
+                        sizeof(struct fbd) * MAX_BUFFER_NUM,
+                        isi->p_fb_descriptors,
+                        isi->fb_descriptors_phys);
+
+        iounmap(isi->regs);
+        clk_put(isi->pclk);
+        kfree(isi);
+
+        return 0;
+}
+
+static int __devinit atmel_isi_probe(struct platform_device *pdev)
+{
+        unsigned int irq;
+        struct atmel_isi *isi;
+        struct clk *pclk;
+        struct resource *regs;
+        int ret, i;
+        struct device *dev = &pdev->dev;
+        struct soc_camera_host *soc_host;
+        struct isi_platform_data *pdata;
+
+        pdata = dev->platform_data;
+        if (!pdata || !pdata->data_width_flags) {
+                dev_err(&pdev->dev,
+                        "No config available for Atmel ISI\n");
+                return -EINVAL;
+        }
+
+        regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!regs)
+                return -ENXIO;
+
+        pclk = clk_get(&pdev->dev, "isi_clk");
+        if (IS_ERR(pclk))
+                return PTR_ERR(pclk);
+
+        isi = kzalloc(sizeof(struct atmel_isi), GFP_KERNEL);
+        if (!isi) {
+                ret = -ENOMEM;
+                dev_err(&pdev->dev, "Can't allocate interface!\n");
+                goto err_alloc_isi;
+        }
+
+        isi->pclk = pclk;
+        isi->pdata = pdata;
+        isi->active = NULL;
+        spin_lock_init(&isi->lock);
+        init_waitqueue_head(&isi->vsync_wq);
+        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) {
+                ret = -ENOMEM;
+                dev_err(&pdev->dev, "Can't allocate descriptors!\n");
+                goto err_alloc_descriptors;
+        }
+
+        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);
+        }
+
+        isi->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+        if (IS_ERR(isi->alloc_ctx)) {
+                ret = PTR_ERR(isi->alloc_ctx);
+                goto err_alloc_ctx;
+        }
+
+        isi->regs = ioremap(regs->start, resource_size(regs));
+        if (!isi->regs) {
+                ret = -ENOMEM;
+                goto err_ioremap;
+        }
+
+        isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
+
+        irq = platform_get_irq(pdev, 0);
+        if (irq < 0) {
+                ret = irq;
+                goto err_req_irq;
+        }
+
+        ret = request_irq(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;
+
+        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:
+        free_irq(isi->irq, isi);
+err_req_irq:
+        iounmap(isi->regs);
+err_ioremap:
+        vb2_dma_contig_cleanup_ctx(isi->alloc_ctx);
+err_alloc_ctx:
+        dma_free_coherent(&pdev->dev,
+                        sizeof(struct fbd) * MAX_BUFFER_NUM,
+                        isi->p_fb_descriptors,
+                        isi->fb_descriptors_phys);
+err_alloc_descriptors:
+        kfree(isi);
+err_alloc_isi:
+        clk_put(isi->pclk);
+
+        return ret;
+}
+
+static struct platform_driver atmel_isi_driver = {
+        .probe          = atmel_isi_probe,
+        .remove         = __devexit_p(atmel_isi_remove),
+        .driver         = {
+                .name = "atmel_isi",
+                .owner = THIS_MODULE,
+        },
+};
+
+static int __init atmel_isi_init_module(void)
+{
+        return  platform_driver_probe(&atmel_isi_driver, &atmel_isi_probe);
+}
+
+static void __exit atmel_isi_exit(void)
+{
+        platform_driver_unregister(&atmel_isi_driver);
+}
+module_init(atmel_isi_init_module);
+module_exit(atmel_isi_exit);
+
+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/video/au0828/au0828-core.c b/drivers/media/video/au0828/au0828-core.c
index ca342e4c61f..1e4ce5068ec 100644
--- a/drivers/media/video/au0828/au0828-core.c
+++ b/drivers/media/video/au0828/au0828-core.c
@@ -292,3 +292,4 @@ module_exit(au0828_exit);
 MODULE_DESCRIPTION("Driver for Auvitek AU0828 based products");
 MODULE_AUTHOR("Steven Toth <stoth@linuxtv.org>");
 MODULE_LICENSE("GPL");
+MODULE_VERSION("0.0.2");
diff --git a/drivers/media/video/au0828/au0828-video.c b/drivers/media/video/au0828/au0828-video.c
index c03eb29a9ee..0b3e481ffe8 100644
--- a/drivers/media/video/au0828/au0828-video.c
+++ b/drivers/media/video/au0828/au0828-video.c
@@ -33,7 +33,6 @@
 #include <linux/init.h>
 #include <linux/device.h>
 #include <linux/suspend.h>
-#include <linux/version.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-chip-ident.h>
@@ -43,8 +42,6 @@
 
 static DEFINE_MUTEX(au0828_sysfs_lock);
 
-#define AU0828_VERSION_CODE KERNEL_VERSION(0, 0, 1)
-
 /* ------------------------------------------------------------------
         Videobuf operations
    ------------------------------------------------------------------*/
@@ -1254,8 +1251,6 @@ static int vidioc_querycap(struct file *file, void  *priv,
         strlcpy(cap->card, dev->board.name, sizeof(cap->card));
         strlcpy(cap->bus_info, dev->v4l2_dev.name, sizeof(cap->bus_info));
 
-        cap->version = AU0828_VERSION_CODE;
-
         /*set the device capabilities */
         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
                 V4L2_CAP_VBI_CAPTURE |
diff --git a/drivers/media/video/bt8xx/bttv-cards.c b/drivers/media/video/bt8xx/bttv-cards.c
index 3c9e6c7e7b5..5b15f63bf06 100644
--- a/drivers/media/video/bt8xx/bttv-cards.c
+++ b/drivers/media/video/bt8xx/bttv-cards.c
@@ -2892,13 +2892,10 @@ void __devinit bttv_idcard(struct bttv *btv)
 {
         unsigned int gpiobits;
         int i,type;
-        unsigned short tmp;
 
         /* read PCI subsystem ID */
-        pci_read_config_word(btv->c.pci, PCI_SUBSYSTEM_ID, &tmp);
-        btv->cardid = tmp << 16;
-        pci_read_config_word(btv->c.pci, PCI_SUBSYSTEM_VENDOR_ID, &tmp);
-        btv->cardid |= tmp;
+        btv->cardid  = btv->c.pci->subsystem_device << 16;
+        btv->cardid |= btv->c.pci->subsystem_vendor;
 
         if (0 != btv->cardid && 0xffffffff != btv->cardid) {
                 /* look for the card */
diff --git a/drivers/media/video/bt8xx/bttv-driver.c b/drivers/media/video/bt8xx/bttv-driver.c
index 834a48394bc..14444de67d5 100644
--- a/drivers/media/video/bt8xx/bttv-driver.c
+++ b/drivers/media/video/bt8xx/bttv-driver.c
@@ -57,6 +57,7 @@
 
 #include <media/saa6588.h>
 
+#define BTTV_VERSION "0.9.19"
 
 unsigned int bttv_num;                  /* number of Bt848s in use */
 struct bttv *bttvs[BTTV_MAX];
@@ -163,6 +164,7 @@ MODULE_PARM_DESC(radio_nr, "radio device numbers");
 MODULE_DESCRIPTION("bttv - v4l/v4l2 driver module for bt848/878 based cards");
 MODULE_AUTHOR("Ralph Metzler & Marcus Metzler & Gerd Knorr");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(BTTV_VERSION);
 
 /* ----------------------------------------------------------------------- */
 /* sysfs                                                                   */
@@ -2616,7 +2618,6 @@ static int bttv_querycap(struct file *file, void  *priv,
         strlcpy(cap->card, btv->video_dev->name, sizeof(cap->card));
         snprintf(cap->bus_info, sizeof(cap->bus_info),
                  "PCI:%s", pci_name(btv->c.pci));
-        cap->version = BTTV_VERSION_CODE;
         cap->capabilities =
                 V4L2_CAP_VIDEO_CAPTURE |
                 V4L2_CAP_VBI_CAPTURE |
@@ -3416,7 +3417,6 @@ static int radio_querycap(struct file *file, void *priv,
         strcpy(cap->driver, "bttv");
         strlcpy(cap->card, btv->radio_dev->name, sizeof(cap->card));
         sprintf(cap->bus_info, "PCI:%s", pci_name(btv->c.pci));
-        cap->version = BTTV_VERSION_CODE;
         cap->capabilities = V4L2_CAP_TUNER;
 
         return 0;
@@ -4585,14 +4585,8 @@ static int __init bttv_init_module(void)
 
         bttv_num = 0;
 
-        printk(KERN_INFO "bttv: driver version %d.%d.%d loaded\n",
-               (BTTV_VERSION_CODE >> 16) & 0xff,
-               (BTTV_VERSION_CODE >> 8) & 0xff,
-               BTTV_VERSION_CODE & 0xff);
-#ifdef SNAPSHOT
-        printk(KERN_INFO "bttv: snapshot date %04d-%02d-%02d\n",
-               SNAPSHOT/10000, (SNAPSHOT/100)%100, SNAPSHOT%100);
-#endif
+        printk(KERN_INFO "bttv: driver version %s loaded\n",
+               BTTV_VERSION);
         if (gbuffers < 2 || gbuffers > VIDEO_MAX_FRAME)
                 gbuffers = 2;
         if (gbufsize > BTTV_MAX_FBUF)
diff --git a/drivers/media/video/bt8xx/bttvp.h b/drivers/media/video/bt8xx/bttvp.h
index 9b776faf074..318edf2830b 100644
--- a/drivers/media/video/bt8xx/bttvp.h
+++ b/drivers/media/video/bt8xx/bttvp.h
@@ -25,9 +25,6 @@
 #ifndef _BTTVP_H_
 #define _BTTVP_H_
 
-#include <linux/version.h>
-#define BTTV_VERSION_CODE KERNEL_VERSION(0,9,18)
-
 #include <linux/types.h>
 #include <linux/wait.h>
 #include <linux/i2c.h>
diff --git a/drivers/media/video/bw-qcam.c b/drivers/media/video/bw-qcam.c
index c1193506131..f09df9dffaa 100644
--- a/drivers/media/video/bw-qcam.c
+++ b/drivers/media/video/bw-qcam.c
@@ -71,7 +71,6 @@ OTHER DEALINGS IN THE SOFTWARE.
 #include <linux/mm.h>
 #include <linux/parport.h>
 #include <linux/sched.h>
-#include <linux/version.h>
 #include <linux/videodev2.h>
 #include <linux/mutex.h>
 #include <asm/uaccess.h>
@@ -647,7 +646,6 @@ static int qcam_querycap(struct file *file, void  *priv,
         strlcpy(vcap->driver, qcam->v4l2_dev.name, sizeof(vcap->driver));
         strlcpy(vcap->card, "B&W Quickcam", sizeof(vcap->card));
         strlcpy(vcap->bus_info, "parport", sizeof(vcap->bus_info));
-        vcap->version = KERNEL_VERSION(0, 0, 2);
         vcap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
         return 0;
 }
@@ -895,6 +893,7 @@ static struct qcam *qcam_init(struct parport *port)
 
         if (v4l2_device_register(NULL, v4l2_dev) < 0) {
                 v4l2_err(v4l2_dev, "Could not register v4l2_device\n");
+                kfree(qcam);
                 return NULL;
         }
 
@@ -1092,3 +1091,4 @@ module_init(init_bw_qcams);
 module_exit(exit_bw_qcams);
 
 MODULE_LICENSE("GPL");
+MODULE_VERSION("0.0.3");
diff --git a/drivers/media/video/c-qcam.c b/drivers/media/video/c-qcam.c
index 24fc00965a1..cd8ff047318 100644
--- a/drivers/media/video/c-qcam.c
+++ b/drivers/media/video/c-qcam.c
@@ -35,7 +35,6 @@
 #include <linux/sched.h>
 #include <linux/mutex.h>
 #include <linux/jiffies.h>
-#include <linux/version.h>
 #include <linux/videodev2.h>
 #include <asm/uaccess.h>
 #include <media/v4l2-device.h>
@@ -517,7 +516,6 @@ static int qcam_querycap(struct file *file, void  *priv,
         strlcpy(vcap->driver, qcam->v4l2_dev.name, sizeof(vcap->driver));
         strlcpy(vcap->card, "Color Quickcam", sizeof(vcap->card));
         strlcpy(vcap->bus_info, "parport", sizeof(vcap->bus_info));
-        vcap->version = KERNEL_VERSION(0, 0, 3);
         vcap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
         return 0;
 }
@@ -752,6 +750,7 @@ static struct qcam *qcam_init(struct parport *port)
 
         if (v4l2_device_register(NULL, v4l2_dev) < 0) {
                 v4l2_err(v4l2_dev, "Could not register v4l2_device\n");
+                kfree(qcam);
                 return NULL;
         }
 
@@ -886,6 +885,7 @@ static void __exit cqcam_cleanup(void)
 MODULE_AUTHOR("Philip Blundell <philb@gnu.org>");
 MODULE_DESCRIPTION(BANNER);
 MODULE_LICENSE("GPL");
+MODULE_VERSION("0.0.4");
 
 module_init(cqcam_init);
 module_exit(cqcam_cleanup);
diff --git a/drivers/media/video/cafe_ccic-regs.h b/drivers/media/video/cafe_ccic-regs.h
deleted file mode 100644
index 8e2a87cdc79..00000000000
--- a/drivers/media/video/cafe_ccic-regs.h
+++ /dev/null
@@ -1,166 +0,0 @@
-/*
- * Register definitions for the m88alp01 camera interface.  Offsets in bytes
- * as given in the spec.
- *
- * Copyright 2006 One Laptop Per Child Association, Inc.
- *
- * Written by Jonathan Corbet, corbet@lwn.net.
- *
- * This file may be distributed under the terms of the GNU General
- * Public License, version 2.
- */
-#define REG_Y0BAR       0x00
-#define REG_Y1BAR       0x04
-#define REG_Y2BAR       0x08
-/* ... */
-
-#define REG_IMGPITCH    0x24    /* Image pitch register */
-#define   IMGP_YP_SHFT    2             /* Y pitch params */
-#define   IMGP_YP_MASK    0x00003ffc    /* Y pitch field */
-#define   IMGP_UVP_SHFT   18            /* UV pitch (planar) */
-#define   IMGP_UVP_MASK   0x3ffc0000
-#define REG_IRQSTATRAW  0x28    /* RAW IRQ Status */
-#define   IRQ_EOF0        0x00000001    /* End of frame 0 */
-#define   IRQ_EOF1        0x00000002    /* End of frame 1 */
-#define   IRQ_EOF2        0x00000004    /* End of frame 2 */
-#define   IRQ_SOF0        0x00000008    /* Start of frame 0 */
-#define   IRQ_SOF1        0x00000010    /* Start of frame 1 */
-#define   IRQ_SOF2        0x00000020    /* Start of frame 2 */
-#define   IRQ_OVERFLOW    0x00000040    /* FIFO overflow */
-#define   IRQ_TWSIW       0x00010000    /* TWSI (smbus) write */
-#define   IRQ_TWSIR       0x00020000    /* TWSI read */
-#define   IRQ_TWSIE       0x00040000    /* TWSI error */
-#define   TWSIIRQS (IRQ_TWSIW|IRQ_TWSIR|IRQ_TWSIE)
-#define   FRAMEIRQS (IRQ_EOF0|IRQ_EOF1|IRQ_EOF2|IRQ_SOF0|IRQ_SOF1|IRQ_SOF2)
-#define   ALLIRQS (TWSIIRQS|FRAMEIRQS|IRQ_OVERFLOW)
-#define REG_IRQMASK     0x2c    /* IRQ mask - same bits as IRQSTAT */
-#define REG_IRQSTAT     0x30    /* IRQ status / clear */
-
-#define REG_IMGSIZE     0x34    /* Image size */
-#define  IMGSZ_V_MASK     0x1fff0000
-#define  IMGSZ_V_SHIFT    16
-#define  IMGSZ_H_MASK     0x00003fff
-#define REG_IMGOFFSET   0x38    /* IMage offset */
-
-#define REG_CTRL0       0x3c    /* Control 0 */
-#define   C0_ENABLE       0x00000001    /* Makes the whole thing go */
-
-/* Mask for all the format bits */
-#define   C0_DF_MASK      0x00fffffc    /* Bits 2-23 */
-
-/* RGB ordering */
-#define   C0_RGB4_RGBX    0x00000000
-#define   C0_RGB4_XRGB    0x00000004
-#define   C0_RGB4_BGRX    0x00000008
-#define   C0_RGB4_XBGR    0x0000000c
-#define   C0_RGB5_RGGB    0x00000000
-#define   C0_RGB5_GRBG    0x00000004
-#define   C0_RGB5_GBRG    0x00000008
-#define   C0_RGB5_BGGR    0x0000000c
-
-/* Spec has two fields for DIN and DOUT, but they must match, so
-   combine them here. */
-#define   C0_DF_YUV       0x00000000    /* Data is YUV      */
-#define   C0_DF_RGB       0x000000a0    /* ... RGB                  */
-#define   C0_DF_BAYER     0x00000140    /* ... Bayer                */
-/* 8-8-8 must be missing from the below - ask */
-#define   C0_RGBF_565     0x00000000
-#define   C0_RGBF_444     0x00000800
-#define   C0_RGB_BGR      0x00001000    /* Blue comes first */
-#define   C0_YUV_PLANAR   0x00000000    /* YUV 422 planar format */
-#define   C0_YUV_PACKED   0x00008000    /* YUV 422 packed       */
-#define   C0_YUV_420PL    0x0000a000    /* YUV 420 planar       */
-/* Think that 420 packed must be 111 - ask */
-#define   C0_YUVE_YUYV    0x00000000    /* Y1CbY0Cr             */
-#define   C0_YUVE_YVYU    0x00010000    /* Y1CrY0Cb             */
-#define   C0_YUVE_VYUY    0x00020000    /* CrY1CbY0             */
-#define   C0_YUVE_UYVY    0x00030000    /* CbY1CrY0             */
-#define   C0_YUVE_XYUV    0x00000000    /* 420: .YUV            */
-#define   C0_YUVE_XYVU    0x00010000    /* 420: .YVU            */
-#define   C0_YUVE_XUVY    0x00020000    /* 420: .UVY            */
-#define   C0_YUVE_XVUY    0x00030000    /* 420: .VUY            */
-/* Bayer bits 18,19 if needed */
-#define   C0_HPOL_LOW     0x01000000    /* HSYNC polarity active low */
-#define   C0_VPOL_LOW     0x02000000    /* VSYNC polarity active low */
-#define   C0_VCLK_LOW     0x04000000    /* VCLK on falling edge */
-#define   C0_DOWNSCALE    0x08000000    /* Enable downscaler */
-#define   C0_SIFM_MASK    0xc0000000    /* SIF mode bits */
-#define   C0_SIF_HVSYNC   0x00000000    /* Use H/VSYNC */
-#define   CO_SOF_NOSYNC   0x40000000    /* Use inband active signaling */
-
-
-#define REG_CTRL1       0x40    /* Control 1 */
-#define   C1_444ALPHA     0x00f00000    /* Alpha field in RGB444 */
-#define   C1_ALPHA_SHFT   20
-#define   C1_DMAB32       0x00000000    /* 32-byte DMA burst */
-#define   C1_DMAB16       0x02000000    /* 16-byte DMA burst */
-#define   C1_DMAB64       0x04000000    /* 64-byte DMA burst */
-#define   C1_DMAB_MASK    0x06000000
-#define   C1_TWOBUFS      0x08000000    /* Use only two DMA buffers */
-#define   C1_PWRDWN       0x10000000    /* Power down */
-
-#define REG_CLKCTRL     0x88    /* Clock control */
-#define   CLK_DIV_MASK    0x0000ffff    /* Upper bits RW "reserved" */
-
-#define REG_GPR         0xb4    /* General purpose register.  This
-                                   controls inputs to the power and reset
-                                   pins on the OV7670 used with OLPC;
-                                   other deployments could differ.  */
-#define   GPR_C1EN        0x00000020    /* Pad 1 (power down) enable */
-#define   GPR_C0EN        0x00000010    /* Pad 0 (reset) enable */
-#define   GPR_C1          0x00000002    /* Control 1 value */
-/*
- * Control 0 is wired to reset on OLPC machines.  For ov7x sensors,
- * it is active low, for 0v6x, instead, it's active high.  What
- * fun.
- */
-#define   GPR_C0          0x00000001    /* Control 0 value */
-
-#define REG_TWSIC0      0xb8    /* TWSI (smbus) control 0 */
-#define   TWSIC0_EN       0x00000001    /* TWSI enable */
-#define   TWSIC0_MODE     0x00000002    /* 1 = 16-bit, 0 = 8-bit */
-#define   TWSIC0_SID      0x000003fc    /* Slave ID */
-#define   TWSIC0_SID_SHIFT 2
-#define   TWSIC0_CLKDIV   0x0007fc00    /* Clock divider */
-#define   TWSIC0_MASKACK  0x00400000    /* Mask ack from sensor */
-#define   TWSIC0_OVMAGIC  0x00800000    /* Make it work on OV sensors */
-
-#define REG_TWSIC1      0xbc    /* TWSI control 1 */
-#define   TWSIC1_DATA     0x0000ffff    /* Data to/from camchip */
-#define   TWSIC1_ADDR     0x00ff0000    /* Address (register) */
-#define   TWSIC1_ADDR_SHIFT 16
-#define   TWSIC1_READ     0x01000000    /* Set for read op */
-#define   TWSIC1_WSTAT    0x02000000    /* Write status */
-#define   TWSIC1_RVALID   0x04000000    /* Read data valid */
-#define   TWSIC1_ERROR    0x08000000    /* Something screwed up */
-
-
-#define REG_UBAR        0xc4    /* Upper base address register */
-
-/*
- * Here's the weird global control registers which are said to live
- * way up here.
- */
-#define REG_GL_CSR     0x3004  /* Control/status register */
-#define   GCSR_SRS       0x00000001     /* SW Reset set */
-#define   GCSR_SRC       0x00000002     /* SW Reset clear */
-#define   GCSR_MRS       0x00000004     /* Master reset set */
-#define   GCSR_MRC       0x00000008     /* HW Reset clear */
-#define   GCSR_CCIC_EN   0x00004000    /* CCIC Clock enable */
-#define REG_GL_IMASK   0x300c  /* Interrupt mask register */
-#define   GIMSK_CCIC_EN          0x00000004    /* CCIC Interrupt enable */
-
-#define REG_GL_FCR      0x3038  /* GPIO functional control register */
-#define   GFCR_GPIO_ON    0x08          /* Camera GPIO enabled */
-#define REG_GL_GPIOR    0x315c  /* GPIO register */
-#define   GGPIO_OUT             0x80000 /* GPIO output */
-#define   GGPIO_VAL             0x00008 /* Output pin value */
-
-#define REG_LEN                REG_GL_IMASK + 4
-
-
-/*
- * Useful stuff that probably belongs somewhere global.
- */
-#define VGA_WIDTH       640
-#define VGA_HEIGHT      480
diff --git a/drivers/media/video/cafe_ccic.c b/drivers/media/video/cafe_ccic.c
deleted file mode 100644
index 66470339849..00000000000
--- a/drivers/media/video/cafe_ccic.c
+++ /dev/null
@@ -1,2267 +0,0 @@
-/*
- * A driver for the CMOS camera controller in the Marvell 88ALP01 "cafe"
- * multifunction chip.  Currently works with the Omnivision OV7670
- * sensor.
- *
- * The data sheet for this device can be found at:
- *    http://www.marvell.com/products/pc_connectivity/88alp01/ 
- *
- * Copyright 2006 One Laptop Per Child Association, Inc.
- * Copyright 2006-7 Jonathan Corbet <corbet@lwn.net>
- *
- * Written by Jonathan Corbet, corbet@lwn.net.
- *
- * v4l2_device/v4l2_subdev conversion by:
- * Copyright (C) 2009 Hans Verkuil <hverkuil@xs4all.nl>
- *
- * Note: this conversion is untested! Please contact the linux-media
- * mailinglist if you can test this, together with the test results.
- *
- * This file may be distributed under the terms of the GNU General
- * Public License, version 2.
- */
-
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/fs.h>
-#include <linux/dmi.h>
-#include <linux/mm.h>
-#include <linux/pci.h>
-#include <linux/i2c.h>
-#include <linux/interrupt.h>
-#include <linux/spinlock.h>
-#include <linux/videodev2.h>
-#include <linux/slab.h>
-#include <media/v4l2-device.h>
-#include <media/v4l2-ioctl.h>
-#include <media/v4l2-chip-ident.h>
-#include <linux/device.h>
-#include <linux/wait.h>
-#include <linux/list.h>
-#include <linux/dma-mapping.h>
-#include <linux/delay.h>
-#include <linux/jiffies.h>
-#include <linux/vmalloc.h>
-
-#include <asm/uaccess.h>
-#include <asm/io.h>
-
-#include "ov7670.h"
-#include "cafe_ccic-regs.h"
-
-#define CAFE_VERSION 0x000002
-
-
-/*
- * Parameters.
- */
-MODULE_AUTHOR("Jonathan Corbet <corbet@lwn.net>");
-MODULE_DESCRIPTION("Marvell 88ALP01 CMOS Camera Controller driver");
-MODULE_LICENSE("GPL");
-MODULE_SUPPORTED_DEVICE("Video");
-
-/*
- * Internal DMA buffer management.  Since the controller cannot do S/G I/O,
- * we must have physically contiguous buffers to bring frames into.
- * These parameters control how many buffers we use, whether we
- * allocate them at load time (better chance of success, but nails down
- * memory) or when somebody tries to use the camera (riskier), and,
- * for load-time allocation, how big they should be.
- *
- * The controller can cycle through three buffers.  We could use
- * more by flipping pointers around, but it probably makes little
- * sense.
- */
-
-#define MAX_DMA_BUFS 3
-static int alloc_bufs_at_read;
-module_param(alloc_bufs_at_read, bool, 0444);
-MODULE_PARM_DESC(alloc_bufs_at_read,
-                "Non-zero value causes DMA buffers to be allocated when the "
-                "video capture device is read, rather than at module load "
-                "time.  This saves memory, but decreases the chances of "
-                "successfully getting those buffers.");
-
-static int n_dma_bufs = 3;
-module_param(n_dma_bufs, uint, 0644);
-MODULE_PARM_DESC(n_dma_bufs,
-                "The number of DMA buffers to allocate.  Can be either two "
-                "(saves memory, makes timing tighter) or three.");
-
-static int dma_buf_size = VGA_WIDTH * VGA_HEIGHT * 2;  /* Worst case */
-module_param(dma_buf_size, uint, 0444);
-MODULE_PARM_DESC(dma_buf_size,
-                "The size of the allocated DMA buffers.  If actual operating "
-                "parameters require larger buffers, an attempt to reallocate "
-                "will be made.");
-
-static int min_buffers = 1;
-module_param(min_buffers, uint, 0644);
-MODULE_PARM_DESC(min_buffers,
-                "The minimum number of streaming I/O buffers we are willing "
-                "to work with.");
-
-static int max_buffers = 10;
-module_param(max_buffers, uint, 0644);
-MODULE_PARM_DESC(max_buffers,
-                "The maximum number of streaming I/O buffers an application "
-                "will be allowed to allocate.  These buffers are big and live "
-                "in vmalloc space.");
-
-static int flip;
-module_param(flip, bool, 0444);
-MODULE_PARM_DESC(flip,
-                "If set, the sensor will be instructed to flip the image "
-                "vertically.");
-
-
-enum cafe_state {
-        S_NOTREADY,     /* Not yet initialized */
-        S_IDLE,         /* Just hanging around */
-        S_FLAKED,       /* Some sort of problem */
-        S_SINGLEREAD,   /* In read() */
-        S_SPECREAD,     /* Speculative read (for future read()) */
-        S_STREAMING     /* Streaming data */
-};
-
-/*
- * Tracking of streaming I/O buffers.
- */
-struct cafe_sio_buffer {
-        struct list_head list;
-        struct v4l2_buffer v4lbuf;
-        char *buffer;   /* Where it lives in kernel space */
-        int mapcount;
-        struct cafe_camera *cam;
-};
-
-/*
- * A description of one of our devices.
- * Locking: controlled by s_mutex.  Certain fields, however, require
- *          the dev_lock spinlock; they are marked as such by comments.
- *          dev_lock is also required for access to device registers.
- */
-struct cafe_camera
-{
-        struct v4l2_device v4l2_dev;
-        enum cafe_state state;
-        unsigned long flags;            /* Buffer status, mainly (dev_lock) */
-        int users;                      /* How many open FDs */
-        struct file *owner;             /* Who has data access (v4l2) */
-
-        /*
-         * Subsystem structures.
-         */
-        struct pci_dev *pdev;
-        struct video_device vdev;
-        struct i2c_adapter i2c_adapter;
-        struct v4l2_subdev *sensor;
-        unsigned short sensor_addr;
-
-        unsigned char __iomem *regs;
-        struct list_head dev_list;      /* link to other devices */
-
-        /* DMA buffers */
-        unsigned int nbufs;             /* How many are alloc'd */
-        int next_buf;                   /* Next to consume (dev_lock) */
-        unsigned int dma_buf_size;      /* allocated size */
-        void *dma_bufs[MAX_DMA_BUFS];   /* Internal buffer addresses */
-        dma_addr_t dma_handles[MAX_DMA_BUFS]; /* Buffer bus addresses */
-        unsigned int specframes;        /* Unconsumed spec frames (dev_lock) */
-        unsigned int sequence;          /* Frame sequence number */
-        unsigned int buf_seq[MAX_DMA_BUFS]; /* Sequence for individual buffers */
-
-        /* Streaming buffers */
-        unsigned int n_sbufs;           /* How many we have */
-        struct cafe_sio_buffer *sb_bufs; /* The array of housekeeping structs */
-        struct list_head sb_avail;      /* Available for data (we own) (dev_lock) */
-        struct list_head sb_full;       /* With data (user space owns) (dev_lock) */
-        struct tasklet_struct s_tasklet;
-
-        /* Current operating parameters */
-        u32 sensor_type;                /* Currently ov7670 only */
-        struct v4l2_pix_format pix_format;
-        enum v4l2_mbus_pixelcode mbus_code;
-
-        /* Locks */
-        struct mutex s_mutex; /* Access to this structure */
-        spinlock_t dev_lock;  /* Access to device */
-
-        /* Misc */
-        wait_queue_head_t smbus_wait;   /* Waiting on i2c events */
-        wait_queue_head_t iowait;       /* Waiting on frame data */
-};
-
-/*
- * Status flags.  Always manipulated with bit operations.
- */
-#define CF_BUF0_VALID    0      /* Buffers valid - first three */
-#define CF_BUF1_VALID    1
-#define CF_BUF2_VALID    2
-#define CF_DMA_ACTIVE    3      /* A frame is incoming */
-#define CF_CONFIG_NEEDED 4      /* Must configure hardware */
-
-#define sensor_call(cam, o, f, args...) \
-        v4l2_subdev_call(cam->sensor, o, f, ##args)
-
-static inline struct cafe_camera *to_cam(struct v4l2_device *dev)
-{
-        return container_of(dev, struct cafe_camera, v4l2_dev);
-}
-
-static struct cafe_format_struct {
-        __u8 *desc;
-        __u32 pixelformat;
-        int bpp;   /* Bytes per pixel */
-        enum v4l2_mbus_pixelcode mbus_code;
-} cafe_formats[] = {
-        {
-                .desc           = "YUYV 4:2:2",
-                .pixelformat    = V4L2_PIX_FMT_YUYV,
-                .mbus_code      = V4L2_MBUS_FMT_YUYV8_2X8,
-                .bpp            = 2,
-        },
-        {
-                .desc           = "RGB 444",
-                .pixelformat    = V4L2_PIX_FMT_RGB444,
-                .mbus_code      = V4L2_MBUS_FMT_RGB444_2X8_PADHI_LE,
-                .bpp            = 2,
-        },
-        {
-                .desc           = "RGB 565",
-                .pixelformat    = V4L2_PIX_FMT_RGB565,
-                .mbus_code      = V4L2_MBUS_FMT_RGB565_2X8_LE,
-                .bpp            = 2,
-        },
-        {
-                .desc           = "Raw RGB Bayer",
-                .pixelformat    = V4L2_PIX_FMT_SBGGR8,
-                .mbus_code      = V4L2_MBUS_FMT_SBGGR8_1X8,
-                .bpp            = 1
-        },
-};
-#define N_CAFE_FMTS ARRAY_SIZE(cafe_formats)
-
-static struct cafe_format_struct *cafe_find_format(u32 pixelformat)
-{
-        unsigned i;
-
-        for (i = 0; i < N_CAFE_FMTS; i++)
-                if (cafe_formats[i].pixelformat == pixelformat)
-                        return cafe_formats + i;
-        /* Not found? Then return the first format. */
-        return cafe_formats;
-}
-
-/*
- * Start over with DMA buffers - dev_lock needed.
- */
-static void cafe_reset_buffers(struct cafe_camera *cam)
-{
-        int i;
-
-        cam->next_buf = -1;
-        for (i = 0; i < cam->nbufs; i++)
-                clear_bit(i, &cam->flags);
-        cam->specframes = 0;
-}
-
-static inline int cafe_needs_config(struct cafe_camera *cam)
-{
-        return test_bit(CF_CONFIG_NEEDED, &cam->flags);
-}
-
-static void cafe_set_config_needed(struct cafe_camera *cam, int needed)
-{
-        if (needed)
-                set_bit(CF_CONFIG_NEEDED, &cam->flags);
-        else
-                clear_bit(CF_CONFIG_NEEDED, &cam->flags);
-}
-
-
-
-
-/*
- * Debugging and related.
- */
-#define cam_err(cam, fmt, arg...) \
-        dev_err(&(cam)->pdev->dev, fmt, ##arg);
-#define cam_warn(cam, fmt, arg...) \
-        dev_warn(&(cam)->pdev->dev, fmt, ##arg);
-#define cam_dbg(cam, fmt, arg...) \
-        dev_dbg(&(cam)->pdev->dev, fmt, ##arg);
-
-
-/* ---------------------------------------------------------------------*/
-
-/*
- * Device register I/O
- */
-static inline void cafe_reg_write(struct cafe_camera *cam, unsigned int reg,
-                unsigned int val)
-{
-        iowrite32(val, cam->regs + reg);
-}
-
-static inline unsigned int cafe_reg_read(struct cafe_camera *cam,
-                unsigned int reg)
-{
-        return ioread32(cam->regs + reg);
-}
-
-
-static inline void cafe_reg_write_mask(struct cafe_camera *cam, unsigned int reg,
-                unsigned int val, unsigned int mask)
-{
-        unsigned int v = cafe_reg_read(cam, reg);
-
-        v = (v & ~mask) | (val & mask);
-        cafe_reg_write(cam, reg, v);
-}
-
-static inline void cafe_reg_clear_bit(struct cafe_camera *cam,
-                unsigned int reg, unsigned int val)
-{
-        cafe_reg_write_mask(cam, reg, 0, val);
-}
-
-static inline void cafe_reg_set_bit(struct cafe_camera *cam,
-                unsigned int reg, unsigned int val)
-{
-        cafe_reg_write_mask(cam, reg, val, val);
-}
-
-
-
-/* -------------------------------------------------------------------- */
-/*
- * The I2C/SMBUS interface to the camera itself starts here.  The
- * controller handles SMBUS itself, presenting a relatively simple register
- * interface; all we have to do is to tell it where to route the data.
- */
-#define CAFE_SMBUS_TIMEOUT (HZ)  /* generous */
-
-static int cafe_smbus_write_done(struct cafe_camera *cam)
-{
-        unsigned long flags;
-        int c1;
-
-        /*
-         * We must delay after the interrupt, or the controller gets confused
-         * and never does give us good status.  Fortunately, we don't do this
-         * often.
-         */
-        udelay(20);
-        spin_lock_irqsave(&cam->dev_lock, flags);
-        c1 = cafe_reg_read(cam, REG_TWSIC1);
-        spin_unlock_irqrestore(&cam->dev_lock, flags);
-        return (c1 & (TWSIC1_WSTAT|TWSIC1_ERROR)) != TWSIC1_WSTAT;
-}
-
-static int cafe_smbus_write_data(struct cafe_camera *cam,
-                u16 addr, u8 command, u8 value)
-{
-        unsigned int rval;
-        unsigned long flags;
-
-        spin_lock_irqsave(&cam->dev_lock, flags);
-        rval = TWSIC0_EN | ((addr << TWSIC0_SID_SHIFT) & TWSIC0_SID);
-        rval |= TWSIC0_OVMAGIC;  /* Make OV sensors work */
-        /*
-         * Marvell sez set clkdiv to all 1's for now.
-         */
-        rval |= TWSIC0_CLKDIV;
-        cafe_reg_write(cam, REG_TWSIC0, rval);
-        (void) cafe_reg_read(cam, REG_TWSIC1); /* force write */
-        rval = value | ((command << TWSIC1_ADDR_SHIFT) & TWSIC1_ADDR);
-        cafe_reg_write(cam, REG_TWSIC1, rval);
-        spin_unlock_irqrestore(&cam->dev_lock, flags);
-
-        /* Unfortunately, reading TWSIC1 too soon after sending a command
-         * causes the device to die.
-         * Use a busy-wait because we often send a large quantity of small
-         * commands at-once; using msleep() would cause a lot of context
-         * switches which take longer than 2ms, resulting in a noticeable
-         * boot-time and capture-start delays.
-         */
-        mdelay(2);
-
-        /*
-         * Another sad fact is that sometimes, commands silently complete but
-         * cafe_smbus_write_done() never becomes aware of this.
-         * This happens at random and appears to possible occur with any
-         * command.
-         * We don't understand why this is. We work around this issue
-         * with the timeout in the wait below, assuming that all commands
-         * complete within the timeout.
-         */
-        wait_event_timeout(cam->smbus_wait, cafe_smbus_write_done(cam),
-                        CAFE_SMBUS_TIMEOUT);
-
-        spin_lock_irqsave(&cam->dev_lock, flags);
-        rval = cafe_reg_read(cam, REG_TWSIC1);
-        spin_unlock_irqrestore(&cam->dev_lock, flags);
-
-        if (rval & TWSIC1_WSTAT) {
-                cam_err(cam, "SMBUS write (%02x/%02x/%02x) timed out\n", addr,
-                                command, value);
-                return -EIO;
-        }
-        if (rval & TWSIC1_ERROR) {
-                cam_err(cam, "SMBUS write (%02x/%02x/%02x) error\n", addr,
-                                command, value);
-                return -EIO;
-        }
-        return 0;
-}
-
-
-
-static int cafe_smbus_read_done(struct cafe_camera *cam)
-{
-        unsigned long flags;
-        int c1;
-
-        /*
-         * We must delay after the interrupt, or the controller gets confused
-         * and never does give us good status.  Fortunately, we don't do this
-         * often.
-         */
-        udelay(20);
-        spin_lock_irqsave(&cam->dev_lock, flags);
-        c1 = cafe_reg_read(cam, REG_TWSIC1);
-        spin_unlock_irqrestore(&cam->dev_lock, flags);
-        return c1 & (TWSIC1_RVALID|TWSIC1_ERROR);
-}
-
-
-
-static int cafe_smbus_read_data(struct cafe_camera *cam,
-                u16 addr, u8 command, u8 *value)
-{
-        unsigned int rval;
-        unsigned long flags;
-
-        spin_lock_irqsave(&cam->dev_lock, flags);
-        rval = TWSIC0_EN | ((addr << TWSIC0_SID_SHIFT) & TWSIC0_SID);
-        rval |= TWSIC0_OVMAGIC; /* Make OV sensors work */
-        /*
-         * Marvel sez set clkdiv to all 1's for now.
-         */
-        rval |= TWSIC0_CLKDIV;
-        cafe_reg_write(cam, REG_TWSIC0, rval);
-        (void) cafe_reg_read(cam, REG_TWSIC1); /* force write */
-        rval = TWSIC1_READ | ((command << TWSIC1_ADDR_SHIFT) & TWSIC1_ADDR);
-        cafe_reg_write(cam, REG_TWSIC1, rval);
-        spin_unlock_irqrestore(&cam->dev_lock, flags);
-
-        wait_event_timeout(cam->smbus_wait,
-                        cafe_smbus_read_done(cam), CAFE_SMBUS_TIMEOUT);
-        spin_lock_irqsave(&cam->dev_lock, flags);
-        rval = cafe_reg_read(cam, REG_TWSIC1);
-        spin_unlock_irqrestore(&cam->dev_lock, flags);
-
-        if (rval & TWSIC1_ERROR) {
-                cam_err(cam, "SMBUS read (%02x/%02x) error\n", addr, command);
-                return -EIO;
-        }
-        if (! (rval & TWSIC1_RVALID)) {
-                cam_err(cam, "SMBUS read (%02x/%02x) timed out\n", addr,
-                                command);
-                return -EIO;
-        }
-        *value = rval & 0xff;
-        return 0;
-}
-
-/*
- * Perform a transfer over SMBUS.  This thing is called under
- * the i2c bus lock, so we shouldn't race with ourselves...
- */
-static int cafe_smbus_xfer(struct i2c_adapter *adapter, u16 addr,
-                unsigned short flags, char rw, u8 command,
-                int size, union i2c_smbus_data *data)
-{
-        struct v4l2_device *v4l2_dev = i2c_get_adapdata(adapter);
-        struct cafe_camera *cam = to_cam(v4l2_dev);
-        int ret = -EINVAL;
-
-        /*
-         * This interface would appear to only do byte data ops.  OK
-         * it can do word too, but the cam chip has no use for that.
-         */
-        if (size != I2C_SMBUS_BYTE_DATA) {
-                cam_err(cam, "funky xfer size %d\n", size);
-                return -EINVAL;
-        }
-
-        if (rw == I2C_SMBUS_WRITE)
-                ret = cafe_smbus_write_data(cam, addr, command, data->byte);
-        else if (rw == I2C_SMBUS_READ)
-                ret = cafe_smbus_read_data(cam, addr, command, &data->byte);
-        return ret;
-}
-
-
-static void cafe_smbus_enable_irq(struct cafe_camera *cam)
-{
-        unsigned long flags;
-
-        spin_lock_irqsave(&cam->dev_lock, flags);
-        cafe_reg_set_bit(cam, REG_IRQMASK, TWSIIRQS);
-        spin_unlock_irqrestore(&cam->dev_lock, flags);
-}
-
-static u32 cafe_smbus_func(struct i2c_adapter *adapter)
-{
-        return I2C_FUNC_SMBUS_READ_BYTE_DATA  |
-               I2C_FUNC_SMBUS_WRITE_BYTE_DATA;
-}
-
-static struct i2c_algorithm cafe_smbus_algo = {
-        .smbus_xfer = cafe_smbus_xfer,
-        .functionality = cafe_smbus_func
-};
-
-/* Somebody is on the bus */
-static void cafe_ctlr_stop_dma(struct cafe_camera *cam);
-static void cafe_ctlr_power_down(struct cafe_camera *cam);
-
-static int cafe_smbus_setup(struct cafe_camera *cam)
-{
-        struct i2c_adapter *adap = &cam->i2c_adapter;
-        int ret;
-
-        cafe_smbus_enable_irq(cam);
-        adap->owner = THIS_MODULE;
-        adap->algo = &cafe_smbus_algo;
-        strcpy(adap->name, "cafe_ccic");
-        adap->dev.parent = &cam->pdev->dev;
-        i2c_set_adapdata(adap, &cam->v4l2_dev);
-        ret = i2c_add_adapter(adap);
-        if (ret)
-                printk(KERN_ERR "Unable to register cafe i2c adapter\n");
-        return ret;
-}
-
-static void cafe_smbus_shutdown(struct cafe_camera *cam)
-{
-        i2c_del_adapter(&cam->i2c_adapter);
-}
-
-
-/* ------------------------------------------------------------------- */
-/*
- * Deal with the controller.
- */
-
-/*
- * Do everything we think we need to have the interface operating
- * according to the desired format.
- */
-static void cafe_ctlr_dma(struct cafe_camera *cam)
-{
-        /*
-         * Store the first two Y buffers (we aren't supporting
-         * planar formats for now, so no UV bufs).  Then either
-         * set the third if it exists, or tell the controller
-         * to just use two.
-         */
-        cafe_reg_write(cam, REG_Y0BAR, cam->dma_handles[0]);
-        cafe_reg_write(cam, REG_Y1BAR, cam->dma_handles[1]);
-        if (cam->nbufs > 2) {
-                cafe_reg_write(cam, REG_Y2BAR, cam->dma_handles[2]);
-                cafe_reg_clear_bit(cam, REG_CTRL1, C1_TWOBUFS);
-        }
-        else
-                cafe_reg_set_bit(cam, REG_CTRL1, C1_TWOBUFS);
-        cafe_reg_write(cam, REG_UBAR, 0); /* 32 bits only for now */
-}
-
-static void cafe_ctlr_image(struct cafe_camera *cam)
-{
-        int imgsz;
-        struct v4l2_pix_format *fmt = &cam->pix_format;
-
-        imgsz = ((fmt->height << IMGSZ_V_SHIFT) & IMGSZ_V_MASK) |
-                (fmt->bytesperline & IMGSZ_H_MASK);
-        cafe_reg_write(cam, REG_IMGSIZE, imgsz);
-        cafe_reg_write(cam, REG_IMGOFFSET, 0);
-        /* YPITCH just drops the last two bits */
-        cafe_reg_write_mask(cam, REG_IMGPITCH, fmt->bytesperline,
-                        IMGP_YP_MASK);
-        /*
-         * Tell the controller about the image format we are using.
-         */
-        switch (cam->pix_format.pixelformat) {
-        case V4L2_PIX_FMT_YUYV:
-            cafe_reg_write_mask(cam, REG_CTRL0,
-                            C0_DF_YUV|C0_YUV_PACKED|C0_YUVE_YUYV,
-                            C0_DF_MASK);
-            break;
-
-        case V4L2_PIX_FMT_RGB444:
-            cafe_reg_write_mask(cam, REG_CTRL0,
-                            C0_DF_RGB|C0_RGBF_444|C0_RGB4_XRGB,
-                            C0_DF_MASK);
-                /* Alpha value? */
-            break;
-
-        case V4L2_PIX_FMT_RGB565:
-            cafe_reg_write_mask(cam, REG_CTRL0,
-                            C0_DF_RGB|C0_RGBF_565|C0_RGB5_BGGR,
-                            C0_DF_MASK);
-            break;
-
-        default:
-            cam_err(cam, "Unknown format %x\n", cam->pix_format.pixelformat);
-            break;
-        }
-        /*
-         * Make sure it knows we want to use hsync/vsync.
-         */
-        cafe_reg_write_mask(cam, REG_CTRL0, C0_SIF_HVSYNC,
-                        C0_SIFM_MASK);
-}
-
-
-/*
- * Configure the controller for operation; caller holds the
- * device mutex.
- */
-static int cafe_ctlr_configure(struct cafe_camera *cam)
-{
-        unsigned long flags;
-
-        spin_lock_irqsave(&cam->dev_lock, flags);
-        cafe_ctlr_dma(cam);
-        cafe_ctlr_image(cam);
-        cafe_set_config_needed(cam, 0);
-        spin_unlock_irqrestore(&cam->dev_lock, flags);
-        return 0;
-}
-
-static void cafe_ctlr_irq_enable(struct cafe_camera *cam)
-{
-        /*
-         * Clear any pending interrupts, since we do not
-         * expect to have I/O active prior to enabling.
-         */
-        cafe_reg_write(cam, REG_IRQSTAT, FRAMEIRQS);
-        cafe_reg_set_bit(cam, REG_IRQMASK, FRAMEIRQS);
-}
-
-static void cafe_ctlr_irq_disable(struct cafe_camera *cam)
-{
-        cafe_reg_clear_bit(cam, REG_IRQMASK, FRAMEIRQS);
-}
-
-/*
- * Make the controller start grabbing images.  Everything must
- * be set up before doing this.
- */
-static void cafe_ctlr_start(struct cafe_camera *cam)
-{
-        /* set_bit performs a read, so no other barrier should be
-           needed here */
-        cafe_reg_set_bit(cam, REG_CTRL0, C0_ENABLE);
-}
-
-static void cafe_ctlr_stop(struct cafe_camera *cam)
-{
-        cafe_reg_clear_bit(cam, REG_CTRL0, C0_ENABLE);
-}
-
-static void cafe_ctlr_init(struct cafe_camera *cam)
-{
-        unsigned long flags;
-
-        spin_lock_irqsave(&cam->dev_lock, flags);
-        /*
-         * Added magic to bring up the hardware on the B-Test board
-         */
-        cafe_reg_write(cam, 0x3038, 0x8);
-        cafe_reg_write(cam, 0x315c, 0x80008);
-        /*
-         * Go through the dance needed to wake the device up.
-         * Note that these registers are global and shared
-         * with the NAND and SD devices.  Interaction between the
-         * three still needs to be examined.
-         */
-        cafe_reg_write(cam, REG_GL_CSR, GCSR_SRS|GCSR_MRS); /* Needed? */
-        cafe_reg_write(cam, REG_GL_CSR, GCSR_SRC|GCSR_MRC);
-        cafe_reg_write(cam, REG_GL_CSR, GCSR_SRC|GCSR_MRS);
-        /*
-         * Here we must wait a bit for the controller to come around.
-         */
-        spin_unlock_irqrestore(&cam->dev_lock, flags);
-        msleep(5);
-        spin_lock_irqsave(&cam->dev_lock, flags);
-
-        cafe_reg_write(cam, REG_GL_CSR, GCSR_CCIC_EN|GCSR_SRC|GCSR_MRC);
-        cafe_reg_set_bit(cam, REG_GL_IMASK, GIMSK_CCIC_EN);
-        /*
-         * Make sure it's not powered down.
-         */
-        cafe_reg_clear_bit(cam, REG_CTRL1, C1_PWRDWN);
-        /*
-         * Turn off the enable bit.  It sure should be off anyway,
-         * but it's good to be sure.
-         */
-        cafe_reg_clear_bit(cam, REG_CTRL0, C0_ENABLE);
-        /*
-         * Mask all interrupts.
-         */
-        cafe_reg_write(cam, REG_IRQMASK, 0);
-        /*
-         * Clock the sensor appropriately.  Controller clock should
-         * be 48MHz, sensor "typical" value is half that.
-         */
-        cafe_reg_write_mask(cam, REG_CLKCTRL, 2, CLK_DIV_MASK);
-        spin_unlock_irqrestore(&cam->dev_lock, flags);
-}
-
-
-/*
- * Stop the controller, and don't return until we're really sure that no
- * further DMA is going on.
- */
-static void cafe_ctlr_stop_dma(struct cafe_camera *cam)
-{
-        unsigned long flags;
-
-        /*
-         * Theory: stop the camera controller (whether it is operating
-         * or not).  Delay briefly just in case we race with the SOF
-         * interrupt, then wait until no DMA is active.
-         */
-        spin_lock_irqsave(&cam->dev_lock, flags);
-        cafe_ctlr_stop(cam);
-        spin_unlock_irqrestore(&cam->dev_lock, flags);
-        mdelay(1);
-        wait_event_timeout(cam->iowait,
-                        !test_bit(CF_DMA_ACTIVE, &cam->flags), HZ);
-        if (test_bit(CF_DMA_ACTIVE, &cam->flags))
-                cam_err(cam, "Timeout waiting for DMA to end\n");
-                /* This would be bad news - what now? */
-        spin_lock_irqsave(&cam->dev_lock, flags);
-        cam->state = S_IDLE;
-        cafe_ctlr_irq_disable(cam);
-        spin_unlock_irqrestore(&cam->dev_lock, flags);
-}
-
-/*
- * Power up and down.
- */
-static void cafe_ctlr_power_up(struct cafe_camera *cam)
-{
-        unsigned long flags;
-
-        spin_lock_irqsave(&cam->dev_lock, flags);
-        cafe_reg_clear_bit(cam, REG_CTRL1, C1_PWRDWN);
-        /*
-         * Part one of the sensor dance: turn the global
-         * GPIO signal on.
-         */
-        cafe_reg_write(cam, REG_GL_FCR, GFCR_GPIO_ON);
-        cafe_reg_write(cam, REG_GL_GPIOR, GGPIO_OUT|GGPIO_VAL);
-        /*
-         * Put the sensor into operational mode (assumes OLPC-style
-         * wiring).  Control 0 is reset - set to 1 to operate.
-         * Control 1 is power down, set to 0 to operate.
-         */
-        cafe_reg_write(cam, REG_GPR, GPR_C1EN|GPR_C0EN); /* pwr up, reset */
-/*      mdelay(1); */ /* Marvell says 1ms will do it */
-        cafe_reg_write(cam, REG_GPR, GPR_C1EN|GPR_C0EN|GPR_C0);
-/*      mdelay(1); */ /* Enough? */
-        spin_unlock_irqrestore(&cam->dev_lock, flags);
-        msleep(5); /* Just to be sure */
-}
-
-static void cafe_ctlr_power_down(struct cafe_camera *cam)
-{
-        unsigned long flags;
-
-        spin_lock_irqsave(&cam->dev_lock, flags);
-        cafe_reg_write(cam, REG_GPR, GPR_C1EN|GPR_C0EN|GPR_C1);
-        cafe_reg_write(cam, REG_GL_FCR, GFCR_GPIO_ON);
-        cafe_reg_write(cam, REG_GL_GPIOR, GGPIO_OUT);
-        cafe_reg_set_bit(cam, REG_CTRL1, C1_PWRDWN);
-        spin_unlock_irqrestore(&cam->dev_lock, flags);
-}
-
-/* -------------------------------------------------------------------- */
-/*
- * Communications with the sensor.
- */
-
-static int __cafe_cam_reset(struct cafe_camera *cam)
-{
-        return sensor_call(cam, core, reset, 0);
-}
-
-/*
- * We have found the sensor on the i2c.  Let's try to have a
- * conversation.
- */
-static int cafe_cam_init(struct cafe_camera *cam)
-{
-        struct v4l2_dbg_chip_ident chip;
-        int ret;
-
-        mutex_lock(&cam->s_mutex);
-        if (cam->state != S_NOTREADY)
-                cam_warn(cam, "Cam init with device in funky state %d",
-                                cam->state);
-        ret = __cafe_cam_reset(cam);
-        if (ret)
-                goto out;
-        chip.ident = V4L2_IDENT_NONE;
-        chip.match.type = V4L2_CHIP_MATCH_I2C_ADDR;
-        chip.match.addr = cam->sensor_addr;
-        ret = sensor_call(cam, core, g_chip_ident, &chip);
-        if (ret)
-                goto out;
-        cam->sensor_type = chip.ident;
-        if (cam->sensor_type != V4L2_IDENT_OV7670) {
-                cam_err(cam, "Unsupported sensor type 0x%x", cam->sensor_type);
-                ret = -EINVAL;
-                goto out;
-        }
-/* Get/set parameters? */
-        ret = 0;
-        cam->state = S_IDLE;
-  out:
-        cafe_ctlr_power_down(cam);
-        mutex_unlock(&cam->s_mutex);
-        return ret;
-}
-
-/*
- * Configure the sensor to match the parameters we have.  Caller should
- * hold s_mutex
- */
-static int cafe_cam_set_flip(struct cafe_camera *cam)
-{
-        struct v4l2_control ctrl;
-
-        memset(&ctrl, 0, sizeof(ctrl));
-        ctrl.id = V4L2_CID_VFLIP;
-        ctrl.value = flip;
-        return sensor_call(cam, core, s_ctrl, &ctrl);
-}
-
-
-static int cafe_cam_configure(struct cafe_camera *cam)
-{
-        struct v4l2_mbus_framefmt mbus_fmt;
-        int ret;
-
-        v4l2_fill_mbus_format(&mbus_fmt, &cam->pix_format, cam->mbus_code);
-        ret = sensor_call(cam, core, init, 0);
-        if (ret == 0)
-                ret = sensor_call(cam, video, s_mbus_fmt, &mbus_fmt);
-        /*
-         * OV7670 does weird things if flip is set *before* format...
-         */
-        ret += cafe_cam_set_flip(cam);
-        return ret;
-}
-
-/* -------------------------------------------------------------------- */
-/*
- * DMA buffer management.  These functions need s_mutex held.
- */
-
-/* FIXME: this is inefficient as hell, since dma_alloc_coherent just
- * does a get_free_pages() call, and we waste a good chunk of an orderN
- * allocation.  Should try to allocate the whole set in one chunk.
- */
-static int cafe_alloc_dma_bufs(struct cafe_camera *cam, int loadtime)
-{
-        int i;
-
-        cafe_set_config_needed(cam, 1);
-        if (loadtime)
-                cam->dma_buf_size = dma_buf_size;
-        else
-                cam->dma_buf_size = cam->pix_format.sizeimage;
-        if (n_dma_bufs > 3)
-                n_dma_bufs = 3;
-
-        cam->nbufs = 0;
-        for (i = 0; i < n_dma_bufs; i++) {
-                cam->dma_bufs[i] = dma_alloc_coherent(&cam->pdev->dev,
-                                cam->dma_buf_size, cam->dma_handles + i,
-                                GFP_KERNEL);
-                if (cam->dma_bufs[i] == NULL) {
-                        cam_warn(cam, "Failed to allocate DMA buffer\n");
-                        break;
-                }
-                /* For debug, remove eventually */
-                memset(cam->dma_bufs[i], 0xcc, cam->dma_buf_size);
-                (cam->nbufs)++;
-        }
-
-        switch (cam->nbufs) {
-        case 1:
-            dma_free_coherent(&cam->pdev->dev, cam->dma_buf_size,
-                            cam->dma_bufs[0], cam->dma_handles[0]);
-            cam->nbufs = 0;
-        case 0:
-            cam_err(cam, "Insufficient DMA buffers, cannot operate\n");
-            return -ENOMEM;
-
-        case 2:
-            if (n_dma_bufs > 2)
-                    cam_warn(cam, "Will limp along with only 2 buffers\n");
-            break;
-        }
-        return 0;
-}
-
-static void cafe_free_dma_bufs(struct cafe_camera *cam)
-{
-        int i;
-
-        for (i = 0; i < cam->nbufs; i++) {
-                dma_free_coherent(&cam->pdev->dev, cam->dma_buf_size,
-                                cam->dma_bufs[i], cam->dma_handles[i]);
-                cam->dma_bufs[i] = NULL;
-        }
-        cam->nbufs = 0;
-}
-
-
-
-
-
-/* ----------------------------------------------------------------------- */
-/*
- * Here starts the V4L2 interface code.
- */
-
-/*
- * Read an image from the device.
- */
-static ssize_t cafe_deliver_buffer(struct cafe_camera *cam,
-                char __user *buffer, size_t len, loff_t *pos)
-{
-        int bufno;
-        unsigned long flags;
-
-        spin_lock_irqsave(&cam->dev_lock, flags);
-        if (cam->next_buf < 0) {
-                cam_err(cam, "deliver_buffer: No next buffer\n");
-                spin_unlock_irqrestore(&cam->dev_lock, flags);
-                return -EIO;
-        }
-        bufno = cam->next_buf;
-        clear_bit(bufno, &cam->flags);
-        if (++(cam->next_buf) >= cam->nbufs)
-                cam->next_buf = 0;
-        if (! test_bit(cam->next_buf, &cam->flags))
-                cam->next_buf = -1;
-        cam->specframes = 0;
-        spin_unlock_irqrestore(&cam->dev_lock, flags);
-
-        if (len > cam->pix_format.sizeimage)
-                len = cam->pix_format.sizeimage;
-        if (copy_to_user(buffer, cam->dma_bufs[bufno], len))
-                return -EFAULT;
-        (*pos) += len;
-        return len;
-}
-
-/*
- * Get everything ready, and start grabbing frames.
- */
-static int cafe_read_setup(struct cafe_camera *cam, enum cafe_state state)
-{
-        int ret;
-        unsigned long flags;
-
-        /*
-         * Configuration.  If we still don't have DMA buffers,
-         * make one last, desperate attempt.
-         */
-        if (cam->nbufs == 0)
-                if (cafe_alloc_dma_bufs(cam, 0))
-                        return -ENOMEM;
-
-        if (cafe_needs_config(cam)) {
-                cafe_cam_configure(cam);
-                ret = cafe_ctlr_configure(cam);
-                if (ret)
-                        return ret;
-        }
-
-        /*
-         * Turn it loose.
-         */
-        spin_lock_irqsave(&cam->dev_lock, flags);
-        cafe_reset_buffers(cam);
-        cafe_ctlr_irq_enable(cam);
-        cam->state = state;
-        cafe_ctlr_start(cam);
-        spin_unlock_irqrestore(&cam->dev_lock, flags);
-        return 0;
-}
-
-
-static ssize_t cafe_v4l_read(struct file *filp,
-                char __user *buffer, size_t len, loff_t *pos)
-{
-        struct cafe_camera *cam = filp->private_data;
-        int ret = 0;
-
-        /*
-         * Perhaps we're in speculative read mode and already
-         * have data?
-         */
-        mutex_lock(&cam->s_mutex);
-        if (cam->state == S_SPECREAD) {
-                if (cam->next_buf >= 0) {
-                        ret = cafe_deliver_buffer(cam, buffer, len, pos);
-                        if (ret != 0)
-                                goto out_unlock;
-                }
-        } else if (cam->state == S_FLAKED || cam->state == S_NOTREADY) {
-                ret = -EIO;
-                goto out_unlock;
-        } else if (cam->state != S_IDLE) {
-                ret = -EBUSY;
-                goto out_unlock;
-        }
-
-        /*
-         * v4l2: multiple processes can open the device, but only
-         * one gets to grab data from it.
-         */
-        if (cam->owner && cam->owner != filp) {
-                ret = -EBUSY;
-                goto out_unlock;
-        }
-        cam->owner = filp;
-
-        /*
-         * Do setup if need be.
-         */
-        if (cam->state != S_SPECREAD) {
-                ret = cafe_read_setup(cam, S_SINGLEREAD);
-                if (ret)
-                        goto out_unlock;
-        }
-        /*
-         * Wait for something to happen.  This should probably
-         * be interruptible (FIXME).
-         */
-        wait_event_timeout(cam->iowait, cam->next_buf >= 0, HZ);
-        if (cam->next_buf < 0) {
-                cam_err(cam, "read() operation timed out\n");
-                cafe_ctlr_stop_dma(cam);
-                ret = -EIO;
-                goto out_unlock;
-        }
-        /*
-         * Give them their data and we should be done.
-         */
-        ret = cafe_deliver_buffer(cam, buffer, len, pos);
-
-  out_unlock:
-        mutex_unlock(&cam->s_mutex);
-        return ret;
-}
-
-
-
-
-
-
-
-
-/*
- * Streaming I/O support.
- */
-
-
-
-static int cafe_vidioc_streamon(struct file *filp, void *priv,
-                enum v4l2_buf_type type)
-{
-        struct cafe_camera *cam = filp->private_data;
-        int ret = -EINVAL;
-
-        if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-                goto out;
-        mutex_lock(&cam->s_mutex);
-        if (cam->state != S_IDLE || cam->n_sbufs == 0)
-                goto out_unlock;
-
-        cam->sequence = 0;
-        ret = cafe_read_setup(cam, S_STREAMING);
-
-  out_unlock:
-        mutex_unlock(&cam->s_mutex);
-  out:
-        return ret;
-}
-
-
-static int cafe_vidioc_streamoff(struct file *filp, void *priv,
-                enum v4l2_buf_type type)
-{
-        struct cafe_camera *cam = filp->private_data;
-        int ret = -EINVAL;
-
-        if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-                goto out;
-        mutex_lock(&cam->s_mutex);
-        if (cam->state != S_STREAMING)
-                goto out_unlock;
-
-        cafe_ctlr_stop_dma(cam);
-        ret = 0;
-
-  out_unlock:
-        mutex_unlock(&cam->s_mutex);
-  out:
-        return ret;
-}
-
-
-
-static int cafe_setup_siobuf(struct cafe_camera *cam, int index)
-{
-        struct cafe_sio_buffer *buf = cam->sb_bufs + index;
-
-        INIT_LIST_HEAD(&buf->list);
-        buf->v4lbuf.length = PAGE_ALIGN(cam->pix_format.sizeimage);
-        buf->buffer = vmalloc_user(buf->v4lbuf.length);
-        if (buf->buffer == NULL)
-                return -ENOMEM;
-        buf->mapcount = 0;
-        buf->cam = cam;
-
-        buf->v4lbuf.index = index;
-        buf->v4lbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-        buf->v4lbuf.field = V4L2_FIELD_NONE;
-        buf->v4lbuf.memory = V4L2_MEMORY_MMAP;
-        /*
-         * Offset: must be 32-bit even on a 64-bit system.  videobuf-dma-sg
-         * just uses the length times the index, but the spec warns
-         * against doing just that - vma merging problems.  So we
-         * leave a gap between each pair of buffers.
-         */
-        buf->v4lbuf.m.offset = 2*index*buf->v4lbuf.length;
-        return 0;
-}
-
-static int cafe_free_sio_buffers(struct cafe_camera *cam)
-{
-        int i;
-
-        /*
-         * If any buffers are mapped, we cannot free them at all.
-         */
-        for (i = 0; i < cam->n_sbufs; i++)
-                if (cam->sb_bufs[i].mapcount > 0)
-                        return -EBUSY;
-        /*
-         * OK, let's do it.
-         */
-        for (i = 0; i < cam->n_sbufs; i++)
-                vfree(cam->sb_bufs[i].buffer);
-        cam->n_sbufs = 0;
-        kfree(cam->sb_bufs);
-        cam->sb_bufs = NULL;
-        INIT_LIST_HEAD(&cam->sb_avail);
-        INIT_LIST_HEAD(&cam->sb_full);
-        return 0;
-}
-
-
-
-static int cafe_vidioc_reqbufs(struct file *filp, void *priv,
-                struct v4l2_requestbuffers *req)
-{
-        struct cafe_camera *cam = filp->private_data;
-        int ret = 0;  /* Silence warning */
-
-        /*
-         * Make sure it's something we can do.  User pointers could be
-         * implemented without great pain, but that's not been done yet.
-         */
-        if (req->memory != V4L2_MEMORY_MMAP)
-                return -EINVAL;
-        /*
-         * If they ask for zero buffers, they really want us to stop streaming
-         * (if it's happening) and free everything.  Should we check owner?
-         */
-        mutex_lock(&cam->s_mutex);
-        if (req->count == 0) {
-                if (cam->state == S_STREAMING)
-                        cafe_ctlr_stop_dma(cam);
-                ret = cafe_free_sio_buffers (cam);
-                goto out;
-        }
-        /*
-         * Device needs to be idle and working.  We *could* try to do the
-         * right thing in S_SPECREAD by shutting things down, but it
-         * probably doesn't matter.
-         */
-        if (cam->state != S_IDLE || (cam->owner && cam->owner != filp)) {
-                ret = -EBUSY;
-                goto out;
-        }
-        cam->owner = filp;
-
-        if (req->count < min_buffers)
-                req->count = min_buffers;
-        else if (req->count > max_buffers)
-                req->count = max_buffers;
-        if (cam->n_sbufs > 0) {
-                ret = cafe_free_sio_buffers(cam);
-                if (ret)
-                        goto out;
-        }
-
-        cam->sb_bufs = kzalloc(req->count*sizeof(struct cafe_sio_buffer),
-                        GFP_KERNEL);
-        if (cam->sb_bufs == NULL) {
-                ret = -ENOMEM;
-                goto out;
-        }
-        for (cam->n_sbufs = 0; cam->n_sbufs < req->count; (cam->n_sbufs++)) {
-                ret = cafe_setup_siobuf(cam, cam->n_sbufs);
-                if (ret)
-                        break;
-        }
-
-        if (cam->n_sbufs == 0)  /* no luck at all - ret already set */
-                kfree(cam->sb_bufs);
-        req->count = cam->n_sbufs;  /* In case of partial success */
-
-  out:
-        mutex_unlock(&cam->s_mutex);
-        return ret;
-}
-
-
-static int cafe_vidioc_querybuf(struct file *filp, void *priv,
-                struct v4l2_buffer *buf)
-{
-        struct cafe_camera *cam = filp->private_data;
-        int ret = -EINVAL;
-
-        mutex_lock(&cam->s_mutex);
-        if (buf->index >= cam->n_sbufs)
-                goto out;
-        *buf = cam->sb_bufs[buf->index].v4lbuf;
-        ret = 0;
-  out:
-        mutex_unlock(&cam->s_mutex);
-        return ret;
-}
-
-static int cafe_vidioc_qbuf(struct file *filp, void *priv,
-                struct v4l2_buffer *buf)
-{
-        struct cafe_camera *cam = filp->private_data;
-        struct cafe_sio_buffer *sbuf;
-        int ret = -EINVAL;
-        unsigned long flags;
-
-        mutex_lock(&cam->s_mutex);
-        if (buf->index >= cam->n_sbufs)
-                goto out;
-        sbuf = cam->sb_bufs + buf->index;
-        if (sbuf->v4lbuf.flags & V4L2_BUF_FLAG_QUEUED) {
-                ret = 0; /* Already queued?? */
-                goto out;
-        }
-        if (sbuf->v4lbuf.flags & V4L2_BUF_FLAG_DONE) {
-                /* Spec doesn't say anything, seems appropriate tho */
-                ret = -EBUSY;
-                goto out;
-        }
-        sbuf->v4lbuf.flags |= V4L2_BUF_FLAG_QUEUED;
-        spin_lock_irqsave(&cam->dev_lock, flags);
-        list_add(&sbuf->list, &cam->sb_avail);
-        spin_unlock_irqrestore(&cam->dev_lock, flags);
-        ret = 0;
-  out:
-        mutex_unlock(&cam->s_mutex);
-        return ret;
-}
-
-static int cafe_vidioc_dqbuf(struct file *filp, void *priv,
-                struct v4l2_buffer *buf)
-{
-        struct cafe_camera *cam = filp->private_data;
-        struct cafe_sio_buffer *sbuf;
-        int ret = -EINVAL;
-        unsigned long flags;
-
-        mutex_lock(&cam->s_mutex);
-        if (cam->state != S_STREAMING)
-                goto out_unlock;
-        if (list_empty(&cam->sb_full) && filp->f_flags & O_NONBLOCK) {
-                ret = -EAGAIN;
-                goto out_unlock;
-        }
-
-        while (list_empty(&cam->sb_full) && cam->state == S_STREAMING) {
-                mutex_unlock(&cam->s_mutex);
-                if (wait_event_interruptible(cam->iowait,
-                                                !list_empty(&cam->sb_full))) {
-                        ret = -ERESTARTSYS;
-                        goto out;
-                }
-                mutex_lock(&cam->s_mutex);
-        }
-
-        if (cam->state != S_STREAMING)
-                ret = -EINTR;
-        else {
-                spin_lock_irqsave(&cam->dev_lock, flags);
-                /* Should probably recheck !list_empty() here */
-                sbuf = list_entry(cam->sb_full.next,
-                                struct cafe_sio_buffer, list);
-                list_del_init(&sbuf->list);
-                spin_unlock_irqrestore(&cam->dev_lock, flags);
-                sbuf->v4lbuf.flags &= ~V4L2_BUF_FLAG_DONE;
-                *buf = sbuf->v4lbuf;
-                ret = 0;
-        }
-
-  out_unlock:
-        mutex_unlock(&cam->s_mutex);
-  out:
-        return ret;
-}
-
-
-
-static void cafe_v4l_vm_open(struct vm_area_struct *vma)
-{
-        struct cafe_sio_buffer *sbuf = vma->vm_private_data;
-        /*
-         * Locking: done under mmap_sem, so we don't need to
-         * go back to the camera lock here.
-         */
-        sbuf->mapcount++;
-}
-
-
-static void cafe_v4l_vm_close(struct vm_area_struct *vma)
-{
-        struct cafe_sio_buffer *sbuf = vma->vm_private_data;
-
-        mutex_lock(&sbuf->cam->s_mutex);
-        sbuf->mapcount--;
-        /* Docs say we should stop I/O too... */
-        if (sbuf->mapcount == 0)
-                sbuf->v4lbuf.flags &= ~V4L2_BUF_FLAG_MAPPED;
-        mutex_unlock(&sbuf->cam->s_mutex);
-}
-
-static const struct vm_operations_struct cafe_v4l_vm_ops = {
-        .open = cafe_v4l_vm_open,
-        .close = cafe_v4l_vm_close
-};
-
-
-static int cafe_v4l_mmap(struct file *filp, struct vm_area_struct *vma)
-{
-        struct cafe_camera *cam = filp->private_data;
-        unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
-        int ret = -EINVAL;
-        int i;
-        struct cafe_sio_buffer *sbuf = NULL;
-
-        if (! (vma->vm_flags & VM_WRITE) || ! (vma->vm_flags & VM_SHARED))
-                return -EINVAL;
-        /*
-         * Find the buffer they are looking for.
-         */
-        mutex_lock(&cam->s_mutex);
-        for (i = 0; i < cam->n_sbufs; i++)
-                if (cam->sb_bufs[i].v4lbuf.m.offset == offset) {
-                        sbuf = cam->sb_bufs + i;
-                        break;
-                }
-        if (sbuf == NULL)
-                goto out;
-
-        ret = remap_vmalloc_range(vma, sbuf->buffer, 0);
-        if (ret)
-                goto out;
-        vma->vm_flags |= VM_DONTEXPAND;
-        vma->vm_private_data = sbuf;
-        vma->vm_ops = &cafe_v4l_vm_ops;
-        sbuf->v4lbuf.flags |= V4L2_BUF_FLAG_MAPPED;
-        cafe_v4l_vm_open(vma);
-        ret = 0;
-  out:
-        mutex_unlock(&cam->s_mutex);
-        return ret;
-}
-
-
-
-static int cafe_v4l_open(struct file *filp)
-{
-        struct cafe_camera *cam = video_drvdata(filp);
-
-        filp->private_data = cam;
-
-        mutex_lock(&cam->s_mutex);
-        if (cam->users == 0) {
-                cafe_ctlr_power_up(cam);
-                __cafe_cam_reset(cam);
-                cafe_set_config_needed(cam, 1);
-        /* FIXME make sure this is complete */
-        }
-        (cam->users)++;
-        mutex_unlock(&cam->s_mutex);
-        return 0;
-}
-
-
-static int cafe_v4l_release(struct file *filp)
-{
-        struct cafe_camera *cam = filp->private_data;
-
-        mutex_lock(&cam->s_mutex);
-        (cam->users)--;
-        if (filp == cam->owner) {
-                cafe_ctlr_stop_dma(cam);
-                cafe_free_sio_buffers(cam);
-                cam->owner = NULL;
-        }
-        if (cam->users == 0) {
-                cafe_ctlr_power_down(cam);
-                if (alloc_bufs_at_read)
-                        cafe_free_dma_bufs(cam);
-        }
-        mutex_unlock(&cam->s_mutex);
-        return 0;
-}
-
-
-
-static unsigned int cafe_v4l_poll(struct file *filp,
-                struct poll_table_struct *pt)
-{
-        struct cafe_camera *cam = filp->private_data;
-
-        poll_wait(filp, &cam->iowait, pt);
-        if (cam->next_buf >= 0)
-                return POLLIN | POLLRDNORM;
-        return 0;
-}
-
-
-
-static int cafe_vidioc_queryctrl(struct file *filp, void *priv,
-                struct v4l2_queryctrl *qc)
-{
-        struct cafe_camera *cam = priv;
-        int ret;
-
-        mutex_lock(&cam->s_mutex);
-        ret = sensor_call(cam, core, queryctrl, qc);
-        mutex_unlock(&cam->s_mutex);
-        return ret;
-}
-
-
-static int cafe_vidioc_g_ctrl(struct file *filp, void *priv,
-                struct v4l2_control *ctrl)
-{
-        struct cafe_camera *cam = priv;
-        int ret;
-
-        mutex_lock(&cam->s_mutex);
-        ret = sensor_call(cam, core, g_ctrl, ctrl);
-        mutex_unlock(&cam->s_mutex);
-        return ret;
-}
-
-
-static int cafe_vidioc_s_ctrl(struct file *filp, void *priv,
-                struct v4l2_control *ctrl)
-{
-        struct cafe_camera *cam = priv;
-        int ret;
-
-        mutex_lock(&cam->s_mutex);
-        ret = sensor_call(cam, core, s_ctrl, ctrl);
-        mutex_unlock(&cam->s_mutex);
-        return ret;
-}
-
-
-
-
-
-static int cafe_vidioc_querycap(struct file *file, void *priv,
-                struct v4l2_capability *cap)
-{
-        strcpy(cap->driver, "cafe_ccic");
-        strcpy(cap->card, "cafe_ccic");
-        cap->version = CAFE_VERSION;
-        cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
-                V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
-        return 0;
-}
-
-
-/*
- * The default format we use until somebody says otherwise.
- */
-static const struct v4l2_pix_format cafe_def_pix_format = {
-        .width          = VGA_WIDTH,
-        .height         = VGA_HEIGHT,
-        .pixelformat    = V4L2_PIX_FMT_YUYV,
-        .field          = V4L2_FIELD_NONE,
-        .bytesperline   = VGA_WIDTH*2,
-        .sizeimage      = VGA_WIDTH*VGA_HEIGHT*2,
-};
-
-static const enum v4l2_mbus_pixelcode cafe_def_mbus_code =
-                                        V4L2_MBUS_FMT_YUYV8_2X8;
-
-static int cafe_vidioc_enum_fmt_vid_cap(struct file *filp,
-                void *priv, struct v4l2_fmtdesc *fmt)
-{
-        if (fmt->index >= N_CAFE_FMTS)
-                return -EINVAL;
-        strlcpy(fmt->description, cafe_formats[fmt->index].desc,
-                        sizeof(fmt->description));
-        fmt->pixelformat = cafe_formats[fmt->index].pixelformat;
-        return 0;
-}
-
-static int cafe_vidioc_try_fmt_vid_cap(struct file *filp, void *priv,
-                struct v4l2_format *fmt)
-{
-        struct cafe_camera *cam = priv;
-        struct cafe_format_struct *f;
-        struct v4l2_pix_format *pix = &fmt->fmt.pix;
-        struct v4l2_mbus_framefmt mbus_fmt;
-        int ret;
-
-        f = cafe_find_format(pix->pixelformat);
-        pix->pixelformat = f->pixelformat;
-        v4l2_fill_mbus_format(&mbus_fmt, pix, f->mbus_code);
-        mutex_lock(&cam->s_mutex);
-        ret = sensor_call(cam, video, try_mbus_fmt, &mbus_fmt);
-        mutex_unlock(&cam->s_mutex);
-        v4l2_fill_pix_format(pix, &mbus_fmt);
-        pix->bytesperline = pix->width * f->bpp;
-        pix->sizeimage = pix->height * pix->bytesperline;
-        return ret;
-}
-
-static int cafe_vidioc_s_fmt_vid_cap(struct file *filp, void *priv,
-                struct v4l2_format *fmt)
-{
-        struct cafe_camera *cam = priv;
-        struct cafe_format_struct *f;
-        int ret;
-
-        /*
-         * Can't do anything if the device is not idle
-         * Also can't if there are streaming buffers in place.
-         */
-        if (cam->state != S_IDLE || cam->n_sbufs > 0)
-                return -EBUSY;
-
-        f = cafe_find_format(fmt->fmt.pix.pixelformat);
-
-        /*
-         * See if the formatting works in principle.
-         */
-        ret = cafe_vidioc_try_fmt_vid_cap(filp, priv, fmt);
-        if (ret)
-                return ret;
-        /*
-         * Now we start to change things for real, so let's do it
-         * under lock.
-         */
-        mutex_lock(&cam->s_mutex);
-        cam->pix_format = fmt->fmt.pix;
-        cam->mbus_code = f->mbus_code;
-
-        /*
-         * Make sure we have appropriate DMA buffers.
-         */
-        ret = -ENOMEM;
-        if (cam->nbufs > 0 && cam->dma_buf_size < cam->pix_format.sizeimage)
-                cafe_free_dma_bufs(cam);
-        if (cam->nbufs == 0) {
-                if (cafe_alloc_dma_bufs(cam, 0))
-                        goto out;
-        }
-        /*
-         * It looks like this might work, so let's program the sensor.
-         */
-        ret = cafe_cam_configure(cam);
-        if (! ret)
-                ret = cafe_ctlr_configure(cam);
-  out:
-        mutex_unlock(&cam->s_mutex);
-        return ret;
-}
-
-/*
- * Return our stored notion of how the camera is/should be configured.
- * The V4l2 spec wants us to be smarter, and actually get this from
- * the camera (and not mess with it at open time).  Someday.
- */
-static int cafe_vidioc_g_fmt_vid_cap(struct file *filp, void *priv,
-                struct v4l2_format *f)
-{
-        struct cafe_camera *cam = priv;
-
-        f->fmt.pix = cam->pix_format;
-        return 0;
-}
-
-/*
- * We only have one input - the sensor - so minimize the nonsense here.
- */
-static int cafe_vidioc_enum_input(struct file *filp, void *priv,
-                struct v4l2_input *input)
-{
-        if (input->index != 0)
-                return -EINVAL;
-
-        input->type = V4L2_INPUT_TYPE_CAMERA;
-        input->std = V4L2_STD_ALL; /* Not sure what should go here */
-        strcpy(input->name, "Camera");
-        return 0;
-}
-
-static int cafe_vidioc_g_input(struct file *filp, void *priv, unsigned int *i)
-{
-        *i = 0;
-        return 0;
-}
-
-static int cafe_vidioc_s_input(struct file *filp, void *priv, unsigned int i)
-{
-        if (i != 0)
-                return -EINVAL;
-        return 0;
-}
-
-/* from vivi.c */
-static int cafe_vidioc_s_std(struct file *filp, void *priv, v4l2_std_id *a)
-{
-        return 0;
-}
-
-/*
- * G/S_PARM.  Most of this is done by the sensor, but we are
- * the level which controls the number of read buffers.
- */
-static int cafe_vidioc_g_parm(struct file *filp, void *priv,
-                struct v4l2_streamparm *parms)
-{
-        struct cafe_camera *cam = priv;
-        int ret;
-
-        mutex_lock(&cam->s_mutex);
-        ret = sensor_call(cam, video, g_parm, parms);
-        mutex_unlock(&cam->s_mutex);
-        parms->parm.capture.readbuffers = n_dma_bufs;
-        return ret;
-}
-
-static int cafe_vidioc_s_parm(struct file *filp, void *priv,
-                struct v4l2_streamparm *parms)
-{
-        struct cafe_camera *cam = priv;
-        int ret;
-
-        mutex_lock(&cam->s_mutex);
-        ret = sensor_call(cam, video, s_parm, parms);
-        mutex_unlock(&cam->s_mutex);
-        parms->parm.capture.readbuffers = n_dma_bufs;
-        return ret;
-}
-
-static int cafe_vidioc_g_chip_ident(struct file *file, void *priv,
-                struct v4l2_dbg_chip_ident *chip)
-{
-        struct cafe_camera *cam = priv;
-
-        chip->ident = V4L2_IDENT_NONE;
-        chip->revision = 0;
-        if (v4l2_chip_match_host(&chip->match)) {
-                chip->ident = V4L2_IDENT_CAFE;
-                return 0;
-        }
-        return sensor_call(cam, core, g_chip_ident, chip);
-}
-
-static int cafe_vidioc_enum_framesizes(struct file *filp, void *priv,
-                struct v4l2_frmsizeenum *sizes)
-{
-        struct cafe_camera *cam = priv;
-        int ret;
-
-        mutex_lock(&cam->s_mutex);
-        ret = sensor_call(cam, video, enum_framesizes, sizes);
-        mutex_unlock(&cam->s_mutex);
-        return ret;
-}
-
-static int cafe_vidioc_enum_frameintervals(struct file *filp, void *priv,
-                struct v4l2_frmivalenum *interval)
-{
-        struct cafe_camera *cam = priv;
-        int ret;
-
-        mutex_lock(&cam->s_mutex);
-        ret = sensor_call(cam, video, enum_frameintervals, interval);
-        mutex_unlock(&cam->s_mutex);
-        return ret;
-}
-
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-static int cafe_vidioc_g_register(struct file *file, void *priv,
-                struct v4l2_dbg_register *reg)
-{
-        struct cafe_camera *cam = priv;
-
-        if (v4l2_chip_match_host(&reg->match)) {
-                reg->val = cafe_reg_read(cam, reg->reg);
-                reg->size = 4;
-                return 0;
-        }
-        return sensor_call(cam, core, g_register, reg);
-}
-
-static int cafe_vidioc_s_register(struct file *file, void *priv,
-                struct v4l2_dbg_register *reg)
-{
-        struct cafe_camera *cam = priv;
-
-        if (v4l2_chip_match_host(&reg->match)) {
-                cafe_reg_write(cam, reg->reg, reg->val);
-                return 0;
-        }
-        return sensor_call(cam, core, s_register, reg);
-}
-#endif
-
-/*
- * This template device holds all of those v4l2 methods; we
- * clone it for specific real devices.
- */
-
-static const struct v4l2_file_operations cafe_v4l_fops = {
-        .owner = THIS_MODULE,
-        .open = cafe_v4l_open,
-        .release = cafe_v4l_release,
-        .read = cafe_v4l_read,
-        .poll = cafe_v4l_poll,
-        .mmap = cafe_v4l_mmap,
-        .unlocked_ioctl = video_ioctl2,
-};
-
-static const struct v4l2_ioctl_ops cafe_v4l_ioctl_ops = {
-        .vidioc_querycap        = cafe_vidioc_querycap,
-        .vidioc_enum_fmt_vid_cap = cafe_vidioc_enum_fmt_vid_cap,
-        .vidioc_try_fmt_vid_cap = cafe_vidioc_try_fmt_vid_cap,
-        .vidioc_s_fmt_vid_cap   = cafe_vidioc_s_fmt_vid_cap,
-        .vidioc_g_fmt_vid_cap   = cafe_vidioc_g_fmt_vid_cap,
-        .vidioc_enum_input      = cafe_vidioc_enum_input,
-        .vidioc_g_input         = cafe_vidioc_g_input,
-        .vidioc_s_input         = cafe_vidioc_s_input,
-        .vidioc_s_std           = cafe_vidioc_s_std,
-        .vidioc_reqbufs         = cafe_vidioc_reqbufs,
-        .vidioc_querybuf        = cafe_vidioc_querybuf,
-        .vidioc_qbuf            = cafe_vidioc_qbuf,
-        .vidioc_dqbuf           = cafe_vidioc_dqbuf,
-        .vidioc_streamon        = cafe_vidioc_streamon,
-        .vidioc_streamoff       = cafe_vidioc_streamoff,
-        .vidioc_queryctrl       = cafe_vidioc_queryctrl,
-        .vidioc_g_ctrl          = cafe_vidioc_g_ctrl,
-        .vidioc_s_ctrl          = cafe_vidioc_s_ctrl,
-        .vidioc_g_parm          = cafe_vidioc_g_parm,
-        .vidioc_s_parm          = cafe_vidioc_s_parm,
-        .vidioc_enum_framesizes = cafe_vidioc_enum_framesizes,
-        .vidioc_enum_frameintervals = cafe_vidioc_enum_frameintervals,
-        .vidioc_g_chip_ident    = cafe_vidioc_g_chip_ident,
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-        .vidioc_g_register      = cafe_vidioc_g_register,
-        .vidioc_s_register      = cafe_vidioc_s_register,
-#endif
-};
-
-static struct video_device cafe_v4l_template = {
-        .name = "cafe",
-        .tvnorms = V4L2_STD_NTSC_M,
-        .current_norm = V4L2_STD_NTSC_M,  /* make mplayer happy */
-
-        .fops = &cafe_v4l_fops,
-        .ioctl_ops = &cafe_v4l_ioctl_ops,
-        .release = video_device_release_empty,
-};
-
-
-/* ---------------------------------------------------------------------- */
-/*
- * Interrupt handler stuff
- */
-
-
-
-static void cafe_frame_tasklet(unsigned long data)
-{
-        struct cafe_camera *cam = (struct cafe_camera *) data;
-        int i;
-        unsigned long flags;
-        struct cafe_sio_buffer *sbuf;
-
-        spin_lock_irqsave(&cam->dev_lock, flags);
-        for (i = 0; i < cam->nbufs; i++) {
-                int bufno = cam->next_buf;
-                if (bufno < 0) {  /* "will never happen" */
-                        cam_err(cam, "No valid bufs in tasklet!\n");
-                        break;
-                }
-                if (++(cam->next_buf) >= cam->nbufs)
-                        cam->next_buf = 0;
-                if (! test_bit(bufno, &cam->flags))
-                        continue;
-                if (list_empty(&cam->sb_avail))
-                        break;  /* Leave it valid, hope for better later */
-                clear_bit(bufno, &cam->flags);
-                sbuf = list_entry(cam->sb_avail.next,
-                                struct cafe_sio_buffer, list);
-                /*
-                 * Drop the lock during the big copy.  This *should* be safe...
-                 */
-                spin_unlock_irqrestore(&cam->dev_lock, flags);
-                memcpy(sbuf->buffer, cam->dma_bufs[bufno],
-                                cam->pix_format.sizeimage);
-                sbuf->v4lbuf.bytesused = cam->pix_format.sizeimage;
-                sbuf->v4lbuf.sequence = cam->buf_seq[bufno];
-                sbuf->v4lbuf.flags &= ~V4L2_BUF_FLAG_QUEUED;
-                sbuf->v4lbuf.flags |= V4L2_BUF_FLAG_DONE;
-                spin_lock_irqsave(&cam->dev_lock, flags);
-                list_move_tail(&sbuf->list, &cam->sb_full);
-        }
-        if (! list_empty(&cam->sb_full))
-                wake_up(&cam->iowait);
-        spin_unlock_irqrestore(&cam->dev_lock, flags);
-}
-
-
-
-static void cafe_frame_complete(struct cafe_camera *cam, int frame)
-{
-        /*
-         * Basic frame housekeeping.
-         */
-        if (test_bit(frame, &cam->flags) && printk_ratelimit())
-                cam_err(cam, "Frame overrun on %d, frames lost\n", frame);
-        set_bit(frame, &cam->flags);
-        clear_bit(CF_DMA_ACTIVE, &cam->flags);
-        if (cam->next_buf < 0)
-                cam->next_buf = frame;
-        cam->buf_seq[frame] = ++(cam->sequence);
-
-        switch (cam->state) {
-        /*
-         * If in single read mode, try going speculative.
-         */
-            case S_SINGLEREAD:
-                cam->state = S_SPECREAD;
-                cam->specframes = 0;
-                wake_up(&cam->iowait);
-                break;
-
-        /*
-         * If we are already doing speculative reads, and nobody is
-         * reading them, just stop.
-         */
-            case S_SPECREAD:
-                if (++(cam->specframes) >= cam->nbufs) {
-                        cafe_ctlr_stop(cam);
-                        cafe_ctlr_irq_disable(cam);
-                        cam->state = S_IDLE;
-                }
-                wake_up(&cam->iowait);
-                break;
-        /*
-         * For the streaming case, we defer the real work to the
-         * camera tasklet.
-         *
-         * FIXME: if the application is not consuming the buffers,
-         * we should eventually put things on hold and restart in
-         * vidioc_dqbuf().
-         */
-            case S_STREAMING:
-                tasklet_schedule(&cam->s_tasklet);
-                break;
-
-            default:
-                cam_err(cam, "Frame interrupt in non-operational state\n");
-                break;
-        }
-}
-
-
-
-
-static void cafe_frame_irq(struct cafe_camera *cam, unsigned int irqs)
-{
-        unsigned int frame;
-
-        cafe_reg_write(cam, REG_IRQSTAT, FRAMEIRQS); /* Clear'em all */
-        /*
-         * Handle any frame completions.  There really should
-         * not be more than one of these, or we have fallen
-         * far behind.
-         */
-        for (frame = 0; frame < cam->nbufs; frame++)
-                if (irqs & (IRQ_EOF0 << frame))
-                        cafe_frame_complete(cam, frame);
-        /*
-         * If a frame starts, note that we have DMA active.  This
-         * code assumes that we won't get multiple frame interrupts
-         * at once; may want to rethink that.
-         */
-        if (irqs & (IRQ_SOF0 | IRQ_SOF1 | IRQ_SOF2))
-                set_bit(CF_DMA_ACTIVE, &cam->flags);
-}
-
-
-
-static irqreturn_t cafe_irq(int irq, void *data)
-{
-        struct cafe_camera *cam = data;
-        unsigned int irqs;
-
-        spin_lock(&cam->dev_lock);
-        irqs = cafe_reg_read(cam, REG_IRQSTAT);
-        if ((irqs & ALLIRQS) == 0) {
-                spin_unlock(&cam->dev_lock);
-                return IRQ_NONE;
-        }
-        if (irqs & FRAMEIRQS)
-                cafe_frame_irq(cam, irqs);
-        if (irqs & TWSIIRQS) {
-                cafe_reg_write(cam, REG_IRQSTAT, TWSIIRQS);
-                wake_up(&cam->smbus_wait);
-        }
-        spin_unlock(&cam->dev_lock);
-        return IRQ_HANDLED;
-}
-
-
-/* -------------------------------------------------------------------------- */
-/*
- * PCI interface stuff.
- */
-
-static const struct dmi_system_id olpc_xo1_dmi[] = {
-        {
-                .matches = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "OLPC"),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "XO"),
-                        DMI_MATCH(DMI_PRODUCT_VERSION, "1"),
-                },
-        },
-        { }
-};
-
-static int cafe_pci_probe(struct pci_dev *pdev,
-                const struct pci_device_id *id)
-{
-        int ret;
-        struct cafe_camera *cam;
-        struct ov7670_config sensor_cfg = {
-                /* This controller only does SMBUS */
-                .use_smbus = true,
-
-                /*
-                 * Exclude QCIF mode, because it only captures a tiny portion
-                 * of the sensor FOV
-                 */
-                .min_width = 320,
-                .min_height = 240,
-        };
-        struct i2c_board_info ov7670_info = {
-                .type = "ov7670",
-                .addr = 0x42,
-                .platform_data = &sensor_cfg,
-        };
-
-        /*
-         * Start putting together one of our big camera structures.
-         */
-        ret = -ENOMEM;
-        cam = kzalloc(sizeof(struct cafe_camera), GFP_KERNEL);
-        if (cam == NULL)
-                goto out;
-        ret = v4l2_device_register(&pdev->dev, &cam->v4l2_dev);
-        if (ret)
-                goto out_free;
-
-        mutex_init(&cam->s_mutex);
-        spin_lock_init(&cam->dev_lock);
-        cam->state = S_NOTREADY;
-        cafe_set_config_needed(cam, 1);
-        init_waitqueue_head(&cam->smbus_wait);
-        init_waitqueue_head(&cam->iowait);
-        cam->pdev = pdev;
-        cam->pix_format = cafe_def_pix_format;
-        cam->mbus_code = cafe_def_mbus_code;
-        INIT_LIST_HEAD(&cam->dev_list);
-        INIT_LIST_HEAD(&cam->sb_avail);
-        INIT_LIST_HEAD(&cam->sb_full);
-        tasklet_init(&cam->s_tasklet, cafe_frame_tasklet, (unsigned long) cam);
-        /*
-         * Get set up on the PCI bus.
-         */
-        ret = pci_enable_device(pdev);
-        if (ret)
-                goto out_unreg;
-        pci_set_master(pdev);
-
-        ret = -EIO;
-        cam->regs = pci_iomap(pdev, 0, 0);
-        if (! cam->regs) {
-                printk(KERN_ERR "Unable to ioremap cafe-ccic regs\n");
-                goto out_unreg;
-        }
-        ret = request_irq(pdev->irq, cafe_irq, IRQF_SHARED, "cafe-ccic", cam);
-        if (ret)
-                goto out_iounmap;
-        /*
-         * Initialize the controller and leave it powered up.  It will
-         * stay that way until the sensor driver shows up.
-         */
-        cafe_ctlr_init(cam);
-        cafe_ctlr_power_up(cam);
-        /*
-         * Set up I2C/SMBUS communications.  We have to drop the mutex here
-         * because the sensor could attach in this call chain, leading to
-         * unsightly deadlocks.
-         */
-        ret = cafe_smbus_setup(cam);
-        if (ret)
-                goto out_freeirq;
-
-        /* Apply XO-1 clock speed */
-        if (dmi_check_system(olpc_xo1_dmi))
-                sensor_cfg.clock_speed = 45;
-
-        cam->sensor_addr = ov7670_info.addr;
-        cam->sensor = v4l2_i2c_new_subdev_board(&cam->v4l2_dev, &cam->i2c_adapter,
-                        &ov7670_info, NULL);
-        if (cam->sensor == NULL) {
-                ret = -ENODEV;
-                goto out_smbus;
-        }
-
-        ret = cafe_cam_init(cam);
-        if (ret)
-                goto out_smbus;
-
-        /*
-         * Get the v4l2 setup done.
-         */
-        mutex_lock(&cam->s_mutex);
-        cam->vdev = cafe_v4l_template;
-        cam->vdev.debug = 0;
-/*      cam->vdev.debug = V4L2_DEBUG_IOCTL_ARG;*/
-        cam->vdev.v4l2_dev = &cam->v4l2_dev;
-        ret = video_register_device(&cam->vdev, VFL_TYPE_GRABBER, -1);
-        if (ret)
-                goto out_unlock;
-        video_set_drvdata(&cam->vdev, cam);
-
-        /*
-         * If so requested, try to get our DMA buffers now.
-         */
-        if (!alloc_bufs_at_read) {
-                if (cafe_alloc_dma_bufs(cam, 1))
-                        cam_warn(cam, "Unable to alloc DMA buffers at load"
-                                        " will try again later.");
-        }
-
-        mutex_unlock(&cam->s_mutex);
-        return 0;
-
-out_unlock:
-        mutex_unlock(&cam->s_mutex);
-out_smbus:
-        cafe_smbus_shutdown(cam);
-out_freeirq:
-        cafe_ctlr_power_down(cam);
-        free_irq(pdev->irq, cam);
-out_iounmap:
-        pci_iounmap(pdev, cam->regs);
-out_free:
-        v4l2_device_unregister(&cam->v4l2_dev);
-out_unreg:
-        kfree(cam);
-out:
-        return ret;
-}
-
-
-/*
- * Shut down an initialized device
- */
-static void cafe_shutdown(struct cafe_camera *cam)
-{
-/* FIXME: Make sure we take care of everything here */
-        if (cam->n_sbufs > 0)
-                /* What if they are still mapped?  Shouldn't be, but... */
-                cafe_free_sio_buffers(cam);
-        cafe_ctlr_stop_dma(cam);
-        cafe_ctlr_power_down(cam);
-        cafe_smbus_shutdown(cam);
-        cafe_free_dma_bufs(cam);
-        free_irq(cam->pdev->irq, cam);
-        pci_iounmap(cam->pdev, cam->regs);
-        video_unregister_device(&cam->vdev);
-}
-
-
-static void cafe_pci_remove(struct pci_dev *pdev)
-{
-        struct v4l2_device *v4l2_dev = dev_get_drvdata(&pdev->dev);
-        struct cafe_camera *cam = to_cam(v4l2_dev);
-
-        if (cam == NULL) {
-                printk(KERN_WARNING "pci_remove on unknown pdev %p\n", pdev);
-                return;
-        }
-        mutex_lock(&cam->s_mutex);
-        if (cam->users > 0)
-                cam_warn(cam, "Removing a device with users!\n");
-        cafe_shutdown(cam);
-        v4l2_device_unregister(&cam->v4l2_dev);
-        kfree(cam);
-/* No unlock - it no longer exists */
-}
-
-
-#ifdef CONFIG_PM
-/*
- * Basic power management.
- */
-static int cafe_pci_suspend(struct pci_dev *pdev, pm_message_t state)
-{
-        struct v4l2_device *v4l2_dev = dev_get_drvdata(&pdev->dev);
-        struct cafe_camera *cam = to_cam(v4l2_dev);
-        int ret;
-        enum cafe_state cstate;
-
-        ret = pci_save_state(pdev);
-        if (ret)
-                return ret;
-        cstate = cam->state; /* HACK - stop_dma sets to idle */
-        cafe_ctlr_stop_dma(cam);
-        cafe_ctlr_power_down(cam);
-        pci_disable_device(pdev);
-        cam->state = cstate;
-        return 0;
-}
-
-
-static int cafe_pci_resume(struct pci_dev *pdev)
-{
-        struct v4l2_device *v4l2_dev = dev_get_drvdata(&pdev->dev);
-        struct cafe_camera *cam = to_cam(v4l2_dev);
-        int ret = 0;
-
-        pci_restore_state(pdev);
-        ret = pci_enable_device(pdev);
-
-        if (ret) {
-                cam_warn(cam, "Unable to re-enable device on resume!\n");
-                return ret;
-        }
-        cafe_ctlr_init(cam);
-
-        mutex_lock(&cam->s_mutex);
-        if (cam->users > 0) {
-                cafe_ctlr_power_up(cam);
-                __cafe_cam_reset(cam);
-        } else {
-                cafe_ctlr_power_down(cam);
-        }
-        mutex_unlock(&cam->s_mutex);
-
-        set_bit(CF_CONFIG_NEEDED, &cam->flags);
-        if (cam->state == S_SPECREAD)
-                cam->state = S_IDLE;  /* Don't bother restarting */
-        else if (cam->state == S_SINGLEREAD || cam->state == S_STREAMING)
-                ret = cafe_read_setup(cam, cam->state);
-        return ret;
-}
-
-#endif  /* CONFIG_PM */
-
-
-static struct pci_device_id cafe_ids[] = {
-        { PCI_DEVICE(PCI_VENDOR_ID_MARVELL,
-                     PCI_DEVICE_ID_MARVELL_88ALP01_CCIC) },
-        { 0, }
-};
-
-MODULE_DEVICE_TABLE(pci, cafe_ids);
-
-static struct pci_driver cafe_pci_driver = {
-        .name = "cafe1000-ccic",
-        .id_table = cafe_ids,
-        .probe = cafe_pci_probe,
-        .remove = cafe_pci_remove,
-#ifdef CONFIG_PM
-        .suspend = cafe_pci_suspend,
-        .resume = cafe_pci_resume,
-#endif
-};
-
-
-
-
-static int __init cafe_init(void)
-{
-        int ret;
-
-        printk(KERN_NOTICE "Marvell M88ALP01 'CAFE' Camera Controller version %d\n",
-                        CAFE_VERSION);
-        ret = pci_register_driver(&cafe_pci_driver);
-        if (ret) {
-                printk(KERN_ERR "Unable to register cafe_ccic driver\n");
-                goto out;
-        }
-        ret = 0;
-
-  out:
-        return ret;
-}
-
-
-static void __exit cafe_exit(void)
-{
-        pci_unregister_driver(&cafe_pci_driver);
-}
-
-module_init(cafe_init);
-module_exit(cafe_exit);
diff --git a/drivers/media/video/cpia2/cpia2.h b/drivers/media/video/cpia2/cpia2.h
index 6d6d1843791..ab252188981 100644
--- a/drivers/media/video/cpia2/cpia2.h
+++ b/drivers/media/video/cpia2/cpia2.h
@@ -31,7 +31,6 @@
 #ifndef __CPIA2_H__
 #define __CPIA2_H__
 
-#include <linux/version.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-common.h>
 #include <linux/usb.h>
@@ -43,10 +42,6 @@
 /* define for verbose debug output */
 //#define _CPIA2_DEBUG_
 
-#define CPIA2_MAJ_VER   3
-#define CPIA2_MIN_VER   0
-#define CPIA2_PATCH_VER 0
-
 /***
  * Image defines
  ***/
diff --git a/drivers/media/video/cpia2/cpia2_v4l.c b/drivers/media/video/cpia2/cpia2_v4l.c
index 40eb6326e48..077eb1db80a 100644
--- a/drivers/media/video/cpia2/cpia2_v4l.c
+++ b/drivers/media/video/cpia2/cpia2_v4l.c
@@ -29,8 +29,7 @@
  *              Alan Cox <alan@lxorguk.ukuu.org.uk>
  ****************************************************************************/
 
-#include <linux/version.h>
-
+#define CPIA_VERSION "3.0.1"
 
 #include <linux/module.h>
 #include <linux/time.h>
@@ -80,6 +79,7 @@ MODULE_AUTHOR("Steve Miller (STMicroelectronics) <steve.miller@st.com>");
 MODULE_DESCRIPTION("V4L-driver for STMicroelectronics CPiA2 based cameras");
 MODULE_SUPPORTED_DEVICE("video");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(CPIA_VERSION);
 
 #define ABOUT "V4L-Driver for Vision CPiA2 based cameras"
 
@@ -465,9 +465,6 @@ static int cpia2_querycap(struct file *file, void *fh, struct v4l2_capability *v
         if (usb_make_path(cam->dev, vc->bus_info, sizeof(vc->bus_info)) <0)
                 memset(vc->bus_info,0, sizeof(vc->bus_info));
 
-        vc->version = KERNEL_VERSION(CPIA2_MAJ_VER, CPIA2_MIN_VER,
-                                     CPIA2_PATCH_VER);
-
         vc->capabilities = V4L2_CAP_VIDEO_CAPTURE |
                            V4L2_CAP_READWRITE |
                            V4L2_CAP_STREAMING;
@@ -1558,8 +1555,8 @@ static void __init check_parameters(void)
  *****************************************************************************/
 static int __init cpia2_init(void)
 {
-        LOG("%s v%d.%d.%d\n",
-            ABOUT, CPIA2_MAJ_VER, CPIA2_MIN_VER, CPIA2_PATCH_VER);
+        LOG("%s v%s\n",
+            ABOUT, CPIA_VERSION);
         check_parameters();
         cpia2_usb_init();
         return 0;
@@ -1579,4 +1576,3 @@ static void __exit cpia2_exit(void)
 
 module_init(cpia2_init);
 module_exit(cpia2_exit);
-
diff --git a/drivers/media/video/cx18/cx18-alsa-main.c b/drivers/media/video/cx18/cx18-alsa-main.c
index d50d69da387..a1e6c2a3247 100644
--- a/drivers/media/video/cx18/cx18-alsa-main.c
+++ b/drivers/media/video/cx18/cx18-alsa-main.c
@@ -192,6 +192,7 @@ static int snd_cx18_init(struct v4l2_device *v4l2_dev)
 err_exit_free:
         if (sc != NULL)
                 snd_card_free(sc);
+        kfree(cxsc);
 err_exit:
         return ret;
 }
diff --git a/drivers/media/video/cx18/cx18-driver.h b/drivers/media/video/cx18/cx18-driver.h
index 086427288de..18342072306 100644
--- a/drivers/media/video/cx18/cx18-driver.h
+++ b/drivers/media/video/cx18/cx18-driver.h
@@ -25,7 +25,6 @@
 #ifndef CX18_DRIVER_H
 #define CX18_DRIVER_H
 
-#include <linux/version.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/init.h>
diff --git a/drivers/media/video/cx18/cx18-ioctl.c b/drivers/media/video/cx18/cx18-ioctl.c
index e80134f52ef..afe0a29e720 100644
--- a/drivers/media/video/cx18/cx18-ioctl.c
+++ b/drivers/media/video/cx18/cx18-ioctl.c
@@ -469,7 +469,6 @@ static int cx18_querycap(struct file *file, void *fh,
         strlcpy(vcap->card, cx->card_name, sizeof(vcap->card));
         snprintf(vcap->bus_info, sizeof(vcap->bus_info),
                  "PCI:%s", pci_name(cx->pci_dev));
-        vcap->version = CX18_DRIVER_VERSION;        /* version */
         vcap->capabilities = cx->v4l2_cap;          /* capabilities */
         return 0;
 }
diff --git a/drivers/media/video/cx18/cx18-version.h b/drivers/media/video/cx18/cx18-version.h
index cd189b6bbe2..fed48b6bb67 100644
--- a/drivers/media/video/cx18/cx18-version.h
+++ b/drivers/media/video/cx18/cx18-version.h
@@ -23,12 +23,6 @@
 #define CX18_VERSION_H
 
 #define CX18_DRIVER_NAME "cx18"
-#define CX18_DRIVER_VERSION_MAJOR 1
-#define CX18_DRIVER_VERSION_MINOR 5
-#define CX18_DRIVER_VERSION_PATCHLEVEL 0
-
-#define CX18_VERSION __stringify(CX18_DRIVER_VERSION_MAJOR) "." __stringify(CX18_DRIVER_VERSION_MINOR) "." __stringify(CX18_DRIVER_VERSION_PATCHLEVEL)
-#define CX18_DRIVER_VERSION KERNEL_VERSION(CX18_DRIVER_VERSION_MAJOR, \
-        CX18_DRIVER_VERSION_MINOR, CX18_DRIVER_VERSION_PATCHLEVEL)
+#define CX18_VERSION "1.5.1"
 
 #endif
diff --git a/drivers/media/video/cx231xx/cx231xx-avcore.c b/drivers/media/video/cx231xx/cx231xx-avcore.c
index 8d781341576..53ff26e7abf 100644
--- a/drivers/media/video/cx231xx/cx231xx-avcore.c
+++ b/drivers/media/video/cx231xx/cx231xx-avcore.c
@@ -355,6 +355,8 @@ int cx231xx_afe_update_power_control(struct cx231xx *dev,
         case CX231XX_BOARD_HAUPPAUGE_EXETER:
         case CX231XX_BOARD_HAUPPAUGE_USBLIVE2:
         case CX231XX_BOARD_PV_PLAYTV_USB_HYBRID:
+        case CX231XX_BOARD_HAUPPAUGE_USB2_FM_PAL:
+        case CX231XX_BOARD_HAUPPAUGE_USB2_FM_NTSC:
                 if (avmode == POLARIS_AVMODE_ANALOGT_TV) {
                         while (afe_power_status != (FLD_PWRDN_TUNING_BIAS |
                                                 FLD_PWRDN_ENABLE_PLL)) {
@@ -1733,6 +1735,8 @@ int cx231xx_dif_set_standard(struct cx231xx *dev, u32 standard)
                 break;
         case CX231XX_BOARD_CNXT_RDE_253S:
         case CX231XX_BOARD_CNXT_RDU_253S:
+        case CX231XX_BOARD_HAUPPAUGE_USB2_FM_PAL:
+        case CX231XX_BOARD_HAUPPAUGE_USB2_FM_NTSC:
                 func_mode = 0x01;
                 break;
         default:
diff --git a/drivers/media/video/cx231xx/cx231xx-cards.c b/drivers/media/video/cx231xx/cx231xx-cards.c
index 22703815a31..53dae2a8272 100644
--- a/drivers/media/video/cx231xx/cx231xx-cards.c
+++ b/drivers/media/video/cx231xx/cx231xx-cards.c
@@ -387,6 +387,7 @@ struct cx231xx_board cx231xx_boards[] = {
                 .norm = V4L2_STD_NTSC,
                 .no_alt_vanc = 1,
                 .external_av = 1,
+                .dont_use_port_3 = 1,
                 .input = {{
                         .type = CX231XX_VMUX_COMPOSITE1,
                         .vmux = CX231XX_VIN_2_1,
@@ -532,6 +533,76 @@ struct cx231xx_board cx231xx_boards[] = {
                         .gpio = NULL,
                 } },
         },
+        [CX231XX_BOARD_HAUPPAUGE_USB2_FM_PAL] = {
+                .name = "Hauppauge WinTV USB2 FM (PAL)",
+                .tuner_type = TUNER_NXP_TDA18271,
+                .tuner_addr = 0x60,
+                .tuner_gpio = RDE250_XCV_TUNER,
+                .tuner_sif_gpio = 0x05,
+                .tuner_scl_gpio = 0x1a,
+                .tuner_sda_gpio = 0x1b,
+                .decoder = CX231XX_AVDECODER,
+                .output_mode = OUT_MODE_VIP11,
+                .ctl_pin_status_mask = 0xFFFFFFC4,
+                .agc_analog_digital_select_gpio = 0x0c,
+                .gpio_pin_status_mask = 0x4001000,
+                .tuner_i2c_master = 1,
+                .norm = V4L2_STD_PAL,
+
+                .input = {{
+                        .type = CX231XX_VMUX_TELEVISION,
+                        .vmux = CX231XX_VIN_3_1,
+                        .amux = CX231XX_AMUX_VIDEO,
+                        .gpio = NULL,
+                }, {
+                        .type = CX231XX_VMUX_COMPOSITE1,
+                        .vmux = CX231XX_VIN_2_1,
+                        .amux = CX231XX_AMUX_LINE_IN,
+                        .gpio = NULL,
+                }, {
+                        .type = CX231XX_VMUX_SVIDEO,
+                        .vmux = CX231XX_VIN_1_1 |
+                                (CX231XX_VIN_1_2 << 8) |
+                                CX25840_SVIDEO_ON,
+                        .amux = CX231XX_AMUX_LINE_IN,
+                        .gpio = NULL,
+                } },
+        },
+        [CX231XX_BOARD_HAUPPAUGE_USB2_FM_NTSC] = {
+                .name = "Hauppauge WinTV USB2 FM (NTSC)",
+                .tuner_type = TUNER_NXP_TDA18271,
+                .tuner_addr = 0x60,
+                .tuner_gpio = RDE250_XCV_TUNER,
+                .tuner_sif_gpio = 0x05,
+                .tuner_scl_gpio = 0x1a,
+                .tuner_sda_gpio = 0x1b,
+                .decoder = CX231XX_AVDECODER,
+                .output_mode = OUT_MODE_VIP11,
+                .ctl_pin_status_mask = 0xFFFFFFC4,
+                .agc_analog_digital_select_gpio = 0x0c,
+                .gpio_pin_status_mask = 0x4001000,
+                .tuner_i2c_master = 1,
+                .norm = V4L2_STD_NTSC,
+
+                .input = {{
+                        .type = CX231XX_VMUX_TELEVISION,
+                        .vmux = CX231XX_VIN_3_1,
+                        .amux = CX231XX_AMUX_VIDEO,
+                        .gpio = NULL,
+                }, {
+                        .type = CX231XX_VMUX_COMPOSITE1,
+                        .vmux = CX231XX_VIN_2_1,
+                        .amux = CX231XX_AMUX_LINE_IN,
+                        .gpio = NULL,
+                }, {
+                        .type = CX231XX_VMUX_SVIDEO,
+                        .vmux = CX231XX_VIN_1_1 |
+                                (CX231XX_VIN_1_2 << 8) |
+                                CX25840_SVIDEO_ON,
+                        .amux = CX231XX_AMUX_LINE_IN,
+                        .gpio = NULL,
+                } },
+        },
 };
 const unsigned int cx231xx_bcount = ARRAY_SIZE(cx231xx_boards);
 
@@ -553,6 +624,10 @@ struct usb_device_id cx231xx_id_table[] = {
          .driver_info = CX231XX_BOARD_CNXT_RDE_250},
         {USB_DEVICE(0x0572, 0x58A0),
          .driver_info = CX231XX_BOARD_CNXT_RDU_250},
+        {USB_DEVICE(0x2040, 0xb110),
+         .driver_info = CX231XX_BOARD_HAUPPAUGE_USB2_FM_PAL},
+        {USB_DEVICE(0x2040, 0xb111),
+         .driver_info = CX231XX_BOARD_HAUPPAUGE_USB2_FM_NTSC},
         {USB_DEVICE(0x2040, 0xb120),
          .driver_info = CX231XX_BOARD_HAUPPAUGE_EXETER},
         {USB_DEVICE(0x2040, 0xb140),
@@ -1051,6 +1126,9 @@ static int cx231xx_usb_probe(struct usb_interface *interface,
         if (assoc_desc->bFirstInterface != ifnum) {
                 cx231xx_err(DRIVER_NAME ": Not found "
                             "matching IAD interface\n");
+                cx231xx_devused &= ~(1 << nr);
+                kfree(dev);
+                dev = NULL;
                 return -ENODEV;
         }
 
diff --git a/drivers/media/video/cx231xx/cx231xx-core.c b/drivers/media/video/cx231xx/cx231xx-core.c
index abe500feb7d..d4457f9488e 100644
--- a/drivers/media/video/cx231xx/cx231xx-core.c
+++ b/drivers/media/video/cx231xx/cx231xx-core.c
@@ -742,6 +742,8 @@ int cx231xx_set_mode(struct cx231xx *dev, enum cx231xx_mode set_mode)
                 case CX231XX_BOARD_CNXT_RDU_253S:
                 case CX231XX_BOARD_HAUPPAUGE_EXETER:
                 case CX231XX_BOARD_PV_PLAYTV_USB_HYBRID:
+                case CX231XX_BOARD_HAUPPAUGE_USB2_FM_PAL:
+                case CX231XX_BOARD_HAUPPAUGE_USB2_FM_NTSC:
                 errCode = cx231xx_set_agc_analog_digital_mux_select(dev, 0);
                         break;
                 default:
@@ -1381,6 +1383,8 @@ int cx231xx_dev_init(struct cx231xx *dev)
         case CX231XX_BOARD_CNXT_RDU_253S:
         case CX231XX_BOARD_HAUPPAUGE_EXETER:
         case CX231XX_BOARD_PV_PLAYTV_USB_HYBRID:
+        case CX231XX_BOARD_HAUPPAUGE_USB2_FM_PAL:
+        case CX231XX_BOARD_HAUPPAUGE_USB2_FM_NTSC:
         errCode = cx231xx_set_agc_analog_digital_mux_select(dev, 0);
                 break;
         default:
diff --git a/drivers/media/video/cx231xx/cx231xx-video.c b/drivers/media/video/cx231xx/cx231xx-video.c
index a69c24d8db0..6e81f970dc7 100644
--- a/drivers/media/video/cx231xx/cx231xx-video.c
+++ b/drivers/media/video/cx231xx/cx231xx-video.c
@@ -29,7 +29,6 @@
 #include <linux/bitmap.h>
 #include <linux/usb.h>
 #include <linux/i2c.h>
-#include <linux/version.h>
 #include <linux/mm.h>
 #include <linux/mutex.h>
 #include <linux/slab.h>
@@ -45,7 +44,7 @@
 #include "cx231xx.h"
 #include "cx231xx-vbi.h"
 
-#define CX231XX_VERSION_CODE            KERNEL_VERSION(0, 0, 1)
+#define CX231XX_VERSION "0.0.2"
 
 #define DRIVER_AUTHOR   "Srinivasa Deevi <srinivasa.deevi@conexant.com>"
 #define DRIVER_DESC     "Conexant cx231xx based USB video device driver"
@@ -70,6 +69,7 @@ do {\
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
+MODULE_VERSION(CX231XX_VERSION);
 
 static unsigned int card[]     = {[0 ... (CX231XX_MAXBOARDS - 1)] = UNSET };
 static unsigned int video_nr[] = {[0 ... (CX231XX_MAXBOARDS - 1)] = UNSET };
@@ -1179,7 +1179,8 @@ static int vidioc_enum_input(struct file *file, void *priv,
 {
         struct cx231xx_fh *fh = priv;
         struct cx231xx *dev = fh->dev;
-        unsigned int n;
+        u32 gen_stat;
+        unsigned int ret, n;
 
         n = i->index;
         if (n >= MAX_CX231XX_INPUT)
@@ -1198,6 +1199,18 @@ static int vidioc_enum_input(struct file *file, void *priv,
 
         i->std = dev->vdev->tvnorms;
 
+        /* If they are asking about the active input, read signal status */
+        if (n == dev->video_input) {
+                ret = cx231xx_read_i2c_data(dev, VID_BLK_I2C_ADDRESS,
+                                            GEN_STAT, 2, &gen_stat, 4);
+                if (ret > 0) {
+                        if ((gen_stat & FLD_VPRES) == 0x00)
+                                i->status |= V4L2_IN_ST_NO_SIGNAL;
+                        if ((gen_stat & FLD_HLOCK) == 0x00)
+                                i->status |= V4L2_IN_ST_NO_H_LOCK;
+                }
+        }
+
         return 0;
 }
 
@@ -1869,8 +1882,6 @@ static int vidioc_querycap(struct file *file, void *priv,
         strlcpy(cap->card, cx231xx_boards[dev->model].name, sizeof(cap->card));
         usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
 
-        cap->version = CX231XX_VERSION_CODE;
-
         cap->capabilities = V4L2_CAP_VBI_CAPTURE |
 #if 0
                 V4L2_CAP_SLICED_VBI_CAPTURE |
@@ -2057,7 +2068,6 @@ static int radio_querycap(struct file *file, void *priv,
         strlcpy(cap->card, cx231xx_boards[dev->model].name, sizeof(cap->card));
         usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
 
-        cap->version = CX231XX_VERSION_CODE;
         cap->capabilities = V4L2_CAP_TUNER;
         return 0;
 }
@@ -2570,11 +2580,8 @@ int cx231xx_register_analog_devices(struct cx231xx *dev)
 {
         int ret;
 
-        cx231xx_info("%s: v4l2 driver version %d.%d.%d\n",
-                     dev->name,
-                     (CX231XX_VERSION_CODE >> 16) & 0xff,
-                     (CX231XX_VERSION_CODE >> 8) & 0xff,
-                     CX231XX_VERSION_CODE & 0xff);
+        cx231xx_info("%s: v4l2 driver version %s\n",
+                     dev->name, CX231XX_VERSION);
 
         /* set default norm */
         /*dev->norm = cx231xx_video_template.current_norm; */
diff --git a/drivers/media/video/cx231xx/cx231xx.h b/drivers/media/video/cx231xx/cx231xx.h
index 46dd8406781..2000bc64c49 100644
--- a/drivers/media/video/cx231xx/cx231xx.h
+++ b/drivers/media/video/cx231xx/cx231xx.h
@@ -43,7 +43,7 @@
 #include "cx231xx-conf-reg.h"
 
 #define DRIVER_NAME                     "cx231xx"
-#define PWR_SLEEP_INTERVAL              5
+#define PWR_SLEEP_INTERVAL              10
 
 /* I2C addresses for control block in Cx231xx */
 #define     AFE_DEVICE_ADDRESS          0x60
@@ -67,6 +67,8 @@
 #define CX231XX_BOARD_PV_XCAPTURE_USB 11
 #define CX231XX_BOARD_KWORLD_UB430_USB_HYBRID 12
 #define CX231XX_BOARD_ICONBIT_U100 13
+#define CX231XX_BOARD_HAUPPAUGE_USB2_FM_PAL 14
+#define CX231XX_BOARD_HAUPPAUGE_USB2_FM_NTSC 15
 
 /* Limits minimum and default number of buffers */
 #define CX231XX_MIN_BUF                 4
@@ -112,7 +114,6 @@
         V4L2_STD_PAL_BG |  V4L2_STD_PAL_DK    |  V4L2_STD_PAL_I    | \
         V4L2_STD_PAL_M  |  V4L2_STD_PAL_N     |  V4L2_STD_PAL_Nc   | \
         V4L2_STD_PAL_60 |  V4L2_STD_SECAM_L   |  V4L2_STD_SECAM_DK)
-#define CX231xx_VERSION_CODE KERNEL_VERSION(0, 0, 2)
 
 #define SLEEP_S5H1432    30
 #define CX23417_OSC_EN   8
diff --git a/drivers/media/video/cx23885/altera-ci.c b/drivers/media/video/cx23885/altera-ci.c
index 678539b2acf..1fa8927f0d3 100644
--- a/drivers/media/video/cx23885/altera-ci.c
+++ b/drivers/media/video/cx23885/altera-ci.c
@@ -52,7 +52,6 @@
  * |  DATA7|  DATA6|  DATA5|  DATA4|  DATA3|  DATA2|  DATA1|  DATA0|
  * +-------+-------+-------+-------+-------+-------+-------+-------+
  */
-#include <linux/version.h>
 #include <media/videobuf-dma-sg.h>
 #include <media/videobuf-dvb.h>
 #include "altera-ci.h"
diff --git a/drivers/media/video/cx23885/cx23885-417.c b/drivers/media/video/cx23885/cx23885-417.c
index 9a98dc55f65..67c4a59bd88 100644
--- a/drivers/media/video/cx23885/cx23885-417.c
+++ b/drivers/media/video/cx23885/cx23885-417.c
@@ -1359,7 +1359,6 @@ static int vidioc_querycap(struct file *file, void  *priv,
         strlcpy(cap->card, cx23885_boards[tsport->dev->board].name,
                 sizeof(cap->card));
         sprintf(cap->bus_info, "PCI:%s", pci_name(dev->pci));
-        cap->version = CX23885_VERSION_CODE;
         cap->capabilities =
                 V4L2_CAP_VIDEO_CAPTURE |
                 V4L2_CAP_READWRITE     |
diff --git a/drivers/media/video/cx23885/cx23885-cards.c b/drivers/media/video/cx23885/cx23885-cards.c
index 934185cca75..76b7563de39 100644
--- a/drivers/media/video/cx23885/cx23885-cards.c
+++ b/drivers/media/video/cx23885/cx23885-cards.c
@@ -29,11 +29,17 @@
 #include "../../../staging/altera-stapl/altera.h"
 #include "cx23885.h"
 #include "tuner-xc2028.h"
+#include "netup-eeprom.h"
 #include "netup-init.h"
 #include "altera-ci.h"
+#include "xc4000.h"
 #include "xc5000.h"
 #include "cx23888-ir.h"
 
+static unsigned int netup_card_rev = 1;
+module_param(netup_card_rev, int, 0644);
+MODULE_PARM_DESC(netup_card_rev,
+                "NetUP Dual DVB-T/C CI card revision");
 static unsigned int enable_885_ir;
 module_param(enable_885_ir, int, 0644);
 MODULE_PARM_DESC(enable_885_ir,
@@ -175,6 +181,34 @@ struct cx23885_board cx23885_boards[] = {
                 .name           = "Leadtek Winfast PxDVR3200 H",
                 .portc          = CX23885_MPEG_DVB,
         },
+        [CX23885_BOARD_LEADTEK_WINFAST_PXDVR3200_H_XC4000] = {
+                .name           = "Leadtek Winfast PxDVR3200 H XC4000",
+                .porta          = CX23885_ANALOG_VIDEO,
+                .portc          = CX23885_MPEG_DVB,
+                .tuner_type     = TUNER_XC4000,
+                .tuner_addr     = 0x61,
+                .radio_type     = TUNER_XC4000,
+                .radio_addr     = 0x61,
+                .input          = {{
+                        .type   = CX23885_VMUX_TELEVISION,
+                        .vmux   = CX25840_VIN2_CH1 |
+                                  CX25840_VIN5_CH2 |
+                                  CX25840_NONE0_CH3,
+                }, {
+                        .type   = CX23885_VMUX_COMPOSITE1,
+                        .vmux   = CX25840_COMPOSITE1,
+                }, {
+                        .type   = CX23885_VMUX_SVIDEO,
+                        .vmux   = CX25840_SVIDEO_LUMA3 |
+                                  CX25840_SVIDEO_CHROMA4,
+                }, {
+                        .type   = CX23885_VMUX_COMPONENT,
+                        .vmux   = CX25840_VIN7_CH1 |
+                                  CX25840_VIN6_CH2 |
+                                  CX25840_VIN8_CH3 |
+                                  CX25840_COMPONENT_ON,
+                } },
+        },
         [CX23885_BOARD_COMPRO_VIDEOMATE_E650F] = {
                 .name           = "Compro VideoMate E650F",
                 .portc          = CX23885_MPEG_DVB,
@@ -433,6 +467,10 @@ struct cx23885_subid cx23885_subids[] = {
                 .subdevice = 0x6681,
                 .card      = CX23885_BOARD_LEADTEK_WINFAST_PXDVR3200_H,
         }, {
+                .subvendor = 0x107d,
+                .subdevice = 0x6f39,
+                .card      = CX23885_BOARD_LEADTEK_WINFAST_PXDVR3200_H_XC4000,
+        }, {
                 .subvendor = 0x185b,
                 .subdevice = 0xe800,
                 .card      = CX23885_BOARD_COMPRO_VIDEOMATE_E650F,
@@ -749,6 +787,7 @@ int cx23885_tuner_callback(void *priv, int component, int command, int arg)
         case CX23885_BOARD_HAUPPAUGE_HVR1500:
         case CX23885_BOARD_HAUPPAUGE_HVR1500Q:
         case CX23885_BOARD_LEADTEK_WINFAST_PXDVR3200_H:
+        case CX23885_BOARD_LEADTEK_WINFAST_PXDVR3200_H_XC4000:
         case CX23885_BOARD_COMPRO_VIDEOMATE_E650F:
         case CX23885_BOARD_COMPRO_VIDEOMATE_E800:
         case CX23885_BOARD_LEADTEK_WINFAST_PXTV1200:
@@ -909,6 +948,7 @@ void cx23885_gpio_setup(struct cx23885_dev *dev)
                 cx_set(GP0_IO, 0x000f000f);
                 break;
         case CX23885_BOARD_LEADTEK_WINFAST_PXDVR3200_H:
+        case CX23885_BOARD_LEADTEK_WINFAST_PXDVR3200_H_XC4000:
         case CX23885_BOARD_COMPRO_VIDEOMATE_E650F:
         case CX23885_BOARD_COMPRO_VIDEOMATE_E800:
         case CX23885_BOARD_LEADTEK_WINFAST_PXTV1200:
@@ -1097,12 +1137,19 @@ int cx23885_ir_init(struct cx23885_dev *dev)
         case CX23885_BOARD_HAUPPAUGE_HVR1800:
         case CX23885_BOARD_HAUPPAUGE_HVR1200:
         case CX23885_BOARD_HAUPPAUGE_HVR1400:
-        case CX23885_BOARD_HAUPPAUGE_HVR1270:
         case CX23885_BOARD_HAUPPAUGE_HVR1275:
         case CX23885_BOARD_HAUPPAUGE_HVR1255:
         case CX23885_BOARD_HAUPPAUGE_HVR1210:
                 /* FIXME: Implement me */
                 break;
+        case CX23885_BOARD_HAUPPAUGE_HVR1270:
+                ret = cx23888_ir_probe(dev);
+                if (ret)
+                        break;
+                dev->sd_ir = cx23885_find_hw(dev, CX23885_HW_888_IR);
+                v4l2_subdev_call(dev->sd_cx25840, core, s_io_pin_config,
+                                 ir_rx_pin_cfg_count, ir_rx_pin_cfg);
+                break;
         case CX23885_BOARD_HAUPPAUGE_HVR1850:
         case CX23885_BOARD_HAUPPAUGE_HVR1290:
                 ret = cx23888_ir_probe(dev);
@@ -1156,6 +1203,7 @@ int cx23885_ir_init(struct cx23885_dev *dev)
 void cx23885_ir_fini(struct cx23885_dev *dev)
 {
         switch (dev->board) {
+        case CX23885_BOARD_HAUPPAUGE_HVR1270:
         case CX23885_BOARD_HAUPPAUGE_HVR1850:
         case CX23885_BOARD_HAUPPAUGE_HVR1290:
                 cx23885_irq_remove(dev, PCI_MSK_IR);
@@ -1199,6 +1247,7 @@ int netup_jtag_io(void *device, int tms, int tdi, int read_tdo)
 void cx23885_ir_pci_int_enable(struct cx23885_dev *dev)
 {
         switch (dev->board) {
+        case CX23885_BOARD_HAUPPAUGE_HVR1270:
         case CX23885_BOARD_HAUPPAUGE_HVR1850:
         case CX23885_BOARD_HAUPPAUGE_HVR1290:
                 if (dev->sd_ir)
@@ -1325,6 +1374,7 @@ void cx23885_card_setup(struct cx23885_dev *dev)
         case CX23885_BOARD_HAUPPAUGE_HVR1700:
         case CX23885_BOARD_HAUPPAUGE_HVR1400:
         case CX23885_BOARD_LEADTEK_WINFAST_PXDVR3200_H:
+        case CX23885_BOARD_LEADTEK_WINFAST_PXDVR3200_H_XC4000:
         case CX23885_BOARD_COMPRO_VIDEOMATE_E650F:
         case CX23885_BOARD_HAUPPAUGE_HVR1270:
         case CX23885_BOARD_HAUPPAUGE_HVR1275:
@@ -1353,10 +1403,12 @@ void cx23885_card_setup(struct cx23885_dev *dev)
         case CX23885_BOARD_HAUPPAUGE_HVR1800lp:
         case CX23885_BOARD_HAUPPAUGE_HVR1700:
         case CX23885_BOARD_LEADTEK_WINFAST_PXDVR3200_H:
+        case CX23885_BOARD_LEADTEK_WINFAST_PXDVR3200_H_XC4000:
         case CX23885_BOARD_COMPRO_VIDEOMATE_E650F:
         case CX23885_BOARD_NETUP_DUAL_DVBS2_CI:
         case CX23885_BOARD_NETUP_DUAL_DVB_T_C_CI_RF:
         case CX23885_BOARD_COMPRO_VIDEOMATE_E800:
+        case CX23885_BOARD_HAUPPAUGE_HVR1270:
         case CX23885_BOARD_HAUPPAUGE_HVR1850:
         case CX23885_BOARD_MYGICA_X8506:
         case CX23885_BOARD_MAGICPRO_PROHDTVE2:
@@ -1383,6 +1435,7 @@ void cx23885_card_setup(struct cx23885_dev *dev)
                 const struct firmware *fw;
                 const char *filename = "dvb-netup-altera-01.fw";
                 char *action = "configure";
+                static struct netup_card_info cinfo;
                 struct altera_config netup_config = {
                         .dev = dev,
                         .action = action,
@@ -1391,6 +1444,21 @@ void cx23885_card_setup(struct cx23885_dev *dev)
 
                 netup_initialize(dev);
 
+                netup_get_card_info(&dev->i2c_bus[0].i2c_adap, &cinfo);
+                if (netup_card_rev)
+                        cinfo.rev = netup_card_rev;
+
+                switch (cinfo.rev) {
+                case 0x4:
+                        filename = "dvb-netup-altera-04.fw";
+                        break;
+                default:
+                        filename = "dvb-netup-altera-01.fw";
+                        break;
+                }
+                printk(KERN_INFO "NetUP card rev=0x%x fw_filename=%s\n",
+                                cinfo.rev, filename);
+
                 ret = request_firmware(&fw, filename, &dev->pci->dev);
                 if (ret != 0)
                         printk(KERN_ERR "did not find the firmware file. (%s) "
diff --git a/drivers/media/video/cx23885/cx23885-core.c b/drivers/media/video/cx23885/cx23885-core.c
index 419777a832e..ee41a8882f5 100644
--- a/drivers/media/video/cx23885/cx23885-core.c
+++ b/drivers/media/video/cx23885/cx23885-core.c
@@ -42,6 +42,7 @@
 MODULE_DESCRIPTION("Driver for cx23885 based TV cards");
 MODULE_AUTHOR("Steven Toth <stoth@linuxtv.org>");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(CX23885_VERSION);
 
 static unsigned int debug;
 module_param(debug, int, 0644);
@@ -2147,14 +2148,8 @@ static struct pci_driver cx23885_pci_driver = {
 
 static int __init cx23885_init(void)
 {
-        printk(KERN_INFO "cx23885 driver version %d.%d.%d loaded\n",
-               (CX23885_VERSION_CODE >> 16) & 0xff,
-               (CX23885_VERSION_CODE >>  8) & 0xff,
-               CX23885_VERSION_CODE & 0xff);
-#ifdef SNAPSHOT
-        printk(KERN_INFO "cx23885: snapshot date %04d-%02d-%02d\n",
-               SNAPSHOT/10000, (SNAPSHOT/100)%100, SNAPSHOT%100);
-#endif
+        printk(KERN_INFO "cx23885 driver version %s loaded\n",
+                CX23885_VERSION);
         return pci_register_driver(&cx23885_pci_driver);
 }
 
@@ -2165,5 +2160,3 @@ static void __exit cx23885_fini(void)
 
 module_init(cx23885_init);
 module_exit(cx23885_fini);
-
-/* ----------------------------------------------------------- */
diff --git a/drivers/media/video/cx23885/cx23885-dvb.c b/drivers/media/video/cx23885/cx23885-dvb.c
index 3c315f94cc8..aa83f07b1b0 100644
--- a/drivers/media/video/cx23885/cx23885-dvb.c
+++ b/drivers/media/video/cx23885/cx23885-dvb.c
@@ -37,6 +37,7 @@
 #include "tda8290.h"
 #include "tda18271.h"
 #include "lgdt330x.h"
+#include "xc4000.h"
 #include "xc5000.h"
 #include "max2165.h"
 #include "tda10048.h"
@@ -921,6 +922,26 @@ static int dvb_register(struct cx23885_tsport *port)
                                 fe->ops.tuner_ops.set_config(fe, &ctl);
                 }
                 break;
+        case CX23885_BOARD_LEADTEK_WINFAST_PXDVR3200_H_XC4000:
+                i2c_bus = &dev->i2c_bus[0];
+
+                fe0->dvb.frontend = dvb_attach(zl10353_attach,
+                                               &dvico_fusionhdtv_xc3028,
+                                               &i2c_bus->i2c_adap);
+                if (fe0->dvb.frontend != NULL) {
+                        struct dvb_frontend     *fe;
+                        struct xc4000_config    cfg = {
+                                .i2c_address      = 0x61,
+                                .default_pm       = 0,
+                                .dvb_amplitude    = 134,
+                                .set_smoothedcvbs = 1,
+                                .if_khz           = 4560
+                        };
+
+                        fe = dvb_attach(xc4000_attach, fe0->dvb.frontend,
+                                        &dev->i2c_bus[1].i2c_adap, &cfg);
+                }
+                break;
         case CX23885_BOARD_TBS_6920:
                 i2c_bus = &dev->i2c_bus[1];
 
@@ -1249,7 +1270,7 @@ int cx23885_dvb_unregister(struct cx23885_tsport *port)
          * implement MFE support.
          */
         fe0 = videobuf_dvb_get_frontend(&port->frontends, 1);
-        if (fe0->dvb.frontend)
+        if (fe0 && fe0->dvb.frontend)
                 videobuf_dvb_unregister_bus(&port->frontends);
 
         switch (port->dev->board) {
diff --git a/drivers/media/video/cx23885/cx23885-input.c b/drivers/media/video/cx23885/cx23885-input.c
index e97cafd8398..ce765e3f77b 100644
--- a/drivers/media/video/cx23885/cx23885-input.c
+++ b/drivers/media/video/cx23885/cx23885-input.c
@@ -82,6 +82,7 @@ void cx23885_input_rx_work_handler(struct cx23885_dev *dev, u32 events)
                 return;
 
         switch (dev->board) {
+        case CX23885_BOARD_HAUPPAUGE_HVR1270:
         case CX23885_BOARD_HAUPPAUGE_HVR1850:
         case CX23885_BOARD_HAUPPAUGE_HVR1290:
         case CX23885_BOARD_TEVII_S470:
@@ -133,6 +134,7 @@ static int cx23885_input_ir_start(struct cx23885_dev *dev)
 
         v4l2_subdev_call(dev->sd_ir, ir, rx_g_parameters, &params);
         switch (dev->board) {
+        case CX23885_BOARD_HAUPPAUGE_HVR1270:
         case CX23885_BOARD_HAUPPAUGE_HVR1850:
         case CX23885_BOARD_HAUPPAUGE_HVR1290:
         case CX23885_BOARD_HAUPPAUGE_HVR1250:
@@ -229,6 +231,9 @@ static void cx23885_input_ir_stop(struct cx23885_dev *dev)
                 v4l2_subdev_call(dev->sd_ir, ir, rx_s_parameters, &params);
                 v4l2_subdev_call(dev->sd_ir, ir, rx_g_parameters, &params);
         }
+        flush_work_sync(&dev->cx25840_work);
+        flush_work_sync(&dev->ir_rx_work);
+        flush_work_sync(&dev->ir_tx_work);
 }
 
 static void cx23885_input_ir_close(struct rc_dev *rc)
@@ -257,6 +262,7 @@ int cx23885_input_init(struct cx23885_dev *dev)
                 return -ENODEV;
 
         switch (dev->board) {
+        case CX23885_BOARD_HAUPPAUGE_HVR1270:
         case CX23885_BOARD_HAUPPAUGE_HVR1850:
         case CX23885_BOARD_HAUPPAUGE_HVR1290:
         case CX23885_BOARD_HAUPPAUGE_HVR1250:
diff --git a/drivers/media/video/cx23885/cx23885-video.c b/drivers/media/video/cx23885/cx23885-video.c
index ee57f6bedbe..896bb32dbf0 100644
--- a/drivers/media/video/cx23885/cx23885-video.c
+++ b/drivers/media/video/cx23885/cx23885-video.c
@@ -1000,7 +1000,6 @@ static int vidioc_querycap(struct file *file, void  *priv,
         strlcpy(cap->card, cx23885_boards[dev->board].name,
                 sizeof(cap->card));
         sprintf(cap->bus_info, "PCIe:%s", pci_name(dev->pci));
-        cap->version = CX23885_VERSION_CODE;
         cap->capabilities =
                 V4L2_CAP_VIDEO_CAPTURE |
                 V4L2_CAP_READWRITE     |
diff --git a/drivers/media/video/cx23885/cx23885.h b/drivers/media/video/cx23885/cx23885.h
index c186473fc57..d86bc0b1317 100644
--- a/drivers/media/video/cx23885/cx23885.h
+++ b/drivers/media/video/cx23885/cx23885.h
@@ -36,10 +36,9 @@
 #include "cx23885-reg.h"
 #include "media/cx2341x.h"
 
-#include <linux/version.h>
 #include <linux/mutex.h>
 
-#define CX23885_VERSION_CODE KERNEL_VERSION(0, 0, 2)
+#define CX23885_VERSION "0.0.3"
 
 #define UNSET (-1U)
 
@@ -86,6 +85,7 @@
 #define CX23885_BOARD_LEADTEK_WINFAST_PXTV1200 28
 #define CX23885_BOARD_GOTVIEW_X5_3D_HYBRID     29
 #define CX23885_BOARD_NETUP_DUAL_DVB_T_C_CI_RF 30
+#define CX23885_BOARD_LEADTEK_WINFAST_PXDVR3200_H_XC4000 31
 
 #define GPIO_0 0x00000001
 #define GPIO_1 0x00000002
diff --git a/drivers/media/video/cx88/cx88-alsa.c b/drivers/media/video/cx88/cx88-alsa.c
index 423c1af8a78..68d1240f493 100644
--- a/drivers/media/video/cx88/cx88-alsa.c
+++ b/drivers/media/video/cx88/cx88-alsa.c
@@ -113,6 +113,8 @@ MODULE_DESCRIPTION("ALSA driver module for cx2388x based TV cards");
 MODULE_AUTHOR("Ricardo Cerqueira");
 MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@infradead.org>");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(CX88_VERSION);
+
 MODULE_SUPPORTED_DEVICE("{{Conexant,23881},"
                         "{{Conexant,23882},"
                         "{{Conexant,23883}");
@@ -973,14 +975,8 @@ static struct pci_driver cx88_audio_pci_driver = {
  */
 static int __init cx88_audio_init(void)
 {
-        printk(KERN_INFO "cx2388x alsa driver version %d.%d.%d loaded\n",
-               (CX88_VERSION_CODE >> 16) & 0xff,
-               (CX88_VERSION_CODE >>  8) & 0xff,
-               CX88_VERSION_CODE & 0xff);
-#ifdef SNAPSHOT
-        printk(KERN_INFO "cx2388x: snapshot date %04d-%02d-%02d\n",
-               SNAPSHOT/10000, (SNAPSHOT/100)%100, SNAPSHOT%100);
-#endif
+        printk(KERN_INFO "cx2388x alsa driver version %s loaded\n",
+               CX88_VERSION);
         return pci_register_driver(&cx88_audio_pci_driver);
 }
 
@@ -994,10 +990,3 @@ static void __exit cx88_audio_fini(void)
 
 module_init(cx88_audio_init);
 module_exit(cx88_audio_fini);
-
-/* ----------------------------------------------------------- */
-/*
- * Local variables:
- * c-basic-offset: 8
- * End:
- */
diff --git a/drivers/media/video/cx88/cx88-blackbird.c b/drivers/media/video/cx88/cx88-blackbird.c
index 11e49bbc4a6..e46446a449c 100644
--- a/drivers/media/video/cx88/cx88-blackbird.c
+++ b/drivers/media/video/cx88/cx88-blackbird.c
@@ -42,6 +42,7 @@
 MODULE_DESCRIPTION("driver for cx2388x/cx23416 based mpeg encoder cards");
 MODULE_AUTHOR("Jelle Foks <jelle@foks.us>, Gerd Knorr <kraxel@bytesex.org> [SuSE Labs]");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(CX88_VERSION);
 
 static unsigned int mpegbufs = 32;
 module_param(mpegbufs,int,0644);
@@ -730,7 +731,6 @@ static int vidioc_querycap (struct file *file, void  *priv,
         strcpy(cap->driver, "cx88_blackbird");
         strlcpy(cap->card, core->board.name, sizeof(cap->card));
         sprintf(cap->bus_info,"PCI:%s",pci_name(dev->pci));
-        cap->version = CX88_VERSION_CODE;
         cap->capabilities =
                 V4L2_CAP_VIDEO_CAPTURE |
                 V4L2_CAP_READWRITE     |
@@ -1368,14 +1368,8 @@ static struct cx8802_driver cx8802_blackbird_driver = {
 
 static int __init blackbird_init(void)
 {
-        printk(KERN_INFO "cx2388x blackbird driver version %d.%d.%d loaded\n",
-               (CX88_VERSION_CODE >> 16) & 0xff,
-               (CX88_VERSION_CODE >>  8) & 0xff,
-               CX88_VERSION_CODE & 0xff);
-#ifdef SNAPSHOT
-        printk(KERN_INFO "cx2388x: snapshot date %04d-%02d-%02d\n",
-               SNAPSHOT/10000, (SNAPSHOT/100)%100, SNAPSHOT%100);
-#endif
+        printk(KERN_INFO "cx2388x blackbird driver version %s loaded\n",
+               CX88_VERSION);
         return cx8802_register_driver(&cx8802_blackbird_driver);
 }
 
@@ -1389,11 +1383,3 @@ module_exit(blackbird_fini);
 
 module_param_named(video_debug,cx8802_mpeg_template.debug, int, 0644);
 MODULE_PARM_DESC(debug,"enable debug messages [video]");
-
-/* ----------------------------------------------------------- */
-/*
- * Local variables:
- * c-basic-offset: 8
- * End:
- * kate: eol "unix"; indent-width 3; remove-trailing-space on; replace-trailing-space-save on; tab-width 8; replace-tabs off; space-indent off; mixed-indent off
- */
diff --git a/drivers/media/video/cx88/cx88-cards.c b/drivers/media/video/cx88/cx88-cards.c
index 27222c92b60..0d719faafd8 100644
--- a/drivers/media/video/cx88/cx88-cards.c
+++ b/drivers/media/video/cx88/cx88-cards.c
@@ -28,6 +28,7 @@
 
 #include "cx88.h"
 #include "tea5767.h"
+#include "xc4000.h"
 
 static unsigned int tuner[] = {[0 ... (CX88_MAXBOARDS - 1)] = UNSET };
 static unsigned int radio[] = {[0 ... (CX88_MAXBOARDS - 1)] = UNSET };
@@ -2119,6 +2120,99 @@ static const struct cx88_board cx88_boards[] = {
                 },
                 .mpeg           = CX88_MPEG_DVB,
         },
+        [CX88_BOARD_WINFAST_DTV1800H_XC4000] = {
+                .name           = "Leadtek WinFast DTV1800 H (XC4000)",
+                .tuner_type     = TUNER_XC4000,
+                .radio_type     = TUNER_XC4000,
+                .tuner_addr     = 0x61,
+                .radio_addr     = 0x61,
+                /*
+                 * GPIO setting
+                 *
+                 *  2: mute (0=off,1=on)
+                 * 12: tuner reset pin
+                 * 13: audio source (0=tuner audio,1=line in)
+                 * 14: FM (0=on,1=off ???)
+                 */
+                .input          = {{
+                        .type   = CX88_VMUX_TELEVISION,
+                        .vmux   = 0,
+                        .gpio0  = 0x0400,       /* pin 2 = 0 */
+                        .gpio1  = 0x6040,       /* pin 13 = 0, pin 14 = 1 */
+                        .gpio2  = 0x0000,
+                }, {
+                        .type   = CX88_VMUX_COMPOSITE1,
+                        .vmux   = 1,
+                        .gpio0  = 0x0400,       /* pin 2 = 0 */
+                        .gpio1  = 0x6060,       /* pin 13 = 1, pin 14 = 1 */
+                        .gpio2  = 0x0000,
+                }, {
+                        .type   = CX88_VMUX_SVIDEO,
+                        .vmux   = 2,
+                        .gpio0  = 0x0400,       /* pin 2 = 0 */
+                        .gpio1  = 0x6060,       /* pin 13 = 1, pin 14 = 1 */
+                        .gpio2  = 0x0000,
+                }},
+                .radio = {
+                        .type   = CX88_RADIO,
+                        .gpio0  = 0x0400,       /* pin 2 = 0 */
+                        .gpio1  = 0x6000,       /* pin 13 = 0, pin 14 = 0 */
+                        .gpio2  = 0x0000,
+                },
+                .mpeg           = CX88_MPEG_DVB,
+        },
+        [CX88_BOARD_WINFAST_DTV2000H_PLUS] = {
+                .name           = "Leadtek WinFast DTV2000 H PLUS",
+                .tuner_type     = TUNER_XC4000,
+                .radio_type     = TUNER_XC4000,
+                .tuner_addr     = 0x61,
+                .radio_addr     = 0x61,
+                /*
+                 * GPIO
+                 *   2: 1: mute audio
+                 *  12: 0: reset XC4000
+                 *  13: 1: audio input is line in (0: tuner)
+                 *  14: 0: FM radio
+                 *  16: 0: RF input is cable
+                 */
+                .input          = {{
+                        .type   = CX88_VMUX_TELEVISION,
+                        .vmux   = 0,
+                        .gpio0  = 0x0403,
+                        .gpio1  = 0xF0D7,
+                        .gpio2  = 0x0101,
+                        .gpio3  = 0x0000,
+                }, {
+                        .type   = CX88_VMUX_CABLE,
+                        .vmux   = 0,
+                        .gpio0  = 0x0403,
+                        .gpio1  = 0xF0D7,
+                        .gpio2  = 0x0100,
+                        .gpio3  = 0x0000,
+                }, {
+                        .type   = CX88_VMUX_COMPOSITE1,
+                        .vmux   = 1,
+                        .gpio0  = 0x0403,       /* was 0x0407 */
+                        .gpio1  = 0xF0F7,
+                        .gpio2  = 0x0101,
+                        .gpio3  = 0x0000,
+                }, {
+                        .type   = CX88_VMUX_SVIDEO,
+                        .vmux   = 2,
+                        .gpio0  = 0x0403,       /* was 0x0407 */
+                        .gpio1  = 0xF0F7,
+                        .gpio2  = 0x0101,
+                        .gpio3  = 0x0000,
+                }},
+                .radio = {
+                        .type   = CX88_RADIO,
+                        .gpio0  = 0x0403,
+                        .gpio1  = 0xF097,
+                        .gpio2  = 0x0100,
+                        .gpio3  = 0x0000,
+                },
+                .mpeg           = CX88_MPEG_DVB,
+        },
         [CX88_BOARD_PROF_7301] = {
                 .name           = "Prof 7301 DVB-S/S2",
                 .tuner_type     = UNSET,
@@ -2581,6 +2675,15 @@ static const struct cx88_subid cx88_subids[] = {
                 .subdevice = 0x6654,
                 .card      = CX88_BOARD_WINFAST_DTV1800H,
         }, {
+                /* WinFast DTV1800 H with XC4000 tuner */
+                .subvendor = 0x107d,
+                .subdevice = 0x6f38,
+                .card      = CX88_BOARD_WINFAST_DTV1800H_XC4000,
+        }, {
+                .subvendor = 0x107d,
+                .subdevice = 0x6f42,
+                .card      = CX88_BOARD_WINFAST_DTV2000H_PLUS,
+        }, {
                 /* PVR2000 PAL Model [107d:6630] */
                 .subvendor = 0x107d,
                 .subdevice = 0x6630,
@@ -2846,6 +2949,23 @@ static int cx88_xc3028_winfast1800h_callback(struct cx88_core *core,
         return -EINVAL;
 }
 
+static int cx88_xc4000_winfast2000h_plus_callback(struct cx88_core *core,
+                                                  int command, int arg)
+{
+        switch (command) {
+        case XC4000_TUNER_RESET:
+                /* GPIO 12 (xc4000 tuner reset) */
+                cx_set(MO_GP1_IO, 0x1010);
+                mdelay(50);
+                cx_clear(MO_GP1_IO, 0x10);
+                mdelay(75);
+                cx_set(MO_GP1_IO, 0x10);
+                mdelay(75);
+                return 0;
+        }
+        return -EINVAL;
+}
+
 /* ------------------------------------------------------------------- */
 /* some Divco specific stuff                                           */
 static int cx88_pv_8000gt_callback(struct cx88_core *core,
@@ -2948,6 +3068,19 @@ static int cx88_xc2028_tuner_callback(struct cx88_core *core,
         return -EINVAL;
 }
 
+static int cx88_xc4000_tuner_callback(struct cx88_core *core,
+                                      int command, int arg)
+{
+        /* Board-specific callbacks */
+        switch (core->boardnr) {
+        case CX88_BOARD_WINFAST_DTV1800H_XC4000:
+        case CX88_BOARD_WINFAST_DTV2000H_PLUS:
+                return cx88_xc4000_winfast2000h_plus_callback(core,
+                                                              command, arg);
+        }
+        return -EINVAL;
+}
+
 /* ----------------------------------------------------------------------- */
 /* Tuner callback function. Currently only needed for the Pinnacle         *
  * PCTV HD 800i with an xc5000 sillicon tuner. This is used for both       *
@@ -3022,6 +3155,9 @@ int cx88_tuner_callback(void *priv, int component, int command, int arg)
                 case TUNER_XC2028:
                         info_printk(core, "Calling XC2028/3028 callback\n");
                         return cx88_xc2028_tuner_callback(core, command, arg);
+                case TUNER_XC4000:
+                        info_printk(core, "Calling XC4000 callback\n");
+                        return cx88_xc4000_tuner_callback(core, command, arg);
                 case TUNER_XC5000:
                         info_printk(core, "Calling XC5000 callback\n");
                         return cx88_xc5000_tuner_callback(core, command, arg);
@@ -3109,13 +3245,13 @@ static void cx88_card_setup_pre_i2c(struct cx88_core *core)
 
         case CX88_BOARD_WINFAST_TV2000_XP_GLOBAL:
         case CX88_BOARD_WINFAST_DTV1800H:
-                /* GPIO 12 (xc3028 tuner reset) */
-                cx_set(MO_GP1_IO, 0x1010);
-                mdelay(50);
-                cx_clear(MO_GP1_IO, 0x10);
-                mdelay(50);
-                cx_set(MO_GP1_IO, 0x10);
-                mdelay(50);
+                cx88_xc3028_winfast1800h_callback(core, XC2028_TUNER_RESET, 0);
+                break;
+
+        case CX88_BOARD_WINFAST_DTV1800H_XC4000:
+        case CX88_BOARD_WINFAST_DTV2000H_PLUS:
+                cx88_xc4000_winfast2000h_plus_callback(core,
+                                                       XC4000_TUNER_RESET, 0);
                 break;
 
         case CX88_BOARD_TWINHAN_VP1027_DVBS:
diff --git a/drivers/media/video/cx88/cx88-core.c b/drivers/media/video/cx88/cx88-core.c
index 2e145f0a5fd..fbcaa1c5b09 100644
--- a/drivers/media/video/cx88/cx88-core.c
+++ b/drivers/media/video/cx88/cx88-core.c
@@ -636,6 +636,9 @@ int cx88_reset(struct cx88_core *core)
         cx_write(MO_PCI_INTSTAT,   0xFFFFFFFF); // Clear PCI int
         cx_write(MO_INT1_STAT,     0xFFFFFFFF); // Clear RISC int
 
+        /* set default notch filter */
+        cx_andor(MO_HTOTAL, 0x1800, (HLNotchFilter4xFsc << 11));
+
         /* Reset on-board parts */
         cx_write(MO_SRST_IO, 0);
         msleep(10);
@@ -759,8 +762,8 @@ int cx88_set_scale(struct cx88_core *core, unsigned int width, unsigned int heig
         if (nocomb)
                 value |= (3 << 5); // disable comb filter
 
-        cx_write(MO_FILTER_EVEN,  value);
-        cx_write(MO_FILTER_ODD,   value);
+        cx_andor(MO_FILTER_EVEN,  0x7ffc7f, value); /* preserve PEAKEN, PSEL */
+        cx_andor(MO_FILTER_ODD,   0x7ffc7f, value);
         dprintk(1,"set_scale: filter  0x%04x\n", value);
 
         return 0;
@@ -994,10 +997,10 @@ int cx88_set_tvnorm(struct cx88_core *core, v4l2_std_id norm)
         // htotal
         tmp64 = norm_htotal(norm) * (u64)vdec_clock;
         do_div(tmp64, fsc8);
-        htotal = (u32)tmp64 | (HLNotchFilter4xFsc << 11);
+        htotal = (u32)tmp64;
         dprintk(1,"set_tvnorm: MO_HTOTAL        0x%08x [old=0x%08x,htotal=%d]\n",
                 htotal, cx_read(MO_HTOTAL), (u32)tmp64);
-        cx_write(MO_HTOTAL, htotal);
+        cx_andor(MO_HTOTAL, 0x07ff, htotal);
 
         // vbi stuff, set vbi offset to 10 (for 20 Clk*2 pixels), this makes
         // the effective vbi offset ~244 samples, the same as the Bt8x8
diff --git a/drivers/media/video/cx88/cx88-dvb.c b/drivers/media/video/cx88/cx88-dvb.c
index c69df7ebb6a..cf3d33ab541 100644
--- a/drivers/media/video/cx88/cx88-dvb.c
+++ b/drivers/media/video/cx88/cx88-dvb.c
@@ -41,6 +41,7 @@
 #include "or51132.h"
 #include "lgdt330x.h"
 #include "s5h1409.h"
+#include "xc4000.h"
 #include "xc5000.h"
 #include "nxt200x.h"
 #include "cx24123.h"
@@ -63,6 +64,7 @@ MODULE_DESCRIPTION("driver for cx2388x based DVB cards");
 MODULE_AUTHOR("Chris Pascoe <c.pascoe@itee.uq.edu.au>");
 MODULE_AUTHOR("Gerd Knorr <kraxel@bytesex.org> [SuSE Labs]");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(CX88_VERSION);
 
 static unsigned int debug;
 module_param(debug, int, 0644);
@@ -605,6 +607,39 @@ static int attach_xc3028(u8 addr, struct cx8802_dev *dev)
         return 0;
 }
 
+static int attach_xc4000(struct cx8802_dev *dev, struct xc4000_config *cfg)
+{
+        struct dvb_frontend *fe;
+        struct videobuf_dvb_frontend *fe0 = NULL;
+
+        /* Get the first frontend */
+        fe0 = videobuf_dvb_get_frontend(&dev->frontends, 1);
+        if (!fe0)
+                return -EINVAL;
+
+        if (!fe0->dvb.frontend) {
+                printk(KERN_ERR "%s/2: dvb frontend not attached. "
+                                "Can't attach xc4000\n",
+                       dev->core->name);
+                return -EINVAL;
+        }
+
+        fe = dvb_attach(xc4000_attach, fe0->dvb.frontend, &dev->core->i2c_adap,
+                        cfg);
+        if (!fe) {
+                printk(KERN_ERR "%s/2: xc4000 attach failed\n",
+                       dev->core->name);
+                dvb_frontend_detach(fe0->dvb.frontend);
+                dvb_unregister_frontend(fe0->dvb.frontend);
+                fe0->dvb.frontend = NULL;
+                return -EINVAL;
+        }
+
+        printk(KERN_INFO "%s/2: xc4000 attached\n", dev->core->name);
+
+        return 0;
+}
+
 static int cx24116_set_ts_param(struct dvb_frontend *fe,
         int is_punctured)
 {
@@ -1294,7 +1329,25 @@ static int dvb_register(struct cx8802_dev *dev)
                                 goto frontend_detach;
                 }
                 break;
-         case CX88_BOARD_GENIATECH_X8000_MT:
+        case CX88_BOARD_WINFAST_DTV1800H_XC4000:
+        case CX88_BOARD_WINFAST_DTV2000H_PLUS:
+                fe0->dvb.frontend = dvb_attach(zl10353_attach,
+                                               &cx88_pinnacle_hybrid_pctv,
+                                               &core->i2c_adap);
+                if (fe0->dvb.frontend) {
+                        struct xc4000_config cfg = {
+                                .i2c_address      = 0x61,
+                                .default_pm       = 0,
+                                .dvb_amplitude    = 134,
+                                .set_smoothedcvbs = 1,
+                                .if_khz           = 4560
+                        };
+                        fe0->dvb.frontend->ops.i2c_gate_ctrl = NULL;
+                        if (attach_xc4000(dev, &cfg) < 0)
+                                goto frontend_detach;
+                }
+                break;
+        case CX88_BOARD_GENIATECH_X8000_MT:
                 dev->ts_gen_cntrl = 0x00;
 
                 fe0->dvb.frontend = dvb_attach(zl10353_attach,
@@ -1577,6 +1630,11 @@ static int cx8802_dvb_advise_acquire(struct cx8802_driver *drv)
                 udelay(1000);
                 break;
 
+        case CX88_BOARD_WINFAST_DTV2000H_PLUS:
+                /* set RF input to AIR for DVB-T (GPIO 16) */
+                cx_write(MO_GP2_IO, 0x0101);
+                break;
+
         default:
                 err = -ENODEV;
         }
@@ -1692,14 +1750,8 @@ static struct cx8802_driver cx8802_dvb_driver = {
 
 static int __init dvb_init(void)
 {
-        printk(KERN_INFO "cx88/2: cx2388x dvb driver version %d.%d.%d loaded\n",
-               (CX88_VERSION_CODE >> 16) & 0xff,
-               (CX88_VERSION_CODE >>  8) & 0xff,
-               CX88_VERSION_CODE & 0xff);
-#ifdef SNAPSHOT
-        printk(KERN_INFO "cx2388x: snapshot date %04d-%02d-%02d\n",
-               SNAPSHOT/10000, (SNAPSHOT/100)%100, SNAPSHOT%100);
-#endif
+        printk(KERN_INFO "cx88/2: cx2388x dvb driver version %s loaded\n",
+               CX88_VERSION);
         return cx8802_register_driver(&cx8802_dvb_driver);
 }
 
@@ -1710,10 +1762,3 @@ static void __exit dvb_fini(void)
 
 module_init(dvb_init);
 module_exit(dvb_fini);
-
-/*
- * Local variables:
- * c-basic-offset: 8
- * compile-command: "make DVB=1"
- * End:
- */
diff --git a/drivers/media/video/cx88/cx88-input.c b/drivers/media/video/cx88/cx88-input.c
index 3f442003623..e614201b5ed 100644
--- a/drivers/media/video/cx88/cx88-input.c
+++ b/drivers/media/video/cx88/cx88-input.c
@@ -100,6 +100,8 @@ static void cx88_ir_handle_key(struct cx88_IR *ir)
                 break;
         case CX88_BOARD_WINFAST_DTV1000:
         case CX88_BOARD_WINFAST_DTV1800H:
+        case CX88_BOARD_WINFAST_DTV1800H_XC4000:
+        case CX88_BOARD_WINFAST_DTV2000H_PLUS:
         case CX88_BOARD_WINFAST_TV2000_XP_GLOBAL:
                 gpio = (gpio & 0x6ff) | ((cx_read(MO_GP1_IO) << 8) & 0x900);
                 auxgpio = gpio;
@@ -289,6 +291,8 @@ int cx88_ir_init(struct cx88_core *core, struct pci_dev *pci)
         case CX88_BOARD_WINFAST_DTV2000H:
         case CX88_BOARD_WINFAST_DTV2000H_J:
         case CX88_BOARD_WINFAST_DTV1800H:
+        case CX88_BOARD_WINFAST_DTV1800H_XC4000:
+        case CX88_BOARD_WINFAST_DTV2000H_PLUS:
                 ir_codes = RC_MAP_WINFAST;
                 ir->gpio_addr = MO_GP0_IO;
                 ir->mask_keycode = 0x8f8;
diff --git a/drivers/media/video/cx88/cx88-mpeg.c b/drivers/media/video/cx88/cx88-mpeg.c
index 1a7b983f829..cd5386ee210 100644
--- a/drivers/media/video/cx88/cx88-mpeg.c
+++ b/drivers/media/video/cx88/cx88-mpeg.c
@@ -39,6 +39,7 @@ MODULE_AUTHOR("Jelle Foks <jelle@foks.us>");
 MODULE_AUTHOR("Chris Pascoe <c.pascoe@itee.uq.edu.au>");
 MODULE_AUTHOR("Gerd Knorr <kraxel@bytesex.org> [SuSE Labs]");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(CX88_VERSION);
 
 static unsigned int debug;
 module_param(debug,int,0644);
@@ -613,13 +614,17 @@ static int cx8802_request_acquire(struct cx8802_driver *drv)
             core->active_type_id != drv->type_id)
                 return -EBUSY;
 
-        core->input = 0;
-        for (i = 0;
-             i < (sizeof(core->board.input) / sizeof(struct cx88_input));
-             i++) {
-                if (core->board.input[i].type == CX88_VMUX_DVB) {
-                        core->input = i;
-                        break;
+        if (drv->type_id == CX88_MPEG_DVB) {
+                /* When switching to DVB, always set the input to the tuner */
+                core->last_analog_input = core->input;
+                core->input = 0;
+                for (i = 0;
+                     i < (sizeof(core->board.input) / sizeof(struct cx88_input));
+                     i++) {
+                        if (core->board.input[i].type == CX88_VMUX_DVB) {
+                                core->input = i;
+                                break;
+                        }
                 }
         }
 
@@ -644,6 +649,12 @@ static int cx8802_request_release(struct cx8802_driver *drv)
 
         if (drv->advise_release && --core->active_ref == 0)
         {
+                if (drv->type_id == CX88_MPEG_DVB) {
+                        /* If the DVB driver is releasing, reset the input
+                           state to the last configured analog input */
+                        core->input = core->last_analog_input;
+                }
+
                 drv->advise_release(drv);
                 core->active_type_id = CX88_BOARD_NONE;
                 mpeg_dbg(1,"%s() Post release GPIO=%x\n", __func__, cx_read(MO_GP0_IO));
@@ -890,14 +901,8 @@ static struct pci_driver cx8802_pci_driver = {
 
 static int __init cx8802_init(void)
 {
-        printk(KERN_INFO "cx88/2: cx2388x MPEG-TS Driver Manager version %d.%d.%d loaded\n",
-               (CX88_VERSION_CODE >> 16) & 0xff,
-               (CX88_VERSION_CODE >>  8) & 0xff,
-               CX88_VERSION_CODE & 0xff);
-#ifdef SNAPSHOT
-        printk(KERN_INFO "cx2388x: snapshot date %04d-%02d-%02d\n",
-               SNAPSHOT/10000, (SNAPSHOT/100)%100, SNAPSHOT%100);
-#endif
+        printk(KERN_INFO "cx88/2: cx2388x MPEG-TS Driver Manager version %s loaded\n",
+               CX88_VERSION);
         return pci_register_driver(&cx8802_pci_driver);
 }
 
diff --git a/drivers/media/video/cx88/cx88-video.c b/drivers/media/video/cx88/cx88-video.c
index cef4f282e5a..60d28fdd779 100644
--- a/drivers/media/video/cx88/cx88-video.c
+++ b/drivers/media/video/cx88/cx88-video.c
@@ -45,6 +45,7 @@
 MODULE_DESCRIPTION("v4l2 driver module for cx2388x based TV cards");
 MODULE_AUTHOR("Gerd Knorr <kraxel@bytesex.org> [SuSE Labs]");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(CX88_VERSION);
 
 /* ------------------------------------------------------------------ */
 
@@ -220,7 +221,23 @@ static const struct cx88_ctrl cx8800_ctls[] = {
                 .reg                   = MO_UV_SATURATION,
                 .mask                  = 0x00ff,
                 .shift                 = 0,
-        },{
+        }, {
+                .v = {
+                        .id            = V4L2_CID_SHARPNESS,
+                        .name          = "Sharpness",
+                        .minimum       = 0,
+                        .maximum       = 4,
+                        .step          = 1,
+                        .default_value = 0x0,
+                        .type          = V4L2_CTRL_TYPE_INTEGER,
+                },
+                .off                   = 0,
+                /* NOTE: the value is converted and written to both even
+                   and odd registers in the code */
+                .reg                   = MO_FILTER_ODD,
+                .mask                  = 7 << 7,
+                .shift                 = 7,
+        }, {
                 .v = {
                         .id            = V4L2_CID_CHROMA_AGC,
                         .name          = "Chroma AGC",
@@ -245,6 +262,20 @@ static const struct cx88_ctrl cx8800_ctls[] = {
                 .mask                  = 1 << 9,
                 .shift                 = 9,
         }, {
+                .v = {
+                        .id            = V4L2_CID_BAND_STOP_FILTER,
+                        .name          = "Notch filter",
+                        .minimum       = 0,
+                        .maximum       = 3,
+                        .step          = 1,
+                        .default_value = 0x0,
+                        .type          = V4L2_CTRL_TYPE_INTEGER,
+                },
+                .off                   = 0,
+                .reg                   = MO_HTOTAL,
+                .mask                  = 3 << 11,
+                .shift                 = 11,
+        }, {
         /* --- audio --- */
                 .v = {
                         .id            = V4L2_CID_AUDIO_MUTE,
@@ -300,8 +331,10 @@ const u32 cx88_user_ctrls[] = {
         V4L2_CID_AUDIO_VOLUME,
         V4L2_CID_AUDIO_BALANCE,
         V4L2_CID_AUDIO_MUTE,
+        V4L2_CID_SHARPNESS,
         V4L2_CID_CHROMA_AGC,
         V4L2_CID_COLOR_KILLER,
+        V4L2_CID_BAND_STOP_FILTER,
         0
 };
 EXPORT_SYMBOL(cx88_user_ctrls);
@@ -962,6 +995,10 @@ int cx88_get_control (struct cx88_core  *core, struct v4l2_control *ctl)
         case V4L2_CID_AUDIO_VOLUME:
                 ctl->value = 0x3f - (value & 0x3f);
                 break;
+        case V4L2_CID_SHARPNESS:
+                ctl->value = ((value & 0x0200) ? (((value & 0x0180) >> 7) + 1)
+                                                 : 0);
+                break;
         default:
                 ctl->value = ((value + (c->off << c->shift)) & c->mask) >> c->shift;
                 break;
@@ -1039,6 +1076,12 @@ int cx88_set_control(struct cx88_core *core, struct v4l2_control *ctl)
                 }
                 mask=0xffff;
                 break;
+        case V4L2_CID_SHARPNESS:
+                /* 0b000, 0b100, 0b101, 0b110, or 0b111 */
+                value = (ctl->value < 1 ? 0 : ((ctl->value + 3) << 7));
+                /* needs to be set for both fields */
+                cx_andor(MO_FILTER_EVEN, mask, value);
+                break;
         case V4L2_CID_CHROMA_AGC:
                 /* Do not allow chroma AGC to be enabled for SECAM */
                 value = ((ctl->value - c->off) << c->shift) & c->mask;
@@ -1161,7 +1204,6 @@ static int vidioc_querycap (struct file *file, void  *priv,
         strcpy(cap->driver, "cx8800");
         strlcpy(cap->card, core->board.name, sizeof(cap->card));
         sprintf(cap->bus_info,"PCI:%s",pci_name(dev->pci));
-        cap->version = CX88_VERSION_CODE;
         cap->capabilities =
                 V4L2_CAP_VIDEO_CAPTURE |
                 V4L2_CAP_READWRITE     |
@@ -1480,7 +1522,6 @@ static int radio_querycap (struct file *file, void  *priv,
         strcpy(cap->driver, "cx8800");
         strlcpy(cap->card, core->board.name, sizeof(cap->card));
         sprintf(cap->bus_info,"PCI:%s", pci_name(dev->pci));
-        cap->version = CX88_VERSION_CODE;
         cap->capabilities = V4L2_CAP_TUNER;
         return 0;
 }
@@ -2139,14 +2180,8 @@ static struct pci_driver cx8800_pci_driver = {
 
 static int __init cx8800_init(void)
 {
-        printk(KERN_INFO "cx88/0: cx2388x v4l2 driver version %d.%d.%d loaded\n",
-               (CX88_VERSION_CODE >> 16) & 0xff,
-               (CX88_VERSION_CODE >>  8) & 0xff,
-               CX88_VERSION_CODE & 0xff);
-#ifdef SNAPSHOT
-        printk(KERN_INFO "cx2388x: snapshot date %04d-%02d-%02d\n",
-               SNAPSHOT/10000, (SNAPSHOT/100)%100, SNAPSHOT%100);
-#endif
+        printk(KERN_INFO "cx88/0: cx2388x v4l2 driver version %s loaded\n",
+               CX88_VERSION);
         return pci_register_driver(&cx8800_pci_driver);
 }
 
@@ -2157,11 +2192,3 @@ static void __exit cx8800_fini(void)
 
 module_init(cx8800_init);
 module_exit(cx8800_fini);
-
-/* ----------------------------------------------------------- */
-/*
- * Local variables:
- * c-basic-offset: 8
- * End:
- * kate: eol "unix"; indent-width 3; remove-trailing-space on; replace-trailing-space-save on; tab-width 8; replace-tabs off; space-indent off; mixed-indent off
- */
diff --git a/drivers/media/video/cx88/cx88.h b/drivers/media/video/cx88/cx88.h
index a399a8b086b..fa8d307e1a3 100644
--- a/drivers/media/video/cx88/cx88.h
+++ b/drivers/media/video/cx88/cx88.h
@@ -39,9 +39,9 @@
 #include "cx88-reg.h"
 #include "tuner-xc2028.h"
 
-#include <linux/version.h>
 #include <linux/mutex.h>
-#define CX88_VERSION_CODE KERNEL_VERSION(0, 0, 8)
+
+#define CX88_VERSION "0.0.9"
 
 #define UNSET (-1U)
 
@@ -242,6 +242,8 @@ extern const struct sram_channel const cx88_sram_channels[];
 #define CX88_BOARD_SAMSUNG_SMT_7020        84
 #define CX88_BOARD_TWINHAN_VP1027_DVBS     85
 #define CX88_BOARD_TEVII_S464              86
+#define CX88_BOARD_WINFAST_DTV2000H_PLUS   87
+#define CX88_BOARD_WINFAST_DTV1800H_XC4000 88
 
 enum cx88_itype {
         CX88_VMUX_COMPOSITE1 = 1,
@@ -375,6 +377,7 @@ struct cx88_core {
         u32                        audiomode_manual;
         u32                        audiomode_current;
         u32                        input;
+        u32                        last_analog_input;
         u32                        astat;
         u32                        use_nicam;
         unsigned long              last_change;
diff --git a/drivers/media/video/davinci/Kconfig b/drivers/media/video/davinci/Kconfig
index 6b195403564..60a456ebdc7 100644
--- a/drivers/media/video/davinci/Kconfig
+++ b/drivers/media/video/davinci/Kconfig
@@ -91,3 +91,26 @@ config VIDEO_ISIF
 
            To compile this driver as a module, choose M here: the
            module will be called vpfe.
+
+config VIDEO_DM644X_VPBE
+        tristate "DM644X VPBE HW module"
+        depends on ARCH_DAVINCI_DM644x
+        select VIDEO_VPSS_SYSTEM
+        select VIDEOBUF_DMA_CONTIG
+        help
+            Enables VPBE modules used for display on a DM644x
+            SoC.
+
+            To compile this driver as a module, choose M here: the
+            module will be called vpbe.
+
+
+config VIDEO_VPBE_DISPLAY
+        tristate "VPBE V4L2 Display driver"
+        depends on ARCH_DAVINCI_DM644x
+        select VIDEO_DM644X_VPBE
+        help
+            Enables VPBE V4L2 Display driver on a DM644x device
+
+            To compile this driver as a module, choose M here: the
+            module will be called vpbe_display.
diff --git a/drivers/media/video/davinci/Makefile b/drivers/media/video/davinci/Makefile
index a37955745aa..ae7dafb689a 100644
--- a/drivers/media/video/davinci/Makefile
+++ b/drivers/media/video/davinci/Makefile
@@ -16,3 +16,5 @@ obj-$(CONFIG_VIDEO_VPFE_CAPTURE) += vpfe_capture.o
 obj-$(CONFIG_VIDEO_DM6446_CCDC) += dm644x_ccdc.o
 obj-$(CONFIG_VIDEO_DM355_CCDC) += dm355_ccdc.o
 obj-$(CONFIG_VIDEO_ISIF) += isif.o
+obj-$(CONFIG_VIDEO_DM644X_VPBE) += vpbe.o vpbe_osd.o vpbe_venc.o
+obj-$(CONFIG_VIDEO_VPBE_DISPLAY) += vpbe_display.o
diff --git a/drivers/media/video/davinci/vpbe.c b/drivers/media/video/davinci/vpbe.c
new file mode 100644
index 00000000000..d773d30de22
--- /dev/null
+++ b/drivers/media/video/davinci/vpbe.c
@@ -0,0 +1,864 @@
+/*
+ * Copyright (C) 2010 Texas Instruments 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 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
+ */
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/string.h>
+#include <linux/wait.h>
+#include <linux/time.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+
+#include <media/v4l2-device.h>
+#include <media/davinci/vpbe_types.h>
+#include <media/davinci/vpbe.h>
+#include <media/davinci/vpss.h>
+#include <media/davinci/vpbe_venc.h>
+
+#define VPBE_DEFAULT_OUTPUT     "Composite"
+#define VPBE_DEFAULT_MODE       "ntsc"
+
+static char *def_output = VPBE_DEFAULT_OUTPUT;
+static char *def_mode = VPBE_DEFAULT_MODE;
+static int debug;
+
+module_param(def_output, charp, S_IRUGO);
+module_param(def_mode, charp, S_IRUGO);
+module_param(debug, int, 0644);
+
+MODULE_PARM_DESC(def_output, "vpbe output name (default:Composite)");
+MODULE_PARM_DESC(def_mode, "vpbe output mode name (default:ntsc");
+MODULE_PARM_DESC(debug, "Debug level 0-1");
+
+MODULE_DESCRIPTION("TI DMXXX VPBE Display controller");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Texas Instruments");
+
+/**
+ * vpbe_current_encoder_info - Get config info for current encoder
+ * @vpbe_dev - vpbe device ptr
+ *
+ * Return ptr to current encoder config info
+ */
+static struct encoder_config_info*
+vpbe_current_encoder_info(struct vpbe_device *vpbe_dev)
+{
+        struct vpbe_config *cfg = vpbe_dev->cfg;
+        int index = vpbe_dev->current_sd_index;
+
+        return ((index == 0) ? &cfg->venc :
+                                &cfg->ext_encoders[index-1]);
+}
+
+/**
+ * vpbe_find_encoder_sd_index - Given a name find encoder sd index
+ *
+ * @vpbe_config - ptr to vpbe cfg
+ * @output_index - index used by application
+ *
+ * Return sd index of the encoder
+ */
+static int vpbe_find_encoder_sd_index(struct vpbe_config *cfg,
+                             int index)
+{
+        char *encoder_name = cfg->outputs[index].subdev_name;
+        int i;
+
+        /* Venc is always first */
+        if (!strcmp(encoder_name, cfg->venc.module_name))
+                return 0;
+
+        for (i = 0; i < cfg->num_ext_encoders; i++) {
+                if (!strcmp(encoder_name,
+                     cfg->ext_encoders[i].module_name))
+                        return i+1;
+        }
+
+        return -EINVAL;
+}
+
+/**
+ * vpbe_g_cropcap - Get crop capabilities of the display
+ * @vpbe_dev - vpbe device ptr
+ * @cropcap - cropcap is a ptr to struct v4l2_cropcap
+ *
+ * Update the crop capabilities in crop cap for current
+ * mode
+ */
+static int vpbe_g_cropcap(struct vpbe_device *vpbe_dev,
+                          struct v4l2_cropcap *cropcap)
+{
+        if (NULL == cropcap)
+                return -EINVAL;
+        cropcap->bounds.left = 0;
+        cropcap->bounds.top = 0;
+        cropcap->bounds.width = vpbe_dev->current_timings.xres;
+        cropcap->bounds.height = vpbe_dev->current_timings.yres;
+        cropcap->defrect = cropcap->bounds;
+
+        return 0;
+}
+
+/**
+ * vpbe_enum_outputs - enumerate outputs
+ * @vpbe_dev - vpbe device ptr
+ * @output - ptr to v4l2_output structure
+ *
+ * Enumerates the outputs available at the vpbe display
+ * returns the status, -EINVAL if end of output list
+ */
+static int vpbe_enum_outputs(struct vpbe_device *vpbe_dev,
+                             struct v4l2_output *output)
+{
+        struct vpbe_config *cfg = vpbe_dev->cfg;
+        int temp_index = output->index;
+
+        if (temp_index >= cfg->num_outputs)
+                return -EINVAL;
+
+        *output = cfg->outputs[temp_index].output;
+        output->index = temp_index;
+
+        return 0;
+}
+
+static int vpbe_get_mode_info(struct vpbe_device *vpbe_dev, char *mode)
+{
+        struct vpbe_config *cfg = vpbe_dev->cfg;
+        struct vpbe_enc_mode_info var;
+        int curr_output = vpbe_dev->current_out_index;
+        int i;
+
+        if (NULL == mode)
+                return -EINVAL;
+
+        for (i = 0; i < cfg->outputs[curr_output].num_modes; i++) {
+                var = cfg->outputs[curr_output].modes[i];
+                if (!strcmp(mode, var.name)) {
+                        vpbe_dev->current_timings = var;
+                        return 0;
+                }
+        }
+
+        return -EINVAL;
+}
+
+static int vpbe_get_current_mode_info(struct vpbe_device *vpbe_dev,
+                                      struct vpbe_enc_mode_info *mode_info)
+{
+        if (NULL == mode_info)
+                return -EINVAL;
+
+        *mode_info = vpbe_dev->current_timings;
+
+        return 0;
+}
+
+static int vpbe_get_dv_preset_info(struct vpbe_device *vpbe_dev,
+                                   unsigned int dv_preset)
+{
+        struct vpbe_config *cfg = vpbe_dev->cfg;
+        struct vpbe_enc_mode_info var;
+        int curr_output = vpbe_dev->current_out_index;
+        int i;
+
+        for (i = 0; i < vpbe_dev->cfg->outputs[curr_output].num_modes; i++) {
+                var = cfg->outputs[curr_output].modes[i];
+                if ((var.timings_type & VPBE_ENC_DV_PRESET) &&
+                  (var.timings.dv_preset == dv_preset)) {
+                        vpbe_dev->current_timings = var;
+                        return 0;
+                }
+        }
+
+        return -EINVAL;
+}
+
+/* Get std by std id */
+static int vpbe_get_std_info(struct vpbe_device *vpbe_dev,
+                             v4l2_std_id std_id)
+{
+        struct vpbe_config *cfg = vpbe_dev->cfg;
+        struct vpbe_enc_mode_info var;
+        int curr_output = vpbe_dev->current_out_index;
+        int i;
+
+        for (i = 0; i < vpbe_dev->cfg->outputs[curr_output].num_modes; i++) {
+                var = cfg->outputs[curr_output].modes[i];
+                if ((var.timings_type & VPBE_ENC_STD) &&
+                  (var.timings.std_id & std_id)) {
+                        vpbe_dev->current_timings = var;
+                        return 0;
+                }
+        }
+
+        return -EINVAL;
+}
+
+static int vpbe_get_std_info_by_name(struct vpbe_device *vpbe_dev,
+                                char *std_name)
+{
+        struct vpbe_config *cfg = vpbe_dev->cfg;
+        struct vpbe_enc_mode_info var;
+        int curr_output = vpbe_dev->current_out_index;
+        int i;
+
+        for (i = 0; i < vpbe_dev->cfg->outputs[curr_output].num_modes; i++) {
+                var = cfg->outputs[curr_output].modes[i];
+                if (!strcmp(var.name, std_name)) {
+                        vpbe_dev->current_timings = var;
+                        return 0;
+                }
+        }
+
+        return -EINVAL;
+}
+
+/**
+ * vpbe_set_output - Set output
+ * @vpbe_dev - vpbe device ptr
+ * @index - index of output
+ *
+ * Set vpbe output to the output specified by the index
+ */
+static int vpbe_set_output(struct vpbe_device *vpbe_dev, int index)
+{
+        struct encoder_config_info *curr_enc_info =
+                        vpbe_current_encoder_info(vpbe_dev);
+        struct vpbe_config *cfg = vpbe_dev->cfg;
+        int enc_out_index;
+        int sd_index;
+        int ret = 0;
+
+        if (index >= cfg->num_outputs)
+                return -EINVAL;
+
+        mutex_lock(&vpbe_dev->lock);
+
+        sd_index = vpbe_dev->current_sd_index;
+        enc_out_index = cfg->outputs[index].output.index;
+        /*
+         * Currently we switch the encoder based on output selected
+         * by the application. If media controller is implemented later
+         * there is will be an API added to setup_link between venc
+         * and external encoder. So in that case below comparison always
+         * match and encoder will not be switched. But if application
+         * chose not to use media controller, then this provides current
+         * way of switching encoder at the venc output.
+         */
+        if (strcmp(curr_enc_info->module_name,
+                   cfg->outputs[index].subdev_name)) {
+                /* Need to switch the encoder at the output */
+                sd_index = vpbe_find_encoder_sd_index(cfg, index);
+                if (sd_index < 0) {
+                        ret = -EINVAL;
+                        goto out;
+                }
+
+                if (ret)
+                        goto out;
+        }
+
+        /* Set output at the encoder */
+        ret = v4l2_subdev_call(vpbe_dev->encoders[sd_index], video,
+                                       s_routing, 0, enc_out_index, 0);
+        if (ret)
+                goto out;
+
+        /*
+         * It is assumed that venc or extenal encoder will set a default
+         * mode in the sub device. For external encoder or LCD pannel output,
+         * we also need to set up the lcd port for the required mode. So setup
+         * the lcd port for the default mode that is configured in the board
+         * arch/arm/mach-davinci/board-dm355-evm.setup file for the external
+         * encoder.
+         */
+        ret = vpbe_get_mode_info(vpbe_dev,
+                                 cfg->outputs[index].default_mode);
+        if (!ret) {
+                struct osd_state *osd_device = vpbe_dev->osd_device;
+
+                osd_device->ops.set_left_margin(osd_device,
+                        vpbe_dev->current_timings.left_margin);
+                osd_device->ops.set_top_margin(osd_device,
+                vpbe_dev->current_timings.upper_margin);
+                vpbe_dev->current_sd_index = sd_index;
+                vpbe_dev->current_out_index = index;
+        }
+out:
+        mutex_unlock(&vpbe_dev->lock);
+        return ret;
+}
+
+static int vpbe_set_default_output(struct vpbe_device *vpbe_dev)
+{
+        struct vpbe_config *cfg = vpbe_dev->cfg;
+        int ret = 0;
+        int i;
+
+        for (i = 0; i < cfg->num_outputs; i++) {
+                if (!strcmp(def_output,
+                            cfg->outputs[i].output.name)) {
+                        ret = vpbe_set_output(vpbe_dev, i);
+                        if (!ret)
+                                vpbe_dev->current_out_index = i;
+                        return ret;
+                }
+        }
+        return ret;
+}
+
+/**
+ * vpbe_get_output - Get output
+ * @vpbe_dev - vpbe device ptr
+ *
+ * return current vpbe output to the the index
+ */
+static unsigned int vpbe_get_output(struct vpbe_device *vpbe_dev)
+{
+        return vpbe_dev->current_out_index;
+}
+
+/**
+ * vpbe_s_dv_preset - Set the given preset timings in the encoder
+ *
+ * Sets the preset if supported by the current encoder. Return the status.
+ * 0 - success & -EINVAL on error
+ */
+static int vpbe_s_dv_preset(struct vpbe_device *vpbe_dev,
+                     struct v4l2_dv_preset *dv_preset)
+{
+        struct vpbe_config *cfg = vpbe_dev->cfg;
+        int out_index = vpbe_dev->current_out_index;
+        int sd_index = vpbe_dev->current_sd_index;
+        int ret;
+
+
+        if (!(cfg->outputs[out_index].output.capabilities &
+            V4L2_OUT_CAP_PRESETS))
+                return -EINVAL;
+
+        ret = vpbe_get_dv_preset_info(vpbe_dev, dv_preset->preset);
+
+        if (ret)
+                return ret;
+
+        mutex_lock(&vpbe_dev->lock);
+
+
+        ret = v4l2_subdev_call(vpbe_dev->encoders[sd_index], video,
+                                        s_dv_preset, dv_preset);
+        /* set the lcd controller output for the given mode */
+        if (!ret) {
+                struct osd_state *osd_device = vpbe_dev->osd_device;
+
+                osd_device->ops.set_left_margin(osd_device,
+                vpbe_dev->current_timings.left_margin);
+                osd_device->ops.set_top_margin(osd_device,
+                vpbe_dev->current_timings.upper_margin);
+        }
+        mutex_unlock(&vpbe_dev->lock);
+
+        return ret;
+}
+
+/**
+ * vpbe_g_dv_preset - Get the preset in the current encoder
+ *
+ * Get the preset in the current encoder. Return the status. 0 - success
+ * -EINVAL on error
+ */
+static int vpbe_g_dv_preset(struct vpbe_device *vpbe_dev,
+                     struct v4l2_dv_preset *dv_preset)
+{
+        if (vpbe_dev->current_timings.timings_type &
+          VPBE_ENC_DV_PRESET) {
+                dv_preset->preset = vpbe_dev->current_timings.timings.dv_preset;
+                return 0;
+        }
+
+        return -EINVAL;
+}
+
+/**
+ * vpbe_enum_dv_presets - Enumerate the dv presets in the current encoder
+ *
+ * Get the preset in the current encoder. Return the status. 0 - success
+ * -EINVAL on error
+ */
+static int vpbe_enum_dv_presets(struct vpbe_device *vpbe_dev,
+                         struct v4l2_dv_enum_preset *preset_info)
+{
+        struct vpbe_config *cfg = vpbe_dev->cfg;
+        int out_index = vpbe_dev->current_out_index;
+        struct vpbe_output *output = &cfg->outputs[out_index];
+        int j = 0;
+        int i;
+
+        if (!(output->output.capabilities & V4L2_OUT_CAP_PRESETS))
+                return -EINVAL;
+
+        for (i = 0; i < output->num_modes; i++) {
+                if (output->modes[i].timings_type == VPBE_ENC_DV_PRESET) {
+                        if (j == preset_info->index)
+                                break;
+                        j++;
+                }
+        }
+
+        if (i == output->num_modes)
+                return -EINVAL;
+
+        return v4l_fill_dv_preset_info(output->modes[i].timings.dv_preset,
+                                        preset_info);
+}
+
+/**
+ * vpbe_s_std - Set the given standard in the encoder
+ *
+ * Sets the standard if supported by the current encoder. Return the status.
+ * 0 - success & -EINVAL on error
+ */
+static int vpbe_s_std(struct vpbe_device *vpbe_dev, v4l2_std_id *std_id)
+{
+        struct vpbe_config *cfg = vpbe_dev->cfg;
+        int out_index = vpbe_dev->current_out_index;
+        int sd_index = vpbe_dev->current_sd_index;
+        int ret;
+
+        if (!(cfg->outputs[out_index].output.capabilities &
+                V4L2_OUT_CAP_STD))
+                return -EINVAL;
+
+        ret = vpbe_get_std_info(vpbe_dev, *std_id);
+        if (ret)
+                return ret;
+
+        mutex_lock(&vpbe_dev->lock);
+
+        ret = v4l2_subdev_call(vpbe_dev->encoders[sd_index], video,
+                               s_std_output, *std_id);
+        /* set the lcd controller output for the given mode */
+        if (!ret) {
+                struct osd_state *osd_device = vpbe_dev->osd_device;
+
+                osd_device->ops.set_left_margin(osd_device,
+                vpbe_dev->current_timings.left_margin);
+                osd_device->ops.set_top_margin(osd_device,
+                vpbe_dev->current_timings.upper_margin);
+        }
+        mutex_unlock(&vpbe_dev->lock);
+
+        return ret;
+}
+
+/**
+ * vpbe_g_std - Get the standard in the current encoder
+ *
+ * Get the standard in the current encoder. Return the status. 0 - success
+ * -EINVAL on error
+ */
+static int vpbe_g_std(struct vpbe_device *vpbe_dev, v4l2_std_id *std_id)
+{
+        struct vpbe_enc_mode_info cur_timings = vpbe_dev->current_timings;
+
+        if (cur_timings.timings_type & VPBE_ENC_STD) {
+                *std_id = cur_timings.timings.std_id;
+                return 0;
+        }
+
+        return -EINVAL;
+}
+
+/**
+ * vpbe_set_mode - Set mode in the current encoder using mode info
+ *
+ * Use the mode string to decide what timings to set in the encoder
+ * This is typically useful when fbset command is used to change the current
+ * timings by specifying a string to indicate the timings.
+ */
+static int vpbe_set_mode(struct vpbe_device *vpbe_dev,
+                         struct vpbe_enc_mode_info *mode_info)
+{
+        struct vpbe_enc_mode_info *preset_mode = NULL;
+        struct vpbe_config *cfg = vpbe_dev->cfg;
+        struct v4l2_dv_preset dv_preset;
+        struct osd_state *osd_device;
+        int out_index = vpbe_dev->current_out_index;
+        int ret = 0;
+        int i;
+
+        if ((NULL == mode_info) || (NULL == mode_info->name))
+                return -EINVAL;
+
+        for (i = 0; i < cfg->outputs[out_index].num_modes; i++) {
+                if (!strcmp(mode_info->name,
+                     cfg->outputs[out_index].modes[i].name)) {
+                        preset_mode = &cfg->outputs[out_index].modes[i];
+                        /*
+                         * it may be one of the 3 timings type. Check and
+                         * invoke right API
+                         */
+                        if (preset_mode->timings_type & VPBE_ENC_STD)
+                                return vpbe_s_std(vpbe_dev,
+                                                 &preset_mode->timings.std_id);
+                        if (preset_mode->timings_type & VPBE_ENC_DV_PRESET) {
+                                dv_preset.preset =
+                                        preset_mode->timings.dv_preset;
+                                return vpbe_s_dv_preset(vpbe_dev, &dv_preset);
+                        }
+                }
+        }
+
+        /* Only custom timing should reach here */
+        if (preset_mode == NULL)
+                return -EINVAL;
+
+        mutex_lock(&vpbe_dev->lock);
+
+        osd_device = vpbe_dev->osd_device;
+        vpbe_dev->current_timings = *preset_mode;
+        osd_device->ops.set_left_margin(osd_device,
+                vpbe_dev->current_timings.left_margin);
+        osd_device->ops.set_top_margin(osd_device,
+                vpbe_dev->current_timings.upper_margin);
+
+        mutex_unlock(&vpbe_dev->lock);
+
+        return ret;
+}
+
+static int vpbe_set_default_mode(struct vpbe_device *vpbe_dev)
+{
+        int ret;
+
+        ret = vpbe_get_std_info_by_name(vpbe_dev, def_mode);
+        if (ret)
+                return ret;
+
+        /* set the default mode in the encoder */
+        return vpbe_set_mode(vpbe_dev, &vpbe_dev->current_timings);
+}
+
+static int platform_device_get(struct device *dev, void *data)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct vpbe_device *vpbe_dev = data;
+
+        if (strcmp("vpbe-osd", pdev->name) == 0)
+                vpbe_dev->osd_device = platform_get_drvdata(pdev);
+
+        return 0;
+}
+
+/**
+ * vpbe_initialize() - Initialize the vpbe display controller
+ * @vpbe_dev - vpbe device ptr
+ *
+ * Master frame buffer device drivers calls this to initialize vpbe
+ * display controller. This will then registers v4l2 device and the sub
+ * devices and sets a current encoder sub device for display. v4l2 display
+ * device driver is the master and frame buffer display device driver is
+ * the slave. Frame buffer display driver checks the initialized during
+ * probe and exit if not initialized. Returns status.
+ */
+static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
+{
+        struct encoder_config_info *enc_info;
+        struct v4l2_subdev **enc_subdev;
+        struct osd_state *osd_device;
+        struct i2c_adapter *i2c_adap;
+        int output_index;
+        int num_encoders;
+        int ret = 0;
+        int err;
+        int i;
+
+        /*
+         * v4l2 abd FBDev frame buffer devices will get the vpbe_dev pointer
+         * from the platform device by iteration of platform drivers and
+         * matching with device name
+         */
+        if (NULL == vpbe_dev || NULL == dev) {
+                printk(KERN_ERR "Null device pointers.\n");
+                return -ENODEV;
+        }
+
+        if (vpbe_dev->initialized)
+                return 0;
+
+        mutex_lock(&vpbe_dev->lock);
+
+        if (strcmp(vpbe_dev->cfg->module_name, "dm644x-vpbe-display") != 0) {
+                /* We have dac clock available for platform */
+                vpbe_dev->dac_clk = clk_get(vpbe_dev->pdev, "vpss_dac");
+                if (IS_ERR(vpbe_dev->dac_clk)) {
+                        ret =  PTR_ERR(vpbe_dev->dac_clk);
+                        goto vpbe_unlock;
+                }
+                if (clk_enable(vpbe_dev->dac_clk)) {
+                        ret =  -ENODEV;
+                        goto vpbe_unlock;
+                }
+        }
+
+        /* first enable vpss clocks */
+        vpss_enable_clock(VPSS_VPBE_CLOCK, 1);
+
+        /* First register a v4l2 device */
+        ret = v4l2_device_register(dev, &vpbe_dev->v4l2_dev);
+        if (ret) {
+                v4l2_err(dev->driver,
+                        "Unable to register v4l2 device.\n");
+                goto vpbe_fail_clock;
+        }
+        v4l2_info(&vpbe_dev->v4l2_dev, "vpbe v4l2 device registered\n");
+
+        err = bus_for_each_dev(&platform_bus_type, NULL, vpbe_dev,
+                               platform_device_get);
+        if (err < 0)
+                return err;
+
+        vpbe_dev->venc = venc_sub_dev_init(&vpbe_dev->v4l2_dev,
+                                           vpbe_dev->cfg->venc.module_name);
+        /* register venc sub device */
+        if (vpbe_dev->venc == NULL) {
+                v4l2_err(&vpbe_dev->v4l2_dev,
+                        "vpbe unable to init venc sub device\n");
+                ret = -ENODEV;
+                goto vpbe_fail_v4l2_device;
+        }
+        /* initialize osd device */
+        osd_device = vpbe_dev->osd_device;
+
+        if (NULL != osd_device->ops.initialize) {
+                err = osd_device->ops.initialize(osd_device);
+                if (err) {
+                        v4l2_err(&vpbe_dev->v4l2_dev,
+                                 "unable to initialize the OSD device");
+                        err = -ENOMEM;
+                        goto vpbe_fail_v4l2_device;
+                }
+        }
+
+        /*
+         * Register any external encoders that are configured. At index 0 we
+         * store venc sd index.
+         */
+        num_encoders = vpbe_dev->cfg->num_ext_encoders + 1;
+        vpbe_dev->encoders = kmalloc(
+                                sizeof(struct v4l2_subdev *)*num_encoders,
+                                GFP_KERNEL);
+        if (NULL == vpbe_dev->encoders) {
+                v4l2_err(&vpbe_dev->v4l2_dev,
+                        "unable to allocate memory for encoders sub devices");
+                ret = -ENOMEM;
+                goto vpbe_fail_v4l2_device;
+        }
+
+        i2c_adap = i2c_get_adapter(vpbe_dev->cfg->i2c_adapter_id);
+        for (i = 0; i < (vpbe_dev->cfg->num_ext_encoders + 1); i++) {
+                if (i == 0) {
+                        /* venc is at index 0 */
+                        enc_subdev = &vpbe_dev->encoders[i];
+                        *enc_subdev = vpbe_dev->venc;
+                        continue;
+                }
+                enc_info = &vpbe_dev->cfg->ext_encoders[i];
+                if (enc_info->is_i2c) {
+                        enc_subdev = &vpbe_dev->encoders[i];
+                        *enc_subdev = v4l2_i2c_new_subdev_board(
+                                                &vpbe_dev->v4l2_dev, i2c_adap,
+                                                &enc_info->board_info, NULL);
+                        if (*enc_subdev)
+                                v4l2_info(&vpbe_dev->v4l2_dev,
+                                          "v4l2 sub device %s registered\n",
+                                          enc_info->module_name);
+                        else {
+                                v4l2_err(&vpbe_dev->v4l2_dev, "encoder %s"
+                                         " failed to register",
+                                         enc_info->module_name);
+                                ret = -ENODEV;
+                                goto vpbe_fail_sd_register;
+                        }
+                } else
+                        v4l2_warn(&vpbe_dev->v4l2_dev, "non-i2c encoders"
+                                 " currently not supported");
+        }
+
+        /* set the current encoder and output to that of venc by default */
+        vpbe_dev->current_sd_index = 0;
+        vpbe_dev->current_out_index = 0;
+        output_index = 0;
+
+        mutex_unlock(&vpbe_dev->lock);
+
+        printk(KERN_NOTICE "Setting default output to %s\n", def_output);
+        ret = vpbe_set_default_output(vpbe_dev);
+        if (ret) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "Failed to set default output %s",
+                         def_output);
+                return ret;
+        }
+
+        printk(KERN_NOTICE "Setting default mode to %s\n", def_mode);
+        ret = vpbe_set_default_mode(vpbe_dev);
+        if (ret) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "Failed to set default mode %s",
+                         def_mode);
+                return ret;
+        }
+        vpbe_dev->initialized = 1;
+        /* TBD handling of bootargs for default output and mode */
+        return 0;
+
+vpbe_fail_sd_register:
+        kfree(vpbe_dev->encoders);
+vpbe_fail_v4l2_device:
+        v4l2_device_unregister(&vpbe_dev->v4l2_dev);
+vpbe_fail_clock:
+        if (strcmp(vpbe_dev->cfg->module_name, "dm644x-vpbe-display") != 0)
+                clk_put(vpbe_dev->dac_clk);
+vpbe_unlock:
+        mutex_unlock(&vpbe_dev->lock);
+        return ret;
+}
+
+/**
+ * vpbe_deinitialize() - de-initialize the vpbe display controller
+ * @dev - Master and slave device ptr
+ *
+ * vpbe_master and slave frame buffer devices calls this to de-initialize
+ * the display controller. It is called when master and slave device
+ * driver modules are removed and no longer requires the display controller.
+ */
+static void vpbe_deinitialize(struct device *dev, struct vpbe_device *vpbe_dev)
+{
+        v4l2_device_unregister(&vpbe_dev->v4l2_dev);
+        if (strcmp(vpbe_dev->cfg->module_name, "dm644x-vpbe-display") != 0)
+                clk_put(vpbe_dev->dac_clk);
+
+        kfree(vpbe_dev->encoders);
+        vpbe_dev->initialized = 0;
+        /* disable vpss clocks */
+        vpss_enable_clock(VPSS_VPBE_CLOCK, 0);
+}
+
+static struct vpbe_device_ops vpbe_dev_ops = {
+        .g_cropcap = vpbe_g_cropcap,
+        .enum_outputs = vpbe_enum_outputs,
+        .set_output = vpbe_set_output,
+        .get_output = vpbe_get_output,
+        .s_dv_preset = vpbe_s_dv_preset,
+        .g_dv_preset = vpbe_g_dv_preset,
+        .enum_dv_presets = vpbe_enum_dv_presets,
+        .s_std = vpbe_s_std,
+        .g_std = vpbe_g_std,
+        .initialize = vpbe_initialize,
+        .deinitialize = vpbe_deinitialize,
+        .get_mode_info = vpbe_get_current_mode_info,
+        .set_mode = vpbe_set_mode,
+};
+
+static __devinit int vpbe_probe(struct platform_device *pdev)
+{
+        struct vpbe_device *vpbe_dev;
+        struct vpbe_config *cfg;
+        int ret = -EINVAL;
+
+        if (pdev->dev.platform_data == NULL) {
+                v4l2_err(pdev->dev.driver, "No platform data\n");
+                return -ENODEV;
+        }
+        cfg = pdev->dev.platform_data;
+
+        if (!cfg->module_name[0] ||
+            !cfg->osd.module_name[0] ||
+            !cfg->venc.module_name[0]) {
+                v4l2_err(pdev->dev.driver, "vpbe display module names not"
+                         " defined\n");
+                return ret;
+        }
+
+        vpbe_dev = kzalloc(sizeof(*vpbe_dev), GFP_KERNEL);
+        if (vpbe_dev == NULL) {
+                v4l2_err(pdev->dev.driver, "Unable to allocate memory"
+                         " for vpbe_device\n");
+                return -ENOMEM;
+        }
+        vpbe_dev->cfg = cfg;
+        vpbe_dev->ops = vpbe_dev_ops;
+        vpbe_dev->pdev = &pdev->dev;
+
+        if (cfg->outputs->num_modes > 0)
+                vpbe_dev->current_timings = vpbe_dev->cfg->outputs[0].modes[0];
+        else
+                return -ENODEV;
+
+        /* set the driver data in platform device */
+        platform_set_drvdata(pdev, vpbe_dev);
+        mutex_init(&vpbe_dev->lock);
+
+        return 0;
+}
+
+static int vpbe_remove(struct platform_device *device)
+{
+        struct vpbe_device *vpbe_dev = platform_get_drvdata(device);
+
+        kfree(vpbe_dev);
+
+        return 0;
+}
+
+static struct platform_driver vpbe_driver = {
+        .driver = {
+                .name   = "vpbe_controller",
+                .owner  = THIS_MODULE,
+        },
+        .probe = vpbe_probe,
+        .remove = vpbe_remove,
+};
+
+/**
+ * vpbe_init: initialize the vpbe driver
+ *
+ * This function registers device and driver to the kernel
+ */
+static __init int vpbe_init(void)
+{
+        return platform_driver_register(&vpbe_driver);
+}
+
+/**
+ * vpbe_cleanup : cleanup function for vpbe driver
+ *
+ * This will un-registers the device and driver to the kernel
+ */
+static void vpbe_cleanup(void)
+{
+        platform_driver_unregister(&vpbe_driver);
+}
+
+/* Function for module initialization and cleanup */
+module_init(vpbe_init);
+module_exit(vpbe_cleanup);
diff --git a/drivers/media/video/davinci/vpbe_display.c b/drivers/media/video/davinci/vpbe_display.c
new file mode 100644
index 00000000000..7f1d83a6d57
--- /dev/null
+++ b/drivers/media/video/davinci/vpbe_display.c
@@ -0,0 +1,1860 @@
+/*
+ * Copyright (C) 2010 Texas Instruments Incorporated - http://www.ti.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 version 2.
+ *
+ * This program is distributed 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/kernel.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/string.h>
+#include <linux/wait.h>
+#include <linux/time.h>
+#include <linux/platform_device.h>
+#include <linux/irq.h>
+#include <linux/mm.h>
+#include <linux/mutex.h>
+#include <linux/videodev2.h>
+#include <linux/slab.h>
+
+#include <asm/pgtable.h>
+#include <mach/cputype.h>
+
+#include <media/v4l2-dev.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-device.h>
+#include <media/davinci/vpbe_display.h>
+#include <media/davinci/vpbe_types.h>
+#include <media/davinci/vpbe.h>
+#include <media/davinci/vpbe_venc.h>
+#include <media/davinci/vpbe_osd.h>
+#include "vpbe_venc_regs.h"
+
+#define VPBE_DISPLAY_DRIVER "vpbe-v4l2"
+
+static int debug;
+
+#define VPBE_DISPLAY_SD_BUF_SIZE (720*576*2)
+#define VPBE_DEFAULT_NUM_BUFS 3
+
+module_param(debug, int, 0644);
+
+static int venc_is_second_field(struct vpbe_display *disp_dev)
+{
+        struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+        int ret;
+        int val;
+
+        ret = v4l2_subdev_call(vpbe_dev->venc,
+                               core,
+                               ioctl,
+                               VENC_GET_FLD,
+                               &val);
+        if (ret < 0) {
+                v4l2_err(&vpbe_dev->v4l2_dev,
+                         "Error in getting Field ID 0\n");
+        }
+        return val;
+}
+
+static void vpbe_isr_even_field(struct vpbe_display *disp_obj,
+                                struct vpbe_layer *layer)
+{
+        struct timespec timevalue;
+
+        if (layer->cur_frm == layer->next_frm)
+                return;
+        ktime_get_ts(&timevalue);
+        layer->cur_frm->ts.tv_sec = timevalue.tv_sec;
+        layer->cur_frm->ts.tv_usec = timevalue.tv_nsec / NSEC_PER_USEC;
+        layer->cur_frm->state = VIDEOBUF_DONE;
+        wake_up_interruptible(&layer->cur_frm->done);
+        /* Make cur_frm pointing to next_frm */
+        layer->cur_frm = layer->next_frm;
+}
+
+static void vpbe_isr_odd_field(struct vpbe_display *disp_obj,
+                                struct vpbe_layer *layer)
+{
+        struct osd_state *osd_device = disp_obj->osd_device;
+        unsigned long addr;
+
+        spin_lock(&disp_obj->dma_queue_lock);
+        if (list_empty(&layer->dma_queue) ||
+                (layer->cur_frm != layer->next_frm)) {
+                spin_unlock(&disp_obj->dma_queue_lock);
+                return;
+        }
+        /*
+         * one field is displayed configure
+         * the next frame if it is available
+         * otherwise hold on current frame
+         * Get next from the buffer queue
+         */
+        layer->next_frm = list_entry(
+                                layer->dma_queue.next,
+                                struct  videobuf_buffer,
+                                queue);
+        /* Remove that from the buffer queue */
+        list_del(&layer->next_frm->queue);
+        spin_unlock(&disp_obj->dma_queue_lock);
+        /* Mark state of the frame to active */
+        layer->next_frm->state = VIDEOBUF_ACTIVE;
+        addr = videobuf_to_dma_contig(layer->next_frm);
+        osd_device->ops.start_layer(osd_device,
+                        layer->layer_info.id,
+                        addr,
+                        disp_obj->cbcr_ofst);
+}
+
+/* interrupt service routine */
+static irqreturn_t venc_isr(int irq, void *arg)
+{
+        struct vpbe_display *disp_dev = (struct vpbe_display *)arg;
+        struct vpbe_layer *layer;
+        static unsigned last_event;
+        unsigned event = 0;
+        int fid;
+        int i;
+
+        if ((NULL == arg) || (NULL == disp_dev->dev[0]))
+                return IRQ_HANDLED;
+
+        if (venc_is_second_field(disp_dev))
+                event |= VENC_SECOND_FIELD;
+        else
+                event |= VENC_FIRST_FIELD;
+
+        if (event == (last_event & ~VENC_END_OF_FRAME)) {
+                /*
+                * If the display is non-interlaced, then we need to flag the
+                * end-of-frame event at every interrupt regardless of the
+                * value of the FIDST bit.  We can conclude that the display is
+                * non-interlaced if the value of the FIDST bit is unchanged
+                * from the previous interrupt.
+                */
+                event |= VENC_END_OF_FRAME;
+        } else if (event == VENC_SECOND_FIELD) {
+                /* end-of-frame for interlaced display */
+                event |= VENC_END_OF_FRAME;
+        }
+        last_event = event;
+
+        for (i = 0; i < VPBE_DISPLAY_MAX_DEVICES; i++) {
+                layer = disp_dev->dev[i];
+                /* If streaming is started in this layer */
+                if (!layer->started)
+                        continue;
+
+                if (layer->layer_first_int) {
+                        layer->layer_first_int = 0;
+                        continue;
+                }
+                /* Check the field format */
+                if ((V4L2_FIELD_NONE == layer->pix_fmt.field) &&
+                        (event & VENC_END_OF_FRAME)) {
+                        /* Progressive mode */
+
+                        vpbe_isr_even_field(disp_dev, layer);
+                        vpbe_isr_odd_field(disp_dev, layer);
+                } else {
+                /* Interlaced mode */
+
+                        layer->field_id ^= 1;
+                        if (event & VENC_FIRST_FIELD)
+                                fid = 0;
+                        else
+                                fid = 1;
+
+                        /*
+                        * If field id does not match with store
+                        * field id
+                        */
+                        if (fid != layer->field_id) {
+                                /* Make them in sync */
+                                layer->field_id = fid;
+                                continue;
+                        }
+                        /*
+                        * device field id and local field id are
+                        * in sync. If this is even field
+                        */
+                        if (0 == fid)
+                                vpbe_isr_even_field(disp_dev, layer);
+                        else  /* odd field */
+                                vpbe_isr_odd_field(disp_dev, layer);
+                }
+        }
+
+        return IRQ_HANDLED;
+}
+
+/*
+ * vpbe_buffer_prepare()
+ * This is the callback function called from videobuf_qbuf() function
+ * the buffer is prepared and user space virtual address is converted into
+ * physical address
+ */
+static int vpbe_buffer_prepare(struct videobuf_queue *q,
+                                  struct videobuf_buffer *vb,
+                                  enum v4l2_field field)
+{
+        struct vpbe_fh *fh = q->priv_data;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+        unsigned long addr;
+        int ret;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+                                "vpbe_buffer_prepare\n");
+
+        /* If buffer is not initialized, initialize it */
+        if (VIDEOBUF_NEEDS_INIT == vb->state) {
+                vb->width = layer->pix_fmt.width;
+                vb->height = layer->pix_fmt.height;
+                vb->size = layer->pix_fmt.sizeimage;
+                vb->field = field;
+
+                ret = videobuf_iolock(q, vb, NULL);
+                if (ret < 0) {
+                        v4l2_err(&vpbe_dev->v4l2_dev, "Failed to map \
+                                user address\n");
+                        return -EINVAL;
+                }
+
+                addr = videobuf_to_dma_contig(vb);
+
+                if (q->streaming) {
+                        if (!IS_ALIGNED(addr, 8)) {
+                                v4l2_err(&vpbe_dev->v4l2_dev,
+                                        "buffer_prepare:offset is \
+                                        not aligned to 32 bytes\n");
+                                return -EINVAL;
+                        }
+                }
+                vb->state = VIDEOBUF_PREPARED;
+        }
+        return 0;
+}
+
+/*
+ * vpbe_buffer_setup()
+ * This function allocates memory for the buffers
+ */
+static int vpbe_buffer_setup(struct videobuf_queue *q,
+                                unsigned int *count,
+                                unsigned int *size)
+{
+        /* Get the file handle object and layer object */
+        struct vpbe_fh *fh = q->priv_data;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "vpbe_buffer_setup\n");
+
+        *size = layer->pix_fmt.sizeimage;
+
+        /* Store number of buffers allocated in numbuffer member */
+        if (*count < VPBE_DEFAULT_NUM_BUFS)
+                *count = layer->numbuffers = VPBE_DEFAULT_NUM_BUFS;
+
+        return 0;
+}
+
+/*
+ * vpbe_buffer_queue()
+ * This function adds the buffer to DMA queue
+ */
+static void vpbe_buffer_queue(struct videobuf_queue *q,
+                                 struct videobuf_buffer *vb)
+{
+        /* Get the file handle object and layer object */
+        struct vpbe_fh *fh = q->priv_data;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_display *disp = fh->disp_dev;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+        unsigned long flags;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+                        "vpbe_buffer_queue\n");
+
+        /* add the buffer to the DMA queue */
+        spin_lock_irqsave(&disp->dma_queue_lock, flags);
+        list_add_tail(&vb->queue, &layer->dma_queue);
+        spin_unlock_irqrestore(&disp->dma_queue_lock, flags);
+        /* Change state of the buffer */
+        vb->state = VIDEOBUF_QUEUED;
+}
+
+/*
+ * vpbe_buffer_release()
+ * This function is called from the videobuf layer to free memory allocated to
+ * the buffers
+ */
+static void vpbe_buffer_release(struct videobuf_queue *q,
+                                   struct videobuf_buffer *vb)
+{
+        /* Get the file handle object and layer object */
+        struct vpbe_fh *fh = q->priv_data;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+                        "vpbe_buffer_release\n");
+
+        if (V4L2_MEMORY_USERPTR != layer->memory)
+                videobuf_dma_contig_free(q, vb);
+
+        vb->state = VIDEOBUF_NEEDS_INIT;
+}
+
+static struct videobuf_queue_ops video_qops = {
+        .buf_setup = vpbe_buffer_setup,
+        .buf_prepare = vpbe_buffer_prepare,
+        .buf_queue = vpbe_buffer_queue,
+        .buf_release = vpbe_buffer_release,
+};
+
+static
+struct vpbe_layer*
+_vpbe_display_get_other_win_layer(struct vpbe_display *disp_dev,
+                        struct vpbe_layer *layer)
+{
+        enum vpbe_display_device_id thiswin, otherwin;
+        thiswin = layer->device_id;
+
+        otherwin = (thiswin == VPBE_DISPLAY_DEVICE_0) ?
+        VPBE_DISPLAY_DEVICE_1 : VPBE_DISPLAY_DEVICE_0;
+        return disp_dev->dev[otherwin];
+}
+
+static int vpbe_set_osd_display_params(struct vpbe_display *disp_dev,
+                        struct vpbe_layer *layer)
+{
+        struct osd_layer_config *cfg  = &layer->layer_info.config;
+        struct osd_state *osd_device = disp_dev->osd_device;
+        struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+        unsigned long addr;
+        int ret;
+
+        addr = videobuf_to_dma_contig(layer->cur_frm);
+        /* Set address in the display registers */
+        osd_device->ops.start_layer(osd_device,
+                                    layer->layer_info.id,
+                                    addr,
+                                    disp_dev->cbcr_ofst);
+
+        ret = osd_device->ops.enable_layer(osd_device,
+                                layer->layer_info.id, 0);
+        if (ret < 0) {
+                v4l2_err(&vpbe_dev->v4l2_dev,
+                        "Error in enabling osd window layer 0\n");
+                return -1;
+        }
+
+        /* Enable the window */
+        layer->layer_info.enable = 1;
+        if (cfg->pixfmt == PIXFMT_NV12) {
+                struct vpbe_layer *otherlayer =
+                        _vpbe_display_get_other_win_layer(disp_dev, layer);
+
+                ret = osd_device->ops.enable_layer(osd_device,
+                                otherlayer->layer_info.id, 1);
+                if (ret < 0) {
+                        v4l2_err(&vpbe_dev->v4l2_dev,
+                                "Error in enabling osd window layer 1\n");
+                        return -1;
+                }
+                otherlayer->layer_info.enable = 1;
+        }
+        return 0;
+}
+
+static void
+vpbe_disp_calculate_scale_factor(struct vpbe_display *disp_dev,
+                        struct vpbe_layer *layer,
+                        int expected_xsize, int expected_ysize)
+{
+        struct display_layer_info *layer_info = &layer->layer_info;
+        struct v4l2_pix_format *pixfmt = &layer->pix_fmt;
+        struct osd_layer_config *cfg  = &layer->layer_info.config;
+        struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+        int calculated_xsize;
+        int h_exp = 0;
+        int v_exp = 0;
+        int h_scale;
+        int v_scale;
+
+        v4l2_std_id standard_id = vpbe_dev->current_timings.timings.std_id;
+
+        /*
+         * Application initially set the image format. Current display
+         * size is obtained from the vpbe display controller. expected_xsize
+         * and expected_ysize are set through S_CROP ioctl. Based on this,
+         * driver will calculate the scale factors for vertical and
+         * horizontal direction so that the image is displayed scaled
+         * and expanded. Application uses expansion to display the image
+         * in a square pixel. Otherwise it is displayed using displays
+         * pixel aspect ratio.It is expected that application chooses
+         * the crop coordinates for cropped or scaled display. if crop
+         * size is less than the image size, it is displayed cropped or
+         * it is displayed scaled and/or expanded.
+         *
+         * to begin with, set the crop window same as expected. Later we
+         * will override with scaled window size
+         */
+
+        cfg->xsize = pixfmt->width;
+        cfg->ysize = pixfmt->height;
+        layer_info->h_zoom = ZOOM_X1;   /* no horizontal zoom */
+        layer_info->v_zoom = ZOOM_X1;   /* no horizontal zoom */
+        layer_info->h_exp = H_EXP_OFF;  /* no horizontal zoom */
+        layer_info->v_exp = V_EXP_OFF;  /* no horizontal zoom */
+
+        if (pixfmt->width < expected_xsize) {
+                h_scale = vpbe_dev->current_timings.xres / pixfmt->width;
+                if (h_scale < 2)
+                        h_scale = 1;
+                else if (h_scale >= 4)
+                        h_scale = 4;
+                else
+                        h_scale = 2;
+                cfg->xsize *= h_scale;
+                if (cfg->xsize < expected_xsize) {
+                        if ((standard_id & V4L2_STD_525_60) ||
+                        (standard_id & V4L2_STD_625_50)) {
+                                calculated_xsize = (cfg->xsize *
+                                        VPBE_DISPLAY_H_EXP_RATIO_N) /
+                                        VPBE_DISPLAY_H_EXP_RATIO_D;
+                                if (calculated_xsize <= expected_xsize) {
+                                        h_exp = 1;
+                                        cfg->xsize = calculated_xsize;
+                                }
+                        }
+                }
+                if (h_scale == 2)
+                        layer_info->h_zoom = ZOOM_X2;
+                else if (h_scale == 4)
+                        layer_info->h_zoom = ZOOM_X4;
+                if (h_exp)
+                        layer_info->h_exp = H_EXP_9_OVER_8;
+        } else {
+                /* no scaling, only cropping. Set display area to crop area */
+                cfg->xsize = expected_xsize;
+        }
+
+        if (pixfmt->height < expected_ysize) {
+                v_scale = expected_ysize / pixfmt->height;
+                if (v_scale < 2)
+                        v_scale = 1;
+                else if (v_scale >= 4)
+                        v_scale = 4;
+                else
+                        v_scale = 2;
+                cfg->ysize *= v_scale;
+                if (cfg->ysize < expected_ysize) {
+                        if ((standard_id & V4L2_STD_625_50)) {
+                                calculated_xsize = (cfg->ysize *
+                                        VPBE_DISPLAY_V_EXP_RATIO_N) /
+                                        VPBE_DISPLAY_V_EXP_RATIO_D;
+                                if (calculated_xsize <= expected_ysize) {
+                                        v_exp = 1;
+                                        cfg->ysize = calculated_xsize;
+                                }
+                        }
+                }
+                if (v_scale == 2)
+                        layer_info->v_zoom = ZOOM_X2;
+                else if (v_scale == 4)
+                        layer_info->v_zoom = ZOOM_X4;
+                if (v_exp)
+                        layer_info->h_exp = V_EXP_6_OVER_5;
+        } else {
+                /* no scaling, only cropping. Set display area to crop area */
+                cfg->ysize = expected_ysize;
+        }
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+                "crop display xsize = %d, ysize = %d\n",
+                cfg->xsize, cfg->ysize);
+}
+
+static void vpbe_disp_adj_position(struct vpbe_display *disp_dev,
+                        struct vpbe_layer *layer,
+                        int top, int left)
+{
+        struct osd_layer_config *cfg = &layer->layer_info.config;
+        struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+
+        cfg->xpos = min((unsigned int)left,
+                        vpbe_dev->current_timings.xres - cfg->xsize);
+        cfg->ypos = min((unsigned int)top,
+                        vpbe_dev->current_timings.yres - cfg->ysize);
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+                "new xpos = %d, ypos = %d\n",
+                cfg->xpos, cfg->ypos);
+}
+
+static void vpbe_disp_check_window_params(struct vpbe_display *disp_dev,
+                        struct v4l2_rect *c)
+{
+        struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+
+        if ((c->width == 0) ||
+          ((c->width + c->left) > vpbe_dev->current_timings.xres))
+                c->width = vpbe_dev->current_timings.xres - c->left;
+
+        if ((c->height == 0) || ((c->height + c->top) >
+          vpbe_dev->current_timings.yres))
+                c->height = vpbe_dev->current_timings.yres - c->top;
+
+        /* window height must be even for interlaced display */
+        if (vpbe_dev->current_timings.interlaced)
+                c->height &= (~0x01);
+
+}
+
+/**
+ * vpbe_try_format()
+ * If user application provides width and height, and have bytesperline set
+ * to zero, driver calculates bytesperline and sizeimage based on hardware
+ * limits.
+ */
+static int vpbe_try_format(struct vpbe_display *disp_dev,
+                        struct v4l2_pix_format *pixfmt, int check)
+{
+        struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+        int min_height = 1;
+        int min_width = 32;
+        int max_height;
+        int max_width;
+        int bpp;
+
+        if ((pixfmt->pixelformat != V4L2_PIX_FMT_UYVY) &&
+            (pixfmt->pixelformat != V4L2_PIX_FMT_NV12))
+                /* choose default as V4L2_PIX_FMT_UYVY */
+                pixfmt->pixelformat = V4L2_PIX_FMT_UYVY;
+
+        /* Check the field format */
+        if ((pixfmt->field != V4L2_FIELD_INTERLACED) &&
+                (pixfmt->field != V4L2_FIELD_NONE)) {
+                if (vpbe_dev->current_timings.interlaced)
+                        pixfmt->field = V4L2_FIELD_INTERLACED;
+                else
+                        pixfmt->field = V4L2_FIELD_NONE;
+        }
+
+        if (pixfmt->field == V4L2_FIELD_INTERLACED)
+                min_height = 2;
+
+        if (pixfmt->pixelformat == V4L2_PIX_FMT_NV12)
+                bpp = 1;
+        else
+                bpp = 2;
+
+        max_width = vpbe_dev->current_timings.xres;
+        max_height = vpbe_dev->current_timings.yres;
+
+        min_width /= bpp;
+
+        if (!pixfmt->width || (pixfmt->width < min_width) ||
+                (pixfmt->width > max_width)) {
+                pixfmt->width = vpbe_dev->current_timings.xres;
+        }
+
+        if (!pixfmt->height || (pixfmt->height  < min_height) ||
+                (pixfmt->height  > max_height)) {
+                pixfmt->height = vpbe_dev->current_timings.yres;
+        }
+
+        if (pixfmt->bytesperline < (pixfmt->width * bpp))
+                pixfmt->bytesperline = pixfmt->width * bpp;
+
+        /* Make the bytesperline 32 byte aligned */
+        pixfmt->bytesperline = ((pixfmt->width * bpp + 31) & ~31);
+
+        if (pixfmt->pixelformat == V4L2_PIX_FMT_NV12)
+                pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height +
+                                (pixfmt->bytesperline * pixfmt->height >> 1);
+        else
+                pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
+
+        return 0;
+}
+
+static int vpbe_display_g_priority(struct file *file, void *priv,
+                                enum v4l2_priority *p)
+{
+        struct vpbe_fh *fh = file->private_data;
+        struct vpbe_layer *layer = fh->layer;
+
+        *p = v4l2_prio_max(&layer->prio);
+
+        return 0;
+}
+
+static int vpbe_display_s_priority(struct file *file, void *priv,
+                                enum v4l2_priority p)
+{
+        struct vpbe_fh *fh = file->private_data;
+        struct vpbe_layer *layer = fh->layer;
+        int ret;
+
+        ret = v4l2_prio_change(&layer->prio, &fh->prio, p);
+
+        return ret;
+}
+
+static int vpbe_display_querycap(struct file *file, void  *priv,
+                               struct v4l2_capability *cap)
+{
+        struct vpbe_fh *fh = file->private_data;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+
+        cap->version = VPBE_DISPLAY_VERSION_CODE;
+        cap->capabilities = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
+        strlcpy(cap->driver, VPBE_DISPLAY_DRIVER, sizeof(cap->driver));
+        strlcpy(cap->bus_info, "platform", sizeof(cap->bus_info));
+        strlcpy(cap->card, vpbe_dev->cfg->module_name, sizeof(cap->card));
+
+        return 0;
+}
+
+static int vpbe_display_s_crop(struct file *file, void *priv,
+                             struct v4l2_crop *crop)
+{
+        struct vpbe_fh *fh = file->private_data;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_display *disp_dev = fh->disp_dev;
+        struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+        struct osd_layer_config *cfg = &layer->layer_info.config;
+        struct osd_state *osd_device = disp_dev->osd_device;
+        struct v4l2_rect *rect = &crop->c;
+        int ret;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+                "VIDIOC_S_CROP, layer id = %d\n", layer->device_id);
+
+        if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "Invalid buf type\n");
+                return -EINVAL;
+        }
+
+        if (rect->top < 0)
+                rect->top = 0;
+        if (rect->left < 0)
+                rect->left = 0;
+
+        vpbe_disp_check_window_params(disp_dev, rect);
+
+        osd_device->ops.get_layer_config(osd_device,
+                        layer->layer_info.id, cfg);
+
+        vpbe_disp_calculate_scale_factor(disp_dev, layer,
+                                        rect->width,
+                                        rect->height);
+        vpbe_disp_adj_position(disp_dev, layer, rect->top,
+                                        rect->left);
+        ret = osd_device->ops.set_layer_config(osd_device,
+                                layer->layer_info.id, cfg);
+        if (ret < 0) {
+                v4l2_err(&vpbe_dev->v4l2_dev,
+                        "Error in set layer config:\n");
+                return -EINVAL;
+        }
+
+        /* apply zooming and h or v expansion */
+        osd_device->ops.set_zoom(osd_device,
+                        layer->layer_info.id,
+                        layer->layer_info.h_zoom,
+                        layer->layer_info.v_zoom);
+        ret = osd_device->ops.set_vid_expansion(osd_device,
+                        layer->layer_info.h_exp,
+                        layer->layer_info.v_exp);
+        if (ret < 0) {
+                v4l2_err(&vpbe_dev->v4l2_dev,
+                "Error in set vid expansion:\n");
+                return -EINVAL;
+        }
+
+        if ((layer->layer_info.h_zoom != ZOOM_X1) ||
+                (layer->layer_info.v_zoom != ZOOM_X1) ||
+                (layer->layer_info.h_exp != H_EXP_OFF) ||
+                (layer->layer_info.v_exp != V_EXP_OFF))
+                /* Enable expansion filter */
+                osd_device->ops.set_interpolation_filter(osd_device, 1);
+        else
+                osd_device->ops.set_interpolation_filter(osd_device, 0);
+
+        return 0;
+}
+
+static int vpbe_display_g_crop(struct file *file, void *priv,
+                             struct v4l2_crop *crop)
+{
+        struct vpbe_fh *fh = file->private_data;
+        struct vpbe_layer *layer = fh->layer;
+        struct osd_layer_config *cfg = &layer->layer_info.config;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+        struct osd_state *osd_device = fh->disp_dev->osd_device;
+        struct v4l2_rect *rect = &crop->c;
+        int ret;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+                        "VIDIOC_G_CROP, layer id = %d\n",
+                        layer->device_id);
+
+        if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "Invalid buf type\n");
+                ret = -EINVAL;
+        }
+        osd_device->ops.get_layer_config(osd_device,
+                                layer->layer_info.id, cfg);
+        rect->top = cfg->ypos;
+        rect->left = cfg->xpos;
+        rect->width = cfg->xsize;
+        rect->height = cfg->ysize;
+
+        return 0;
+}
+
+static int vpbe_display_cropcap(struct file *file, void *priv,
+                              struct v4l2_cropcap *cropcap)
+{
+        struct vpbe_fh *fh = file->private_data;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_CROPCAP ioctl\n");
+
+        cropcap->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+        cropcap->bounds.left = 0;
+        cropcap->bounds.top = 0;
+        cropcap->bounds.width = vpbe_dev->current_timings.xres;
+        cropcap->bounds.height = vpbe_dev->current_timings.yres;
+        cropcap->pixelaspect = vpbe_dev->current_timings.aspect;
+        cropcap->defrect = cropcap->bounds;
+        return 0;
+}
+
+static int vpbe_display_g_fmt(struct file *file, void *priv,
+                                struct v4l2_format *fmt)
+{
+        struct vpbe_fh *fh = file->private_data;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+                        "VIDIOC_G_FMT, layer id = %d\n",
+                        layer->device_id);
+
+        /* If buffer type is video output */
+        if (V4L2_BUF_TYPE_VIDEO_OUTPUT != fmt->type) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "invalid type\n");
+                return -EINVAL;
+        }
+        /* Fill in the information about format */
+        fmt->fmt.pix = layer->pix_fmt;
+
+        return 0;
+}
+
+static int vpbe_display_enum_fmt(struct file *file, void  *priv,
+                                   struct v4l2_fmtdesc *fmt)
+{
+        struct vpbe_fh *fh = file->private_data;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+        unsigned int index = 0;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+                                "VIDIOC_ENUM_FMT, layer id = %d\n",
+                                layer->device_id);
+        if (fmt->index > 1) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "Invalid format index\n");
+                return -EINVAL;
+        }
+
+        /* Fill in the information about format */
+        index = fmt->index;
+        memset(fmt, 0, sizeof(*fmt));
+        fmt->index = index;
+        fmt->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+        if (index == 0) {
+                strcpy(fmt->description, "YUV 4:2:2 - UYVY");
+                fmt->pixelformat = V4L2_PIX_FMT_UYVY;
+        } else {
+                strcpy(fmt->description, "Y/CbCr 4:2:0");
+                fmt->pixelformat = V4L2_PIX_FMT_NV12;
+        }
+
+        return 0;
+}
+
+static int vpbe_display_s_fmt(struct file *file, void *priv,
+                                struct v4l2_format *fmt)
+{
+        struct vpbe_fh *fh = file->private_data;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_display *disp_dev = fh->disp_dev;
+        struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+        struct osd_layer_config *cfg  = &layer->layer_info.config;
+        struct v4l2_pix_format *pixfmt = &fmt->fmt.pix;
+        struct osd_state *osd_device = disp_dev->osd_device;
+        int ret;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+                        "VIDIOC_S_FMT, layer id = %d\n",
+                        layer->device_id);
+
+        /* If streaming is started, return error */
+        if (layer->started) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "Streaming is started\n");
+                return -EBUSY;
+        }
+        if (V4L2_BUF_TYPE_VIDEO_OUTPUT != fmt->type) {
+                v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "invalid type\n");
+                return -EINVAL;
+        }
+        /* Check for valid pixel format */
+        ret = vpbe_try_format(disp_dev, pixfmt, 1);
+        if (ret)
+                return ret;
+
+        /* YUV420 is requested, check availability of the
+        other video window */
+
+        layer->pix_fmt = *pixfmt;
+
+        /* Get osd layer config */
+        osd_device->ops.get_layer_config(osd_device,
+                        layer->layer_info.id, cfg);
+        /* Store the pixel format in the layer object */
+        cfg->xsize = pixfmt->width;
+        cfg->ysize = pixfmt->height;
+        cfg->line_length = pixfmt->bytesperline;
+        cfg->ypos = 0;
+        cfg->xpos = 0;
+        cfg->interlaced = vpbe_dev->current_timings.interlaced;
+
+        if (V4L2_PIX_FMT_UYVY == pixfmt->pixelformat)
+                cfg->pixfmt = PIXFMT_YCbCrI;
+
+        /* Change of the default pixel format for both video windows */
+        if (V4L2_PIX_FMT_NV12 == pixfmt->pixelformat) {
+                struct vpbe_layer *otherlayer;
+                cfg->pixfmt = PIXFMT_NV12;
+                otherlayer = _vpbe_display_get_other_win_layer(disp_dev,
+                                                                layer);
+                otherlayer->layer_info.config.pixfmt = PIXFMT_NV12;
+        }
+
+        /* Set the layer config in the osd window */
+        ret = osd_device->ops.set_layer_config(osd_device,
+                                layer->layer_info.id, cfg);
+        if (ret < 0) {
+                v4l2_err(&vpbe_dev->v4l2_dev,
+                                "Error in S_FMT params:\n");
+                return -EINVAL;
+        }
+
+        /* Readback and fill the local copy of current pix format */
+        osd_device->ops.get_layer_config(osd_device,
+                        layer->layer_info.id, cfg);
+
+        return 0;
+}
+
+static int vpbe_display_try_fmt(struct file *file, void *priv,
+                                  struct v4l2_format *fmt)
+{
+        struct vpbe_fh *fh = file->private_data;
+        struct vpbe_display *disp_dev = fh->disp_dev;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+        struct v4l2_pix_format *pixfmt = &fmt->fmt.pix;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_TRY_FMT\n");
+
+        if (V4L2_BUF_TYPE_VIDEO_OUTPUT != fmt->type) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "invalid type\n");
+                return -EINVAL;
+        }
+
+        /* Check for valid field format */
+        return  vpbe_try_format(disp_dev, pixfmt, 0);
+
+}
+
+/**
+ * vpbe_display_s_std - Set the given standard in the encoder
+ *
+ * Sets the standard if supported by the current encoder. Return the status.
+ * 0 - success & -EINVAL on error
+ */
+static int vpbe_display_s_std(struct file *file, void *priv,
+                                v4l2_std_id *std_id)
+{
+        struct vpbe_fh *fh = priv;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+        int ret;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_S_STD\n");
+
+        /* If streaming is started, return error */
+        if (layer->started) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "Streaming is started\n");
+                return -EBUSY;
+        }
+        if (NULL != vpbe_dev->ops.s_std) {
+                ret = vpbe_dev->ops.s_std(vpbe_dev, std_id);
+                if (ret) {
+                        v4l2_err(&vpbe_dev->v4l2_dev,
+                        "Failed to set standard for sub devices\n");
+                        return -EINVAL;
+                }
+        } else {
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+/**
+ * vpbe_display_g_std - Get the standard in the current encoder
+ *
+ * Get the standard in the current encoder. Return the status. 0 - success
+ * -EINVAL on error
+ */
+static int vpbe_display_g_std(struct file *file, void *priv,
+                                v4l2_std_id *std_id)
+{
+        struct vpbe_fh *fh = priv;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_G_STD\n");
+
+        /* Get the standard from the current encoder */
+        if (vpbe_dev->current_timings.timings_type & VPBE_ENC_STD) {
+                *std_id = vpbe_dev->current_timings.timings.std_id;
+                return 0;
+        }
+
+        return -EINVAL;
+}
+
+/**
+ * vpbe_display_enum_output - enumerate outputs
+ *
+ * Enumerates the outputs available at the vpbe display
+ * returns the status, -EINVAL if end of output list
+ */
+static int vpbe_display_enum_output(struct file *file, void *priv,
+                                    struct v4l2_output *output)
+{
+        struct vpbe_fh *fh = priv;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+        int ret;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_ENUM_OUTPUT\n");
+
+        /* Enumerate outputs */
+
+        if (NULL == vpbe_dev->ops.enum_outputs)
+                return -EINVAL;
+
+        ret = vpbe_dev->ops.enum_outputs(vpbe_dev, output);
+        if (ret) {
+                v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+                        "Failed to enumerate outputs\n");
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+/**
+ * vpbe_display_s_output - Set output to
+ * the output specified by the index
+ */
+static int vpbe_display_s_output(struct file *file, void *priv,
+                                unsigned int i)
+{
+        struct vpbe_fh *fh = priv;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+        int ret;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_S_OUTPUT\n");
+        /* If streaming is started, return error */
+        if (layer->started) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "Streaming is started\n");
+                return -EBUSY;
+        }
+        if (NULL == vpbe_dev->ops.set_output)
+                return -EINVAL;
+
+        ret = vpbe_dev->ops.set_output(vpbe_dev, i);
+        if (ret) {
+                v4l2_err(&vpbe_dev->v4l2_dev,
+                        "Failed to set output for sub devices\n");
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+/**
+ * vpbe_display_g_output - Get output from subdevice
+ * for a given by the index
+ */
+static int vpbe_display_g_output(struct file *file, void *priv,
+                                unsigned int *i)
+{
+        struct vpbe_fh *fh = priv;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_G_OUTPUT\n");
+        /* Get the standard from the current encoder */
+        *i = vpbe_dev->current_out_index;
+
+        return 0;
+}
+
+/**
+ * vpbe_display_enum_dv_presets - Enumerate the dv presets
+ *
+ * enum the preset in the current encoder. Return the status. 0 - success
+ * -EINVAL on error
+ */
+static int
+vpbe_display_enum_dv_presets(struct file *file, void *priv,
+                        struct v4l2_dv_enum_preset *preset)
+{
+        struct vpbe_fh *fh = priv;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+        int ret;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_ENUM_DV_PRESETS\n");
+
+        /* Enumerate outputs */
+        if (NULL == vpbe_dev->ops.enum_dv_presets)
+                return -EINVAL;
+
+        ret = vpbe_dev->ops.enum_dv_presets(vpbe_dev, preset);
+        if (ret) {
+                v4l2_err(&vpbe_dev->v4l2_dev,
+                        "Failed to enumerate dv presets info\n");
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+/**
+ * vpbe_display_s_dv_preset - Set the dv presets
+ *
+ * Set the preset in the current encoder. Return the status. 0 - success
+ * -EINVAL on error
+ */
+static int
+vpbe_display_s_dv_preset(struct file *file, void *priv,
+                                struct v4l2_dv_preset *preset)
+{
+        struct vpbe_fh *fh = priv;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+        int ret;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_S_DV_PRESETS\n");
+
+
+        /* If streaming is started, return error */
+        if (layer->started) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "Streaming is started\n");
+                return -EBUSY;
+        }
+
+        /* Set the given standard in the encoder */
+        if (NULL != vpbe_dev->ops.s_dv_preset)
+                return -EINVAL;
+
+        ret = vpbe_dev->ops.s_dv_preset(vpbe_dev, preset);
+        if (ret) {
+                v4l2_err(&vpbe_dev->v4l2_dev,
+                        "Failed to set the dv presets info\n");
+                return -EINVAL;
+        }
+        /* set the current norm to zero to be consistent. If STD is used
+         * v4l2 layer will set the norm properly on successful s_std call
+         */
+        layer->video_dev.current_norm = 0;
+
+        return 0;
+}
+
+/**
+ * vpbe_display_g_dv_preset - Set the dv presets
+ *
+ * Get the preset in the current encoder. Return the status. 0 - success
+ * -EINVAL on error
+ */
+static int
+vpbe_display_g_dv_preset(struct file *file, void *priv,
+                                struct v4l2_dv_preset *dv_preset)
+{
+        struct vpbe_fh *fh = priv;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_G_DV_PRESETS\n");
+
+        /* Get the given standard in the encoder */
+
+        if (vpbe_dev->current_timings.timings_type &
+                                VPBE_ENC_DV_PRESET) {
+                dv_preset->preset =
+                        vpbe_dev->current_timings.timings.dv_preset;
+        } else {
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+static int vpbe_display_streamoff(struct file *file, void *priv,
+                                enum v4l2_buf_type buf_type)
+{
+        struct vpbe_fh *fh = file->private_data;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+        struct osd_state *osd_device = fh->disp_dev->osd_device;
+        int ret;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+                        "VIDIOC_STREAMOFF,layer id = %d\n",
+                        layer->device_id);
+
+        if (V4L2_BUF_TYPE_VIDEO_OUTPUT != buf_type) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "Invalid buffer type\n");
+                return -EINVAL;
+        }
+
+        /* If io is allowed for this file handle, return error */
+        if (!fh->io_allowed) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "No io_allowed\n");
+                return -EACCES;
+        }
+
+        /* If streaming is not started, return error */
+        if (!layer->started) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "streaming not started in layer"
+                        " id = %d\n", layer->device_id);
+                return -EINVAL;
+        }
+
+        osd_device->ops.disable_layer(osd_device,
+                        layer->layer_info.id);
+        layer->started = 0;
+        ret = videobuf_streamoff(&layer->buffer_queue);
+
+        return ret;
+}
+
+static int vpbe_display_streamon(struct file *file, void *priv,
+                         enum v4l2_buf_type buf_type)
+{
+        struct vpbe_fh *fh = file->private_data;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_display *disp_dev = fh->disp_dev;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+        struct osd_state *osd_device = disp_dev->osd_device;
+        int ret;
+
+        osd_device->ops.disable_layer(osd_device,
+                        layer->layer_info.id);
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_STREAMON, layerid=%d\n",
+                                                layer->device_id);
+
+        if (V4L2_BUF_TYPE_VIDEO_OUTPUT != buf_type) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "Invalid buffer type\n");
+                return -EINVAL;
+        }
+
+        /* If file handle is not allowed IO, return error */
+        if (!fh->io_allowed) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "No io_allowed\n");
+                return -EACCES;
+        }
+        /* If Streaming is already started, return error */
+        if (layer->started) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "layer is already streaming\n");
+                return -EBUSY;
+        }
+
+        /*
+         * Call videobuf_streamon to start streaming
+         * in videobuf
+         */
+        ret = videobuf_streamon(&layer->buffer_queue);
+        if (ret) {
+                v4l2_err(&vpbe_dev->v4l2_dev,
+                "error in videobuf_streamon\n");
+                return ret;
+        }
+        /* If buffer queue is empty, return error */
+        if (list_empty(&layer->dma_queue)) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "buffer queue is empty\n");
+                goto streamoff;
+        }
+        /* Get the next frame from the buffer queue */
+        layer->next_frm = layer->cur_frm = list_entry(layer->dma_queue.next,
+                                struct videobuf_buffer, queue);
+        /* Remove buffer from the buffer queue */
+        list_del(&layer->cur_frm->queue);
+        /* Mark state of the current frame to active */
+        layer->cur_frm->state = VIDEOBUF_ACTIVE;
+        /* Initialize field_id and started member */
+        layer->field_id = 0;
+
+        /* Set parameters in OSD and VENC */
+        ret = vpbe_set_osd_display_params(disp_dev, layer);
+        if (ret < 0)
+                goto streamoff;
+
+        /*
+         * if request format is yuv420 semiplanar, need to
+         * enable both video windows
+         */
+        layer->started = 1;
+
+        layer->layer_first_int = 1;
+
+        return ret;
+streamoff:
+        ret = videobuf_streamoff(&layer->buffer_queue);
+        return ret;
+}
+
+static int vpbe_display_dqbuf(struct file *file, void *priv,
+                      struct v4l2_buffer *buf)
+{
+        struct vpbe_fh *fh = file->private_data;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+        int ret;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+                "VIDIOC_DQBUF, layer id = %d\n",
+                layer->device_id);
+
+        if (V4L2_BUF_TYPE_VIDEO_OUTPUT != buf->type) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "Invalid buffer type\n");
+                return -EINVAL;
+        }
+        /* If this file handle is not allowed to do IO, return error */
+        if (!fh->io_allowed) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "No io_allowed\n");
+                return -EACCES;
+        }
+        if (file->f_flags & O_NONBLOCK)
+                /* Call videobuf_dqbuf for non blocking mode */
+                ret = videobuf_dqbuf(&layer->buffer_queue, buf, 1);
+        else
+                /* Call videobuf_dqbuf for blocking mode */
+                ret = videobuf_dqbuf(&layer->buffer_queue, buf, 0);
+
+        return ret;
+}
+
+static int vpbe_display_qbuf(struct file *file, void *priv,
+                     struct v4l2_buffer *p)
+{
+        struct vpbe_fh *fh = file->private_data;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+                "VIDIOC_QBUF, layer id = %d\n",
+                layer->device_id);
+
+        if (V4L2_BUF_TYPE_VIDEO_OUTPUT != p->type) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "Invalid buffer type\n");
+                return -EINVAL;
+        }
+
+        /* If this file handle is not allowed to do IO, return error */
+        if (!fh->io_allowed) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "No io_allowed\n");
+                return -EACCES;
+        }
+
+        return videobuf_qbuf(&layer->buffer_queue, p);
+}
+
+static int vpbe_display_querybuf(struct file *file, void *priv,
+                         struct v4l2_buffer *buf)
+{
+        struct vpbe_fh *fh = file->private_data;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+        int ret;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+                "VIDIOC_QUERYBUF, layer id = %d\n",
+                layer->device_id);
+
+        if (V4L2_BUF_TYPE_VIDEO_OUTPUT != buf->type) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "Invalid buffer type\n");
+                return -EINVAL;
+        }
+
+        /* Call videobuf_querybuf to get information */
+        ret = videobuf_querybuf(&layer->buffer_queue, buf);
+
+        return ret;
+}
+
+static int vpbe_display_reqbufs(struct file *file, void *priv,
+                        struct v4l2_requestbuffers *req_buf)
+{
+        struct vpbe_fh *fh = file->private_data;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+        int ret;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "vpbe_display_reqbufs\n");
+
+        if (V4L2_BUF_TYPE_VIDEO_OUTPUT != req_buf->type) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "Invalid buffer type\n");
+                return -EINVAL;
+        }
+
+        /* If io users of the layer is not zero, return error */
+        if (0 != layer->io_usrs) {
+                v4l2_err(&vpbe_dev->v4l2_dev, "not IO user\n");
+                return -EBUSY;
+        }
+        /* Initialize videobuf queue as per the buffer type */
+        videobuf_queue_dma_contig_init(&layer->buffer_queue,
+                                &video_qops,
+                                vpbe_dev->pdev,
+                                &layer->irqlock,
+                                V4L2_BUF_TYPE_VIDEO_OUTPUT,
+                                layer->pix_fmt.field,
+                                sizeof(struct videobuf_buffer),
+                                fh, NULL);
+
+        /* Set io allowed member of file handle to TRUE */
+        fh->io_allowed = 1;
+        /* Increment io usrs member of layer object to 1 */
+        layer->io_usrs = 1;
+        /* Store type of memory requested in layer object */
+        layer->memory = req_buf->memory;
+        /* Initialize buffer queue */
+        INIT_LIST_HEAD(&layer->dma_queue);
+        /* Allocate buffers */
+        ret = videobuf_reqbufs(&layer->buffer_queue, req_buf);
+
+        return ret;
+}
+
+/*
+ * vpbe_display_mmap()
+ * It is used to map kernel space buffers into user spaces
+ */
+static int vpbe_display_mmap(struct file *filep, struct vm_area_struct *vma)
+{
+        /* Get the layer object and file handle object */
+        struct vpbe_fh *fh = filep->private_data;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "vpbe_display_mmap\n");
+
+        return videobuf_mmap_mapper(&layer->buffer_queue, vma);
+}
+
+/* vpbe_display_poll(): It is used for select/poll system call
+ */
+static unsigned int vpbe_display_poll(struct file *filep, poll_table *wait)
+{
+        struct vpbe_fh *fh = filep->private_data;
+        struct vpbe_layer *layer = fh->layer;
+        struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
+        unsigned int err = 0;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "vpbe_display_poll\n");
+        if (layer->started)
+                err = videobuf_poll_stream(filep, &layer->buffer_queue, wait);
+        return err;
+}
+
+/*
+ * vpbe_display_open()
+ * It creates object of file handle structure and stores it in private_data
+ * member of filepointer
+ */
+static int vpbe_display_open(struct file *file)
+{
+        struct vpbe_fh *fh = NULL;
+        struct vpbe_layer *layer = video_drvdata(file);
+        struct vpbe_display *disp_dev = layer->disp_dev;
+        struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+        struct osd_state *osd_device = disp_dev->osd_device;
+        int err;
+
+        /* Allocate memory for the file handle object */
+        fh = kmalloc(sizeof(struct vpbe_fh), GFP_KERNEL);
+        if (fh == NULL) {
+                v4l2_err(&vpbe_dev->v4l2_dev,
+                        "unable to allocate memory for file handle object\n");
+                return -ENOMEM;
+        }
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+                        "vpbe display open plane = %d\n",
+                        layer->device_id);
+
+        /* store pointer to fh in private_data member of filep */
+        file->private_data = fh;
+        fh->layer = layer;
+        fh->disp_dev = disp_dev;
+
+        if (!layer->usrs) {
+
+                /* First claim the layer for this device */
+                err = osd_device->ops.request_layer(osd_device,
+                                                layer->layer_info.id);
+                if (err < 0) {
+                        /* Couldn't get layer */
+                        v4l2_err(&vpbe_dev->v4l2_dev,
+                                "Display Manager failed to allocate layer\n");
+                        kfree(fh);
+                        return -EINVAL;
+                }
+        }
+        /* Increment layer usrs counter */
+        layer->usrs++;
+        /* Set io_allowed member to false */
+        fh->io_allowed = 0;
+        /* Initialize priority of this instance to default priority */
+        fh->prio = V4L2_PRIORITY_UNSET;
+        v4l2_prio_open(&layer->prio, &fh->prio);
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+                        "vpbe display device opened successfully\n");
+        return 0;
+}
+
+/*
+ * vpbe_display_release()
+ * This function deletes buffer queue, frees the buffers and the davinci
+ * display file * handle
+ */
+static int vpbe_display_release(struct file *file)
+{
+        /* Get the layer object and file handle object */
+        struct vpbe_fh *fh = file->private_data;
+        struct vpbe_layer *layer = fh->layer;
+        struct osd_layer_config *cfg  = &layer->layer_info.config;
+        struct vpbe_display *disp_dev = fh->disp_dev;
+        struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+        struct osd_state *osd_device = disp_dev->osd_device;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "vpbe_display_release\n");
+
+        /* if this instance is doing IO */
+        if (fh->io_allowed) {
+                /* Reset io_usrs member of layer object */
+                layer->io_usrs = 0;
+
+                osd_device->ops.disable_layer(osd_device,
+                                layer->layer_info.id);
+                layer->started = 0;
+                /* Free buffers allocated */
+                videobuf_queue_cancel(&layer->buffer_queue);
+                videobuf_mmap_free(&layer->buffer_queue);
+        }
+
+        /* Decrement layer usrs counter */
+        layer->usrs--;
+        /* If this file handle has initialize encoder device, reset it */
+        if (!layer->usrs) {
+                if (cfg->pixfmt == PIXFMT_NV12) {
+                        struct vpbe_layer *otherlayer;
+                        otherlayer =
+                        _vpbe_display_get_other_win_layer(disp_dev, layer);
+                        osd_device->ops.disable_layer(osd_device,
+                                        otherlayer->layer_info.id);
+                        osd_device->ops.release_layer(osd_device,
+                                        otherlayer->layer_info.id);
+                }
+                osd_device->ops.disable_layer(osd_device,
+                                layer->layer_info.id);
+                osd_device->ops.release_layer(osd_device,
+                                layer->layer_info.id);
+        }
+        /* Close the priority */
+        v4l2_prio_close(&layer->prio, fh->prio);
+        file->private_data = NULL;
+
+        /* Free memory allocated to file handle object */
+        kfree(fh);
+
+        disp_dev->cbcr_ofst = 0;
+
+        return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int vpbe_display_g_register(struct file *file, void *priv,
+                        struct v4l2_dbg_register *reg)
+{
+        struct v4l2_dbg_match *match = &reg->match;
+
+        if (match->type >= 2) {
+                v4l2_subdev_call(vpbe_dev->venc,
+                                 core,
+                                 g_register,
+                                 reg);
+        }
+
+        return 0;
+}
+
+static int vpbe_display_s_register(struct file *file, void *priv,
+                        struct v4l2_dbg_register *reg)
+{
+        return 0;
+}
+#endif
+
+/* vpbe capture ioctl operations */
+static const struct v4l2_ioctl_ops vpbe_ioctl_ops = {
+        .vidioc_querycap         = vpbe_display_querycap,
+        .vidioc_g_fmt_vid_out    = vpbe_display_g_fmt,
+        .vidioc_enum_fmt_vid_out = vpbe_display_enum_fmt,
+        .vidioc_s_fmt_vid_out    = vpbe_display_s_fmt,
+        .vidioc_try_fmt_vid_out  = vpbe_display_try_fmt,
+        .vidioc_reqbufs          = vpbe_display_reqbufs,
+        .vidioc_querybuf         = vpbe_display_querybuf,
+        .vidioc_qbuf             = vpbe_display_qbuf,
+        .vidioc_dqbuf            = vpbe_display_dqbuf,
+        .vidioc_streamon         = vpbe_display_streamon,
+        .vidioc_streamoff        = vpbe_display_streamoff,
+        .vidioc_cropcap          = vpbe_display_cropcap,
+        .vidioc_g_crop           = vpbe_display_g_crop,
+        .vidioc_s_crop           = vpbe_display_s_crop,
+        .vidioc_g_priority       = vpbe_display_g_priority,
+        .vidioc_s_priority       = vpbe_display_s_priority,
+        .vidioc_s_std            = vpbe_display_s_std,
+        .vidioc_g_std            = vpbe_display_g_std,
+        .vidioc_enum_output      = vpbe_display_enum_output,
+        .vidioc_s_output         = vpbe_display_s_output,
+        .vidioc_g_output         = vpbe_display_g_output,
+        .vidioc_s_dv_preset      = vpbe_display_s_dv_preset,
+        .vidioc_g_dv_preset      = vpbe_display_g_dv_preset,
+        .vidioc_enum_dv_presets  = vpbe_display_enum_dv_presets,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+        .vidioc_g_register       = vpbe_display_g_register,
+        .vidioc_s_register       = vpbe_display_s_register,
+#endif
+};
+
+static struct v4l2_file_operations vpbe_fops = {
+        .owner = THIS_MODULE,
+        .open = vpbe_display_open,
+        .release = vpbe_display_release,
+        .unlocked_ioctl = video_ioctl2,
+        .mmap = vpbe_display_mmap,
+        .poll = vpbe_display_poll
+};
+
+static int vpbe_device_get(struct device *dev, void *data)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct vpbe_display *vpbe_disp  = data;
+
+        if (strcmp("vpbe_controller", pdev->name) == 0)
+                vpbe_disp->vpbe_dev = platform_get_drvdata(pdev);
+
+        if (strcmp("vpbe-osd", pdev->name) == 0)
+                vpbe_disp->osd_device = platform_get_drvdata(pdev);
+
+        return 0;
+}
+
+static __devinit int init_vpbe_layer(int i, struct vpbe_display *disp_dev,
+                                     struct platform_device *pdev)
+{
+        struct vpbe_layer *vpbe_display_layer = NULL;
+        struct video_device *vbd = NULL;
+
+        /* Allocate memory for four plane display objects */
+
+        disp_dev->dev[i] =
+                kzalloc(sizeof(struct vpbe_layer), GFP_KERNEL);
+
+        /* If memory allocation fails, return error */
+        if (!disp_dev->dev[i]) {
+                printk(KERN_ERR "ran out of memory\n");
+                return  -ENOMEM;
+        }
+        spin_lock_init(&disp_dev->dev[i]->irqlock);
+        mutex_init(&disp_dev->dev[i]->opslock);
+
+        /* Get the pointer to the layer object */
+        vpbe_display_layer = disp_dev->dev[i];
+        vbd = &vpbe_display_layer->video_dev;
+        /* Initialize field of video device */
+        vbd->release    = video_device_release_empty;
+        vbd->fops       = &vpbe_fops;
+        vbd->ioctl_ops  = &vpbe_ioctl_ops;
+        vbd->minor      = -1;
+        vbd->v4l2_dev   = &disp_dev->vpbe_dev->v4l2_dev;
+        vbd->lock       = &vpbe_display_layer->opslock;
+
+        if (disp_dev->vpbe_dev->current_timings.timings_type &
+                        VPBE_ENC_STD) {
+                vbd->tvnorms = (V4L2_STD_525_60 | V4L2_STD_625_50);
+                vbd->current_norm =
+                        disp_dev->vpbe_dev->
+                        current_timings.timings.std_id;
+        } else
+                vbd->current_norm = 0;
+
+        snprintf(vbd->name, sizeof(vbd->name),
+                        "DaVinci_VPBE Display_DRIVER_V%d.%d.%d",
+                        (VPBE_DISPLAY_VERSION_CODE >> 16) & 0xff,
+                        (VPBE_DISPLAY_VERSION_CODE >> 8) & 0xff,
+                        (VPBE_DISPLAY_VERSION_CODE) & 0xff);
+
+        vpbe_display_layer->device_id = i;
+
+        vpbe_display_layer->layer_info.id =
+                ((i == VPBE_DISPLAY_DEVICE_0) ? WIN_VID0 : WIN_VID1);
+
+        /* Initialize prio member of layer object */
+        v4l2_prio_init(&vpbe_display_layer->prio);
+
+        return 0;
+}
+
+static __devinit int register_device(struct vpbe_layer *vpbe_display_layer,
+                                        struct vpbe_display *disp_dev,
+                                        struct platform_device *pdev) {
+        int err;
+
+        v4l2_info(&disp_dev->vpbe_dev->v4l2_dev,
+                  "Trying to register VPBE display device.\n");
+        v4l2_info(&disp_dev->vpbe_dev->v4l2_dev,
+                  "layer=%x,layer->video_dev=%x\n",
+                  (int)vpbe_display_layer,
+                  (int)&vpbe_display_layer->video_dev);
+
+        err = video_register_device(&vpbe_display_layer->video_dev,
+                                    VFL_TYPE_GRABBER,
+                                    -1);
+        if (err)
+                return -ENODEV;
+
+        vpbe_display_layer->disp_dev = disp_dev;
+        /* set the driver data in platform device */
+        platform_set_drvdata(pdev, disp_dev);
+        video_set_drvdata(&vpbe_display_layer->video_dev,
+                          vpbe_display_layer);
+
+        return 0;
+}
+
+
+
+/*
+ * vpbe_display_probe()
+ * This function creates device entries by register itself to the V4L2 driver
+ * and initializes fields of each layer objects
+ */
+static __devinit int vpbe_display_probe(struct platform_device *pdev)
+{
+        struct vpbe_layer *vpbe_display_layer;
+        struct vpbe_display *disp_dev;
+        struct resource *res = NULL;
+        int k;
+        int i;
+        int err;
+        int irq;
+
+        printk(KERN_DEBUG "vpbe_display_probe\n");
+        /* Allocate memory for vpbe_display */
+        disp_dev = kzalloc(sizeof(struct vpbe_display), GFP_KERNEL);
+        if (!disp_dev) {
+                printk(KERN_ERR "ran out of memory\n");
+                return -ENOMEM;
+        }
+
+        spin_lock_init(&disp_dev->dma_queue_lock);
+        /*
+         * Scan all the platform devices to find the vpbe
+         * controller device and get the vpbe_dev object
+         */
+        err = bus_for_each_dev(&platform_bus_type, NULL, disp_dev,
+                        vpbe_device_get);
+        if (err < 0)
+                return err;
+        /* Initialize the vpbe display controller */
+        if (NULL != disp_dev->vpbe_dev->ops.initialize) {
+                err = disp_dev->vpbe_dev->ops.initialize(&pdev->dev,
+                                                         disp_dev->vpbe_dev);
+                if (err) {
+                        v4l2_err(&disp_dev->vpbe_dev->v4l2_dev,
+                                        "Error initing vpbe\n");
+                        err = -ENOMEM;
+                        goto probe_out;
+                }
+        }
+
+        for (i = 0; i < VPBE_DISPLAY_MAX_DEVICES; i++) {
+                if (init_vpbe_layer(i, disp_dev, pdev)) {
+                        err = -ENODEV;
+                        goto probe_out;
+                }
+        }
+
+        res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+        if (!res) {
+                v4l2_err(&disp_dev->vpbe_dev->v4l2_dev,
+                         "Unable to get VENC interrupt resource\n");
+                err = -ENODEV;
+                goto probe_out;
+        }
+
+        irq = res->start;
+        if (request_irq(irq, venc_isr,  IRQF_DISABLED, VPBE_DISPLAY_DRIVER,
+                disp_dev)) {
+                v4l2_err(&disp_dev->vpbe_dev->v4l2_dev,
+                                "Unable to request interrupt\n");
+                err = -ENODEV;
+                goto probe_out;
+        }
+
+        for (i = 0; i < VPBE_DISPLAY_MAX_DEVICES; i++) {
+                if (register_device(disp_dev->dev[i], disp_dev, pdev)) {
+                        err = -ENODEV;
+                        goto probe_out;
+                }
+        }
+
+        printk(KERN_DEBUG "Successfully completed the probing of vpbe v4l2 device\n");
+        return 0;
+
+probe_out:
+        free_irq(res->start, disp_dev);
+        for (k = 0; k < VPBE_DISPLAY_MAX_DEVICES; k++) {
+                /* Get the pointer to the layer object */
+                vpbe_display_layer = disp_dev->dev[k];
+                /* Unregister video device */
+                if (vpbe_display_layer) {
+                        video_unregister_device(
+                                &vpbe_display_layer->video_dev);
+                                kfree(disp_dev->dev[k]);
+                }
+        }
+        kfree(disp_dev);
+        return err;
+}
+
+/*
+ * vpbe_display_remove()
+ * It un-register hardware layer from V4L2 driver
+ */
+static int vpbe_display_remove(struct platform_device *pdev)
+{
+        struct vpbe_layer *vpbe_display_layer;
+        struct vpbe_display *disp_dev = platform_get_drvdata(pdev);
+        struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+        struct resource *res;
+        int i;
+
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "vpbe_display_remove\n");
+
+        /* unregister irq */
+        res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+        free_irq(res->start, disp_dev);
+
+        /* deinitialize the vpbe display controller */
+        if (NULL != vpbe_dev->ops.deinitialize)
+                vpbe_dev->ops.deinitialize(&pdev->dev, vpbe_dev);
+        /* un-register device */
+        for (i = 0; i < VPBE_DISPLAY_MAX_DEVICES; i++) {
+                /* Get the pointer to the layer object */
+                vpbe_display_layer = disp_dev->dev[i];
+                /* Unregister video device */
+                video_unregister_device(&vpbe_display_layer->video_dev);
+
+        }
+        for (i = 0; i < VPBE_DISPLAY_MAX_DEVICES; i++) {
+                kfree(disp_dev->dev[i]);
+                disp_dev->dev[i] = NULL;
+        }
+
+        return 0;
+}
+
+static struct platform_driver vpbe_display_driver = {
+        .driver = {
+                .name = VPBE_DISPLAY_DRIVER,
+                .owner = THIS_MODULE,
+                .bus = &platform_bus_type,
+        },
+        .probe = vpbe_display_probe,
+        .remove = __devexit_p(vpbe_display_remove),
+};
+
+/*
+ * vpbe_display_init()
+ * This function registers device and driver to the kernel, requests irq
+ * handler and allocates memory for layer objects
+ */
+static __devinit int vpbe_display_init(void)
+{
+        int err;
+
+        printk(KERN_DEBUG "vpbe_display_init\n");
+
+        /* Register driver to the kernel */
+        err = platform_driver_register(&vpbe_display_driver);
+        if (0 != err)
+                return err;
+
+        printk(KERN_DEBUG "vpbe_display_init:"
+                        "VPBE V4L2 Display Driver V1.0 loaded\n");
+        return 0;
+}
+
+/*
+ * vpbe_display_cleanup()
+ * This function un-registers device and driver to the kernel, frees requested
+ * irq handler and de-allocates memory allocated for layer objects.
+ */
+static void vpbe_display_cleanup(void)
+{
+        printk(KERN_DEBUG "vpbe_display_cleanup\n");
+
+        /* platform driver unregister */
+        platform_driver_unregister(&vpbe_display_driver);
+}
+
+/* Function for module initialization and cleanup */
+module_init(vpbe_display_init);
+module_exit(vpbe_display_cleanup);
+
+MODULE_DESCRIPTION("TI DM644x/DM355/DM365 VPBE Display controller");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Texas Instruments");
diff --git a/drivers/media/video/davinci/vpbe_osd.c b/drivers/media/video/davinci/vpbe_osd.c
new file mode 100644
index 00000000000..5352884998f
--- /dev/null
+++ b/drivers/media/video/davinci/vpbe_osd.c
@@ -0,0 +1,1231 @@
+/*
+ * Copyright (C) 2007-2010 Texas Instruments Inc
+ * Copyright (C) 2007 MontaVista Software, Inc.
+ *
+ * Andy Lowe (alowe@mvista.com), MontaVista Software
+ * - Initial version
+ * Murali Karicheri (mkaricheri@gmail.com), Texas Instruments Ltd.
+ * - ported to sub device interface
+ *
+ * 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 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
+ *
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/slab.h>
+
+#include <mach/io.h>
+#include <mach/cputype.h>
+#include <mach/hardware.h>
+
+#include <media/davinci/vpss.h>
+#include <media/v4l2-device.h>
+#include <media/davinci/vpbe_types.h>
+#include <media/davinci/vpbe_osd.h>
+
+#include <linux/io.h>
+#include "vpbe_osd_regs.h"
+
+#define MODULE_NAME     VPBE_OSD_SUBDEV_NAME
+
+/* register access routines */
+static inline u32 osd_read(struct osd_state *sd, u32 offset)
+{
+        struct osd_state *osd = sd;
+
+        return readl(osd->osd_base + offset);
+}
+
+static inline u32 osd_write(struct osd_state *sd, u32 val, u32 offset)
+{
+        struct osd_state *osd = sd;
+
+        writel(val, osd->osd_base + offset);
+
+        return val;
+}
+
+static inline u32 osd_set(struct osd_state *sd, u32 mask, u32 offset)
+{
+        struct osd_state *osd = sd;
+
+        u32 addr = osd->osd_base + offset;
+        u32 val = readl(addr) | mask;
+
+        writel(val, addr);
+
+        return val;
+}
+
+static inline u32 osd_clear(struct osd_state *sd, u32 mask, u32 offset)
+{
+        struct osd_state *osd = sd;
+
+        u32 addr = osd->osd_base + offset;
+        u32 val = readl(addr) & ~mask;
+
+        writel(val, addr);
+
+        return val;
+}
+
+static inline u32 osd_modify(struct osd_state *sd, u32 mask, u32 val,
+                                 u32 offset)
+{
+        struct osd_state *osd = sd;
+
+        u32 addr = osd->osd_base + offset;
+        u32 new_val = (readl(addr) & ~mask) | (val & mask);
+
+        writel(new_val, addr);
+
+        return new_val;
+}
+
+/* define some macros for layer and pixfmt classification */
+#define is_osd_win(layer) (((layer) == WIN_OSD0) || ((layer) == WIN_OSD1))
+#define is_vid_win(layer) (((layer) == WIN_VID0) || ((layer) == WIN_VID1))
+#define is_rgb_pixfmt(pixfmt) \
+        (((pixfmt) == PIXFMT_RGB565) || ((pixfmt) == PIXFMT_RGB888))
+#define is_yc_pixfmt(pixfmt) \
+        (((pixfmt) == PIXFMT_YCbCrI) || ((pixfmt) == PIXFMT_YCrCbI) || \
+        ((pixfmt) == PIXFMT_NV12))
+#define MAX_WIN_SIZE OSD_VIDWIN0XP_V0X
+#define MAX_LINE_LENGTH (OSD_VIDWIN0OFST_V0LO << 5)
+
+/**
+ * _osd_dm6446_vid0_pingpong() - field inversion fix for DM6446
+ * @sd - ptr to struct osd_state
+ * @field_inversion - inversion flag
+ * @fb_base_phys - frame buffer address
+ * @lconfig - ptr to layer config
+ *
+ * This routine implements a workaround for the field signal inversion silicon
+ * erratum described in Advisory 1.3.8 for the DM6446.  The fb_base_phys and
+ * lconfig parameters apply to the vid0 window.  This routine should be called
+ * whenever the vid0 layer configuration or start address is modified, or when
+ * the OSD field inversion setting is modified.
+ * Returns: 1 if the ping-pong buffers need to be toggled in the vsync isr, or
+ *          0 otherwise
+ */
+static int _osd_dm6446_vid0_pingpong(struct osd_state *sd,
+                                     int field_inversion,
+                                     unsigned long fb_base_phys,
+                                     const struct osd_layer_config *lconfig)
+{
+        struct osd_platform_data *pdata;
+
+        pdata = (struct osd_platform_data *)sd->dev->platform_data;
+        if (pdata->field_inv_wa_enable) {
+
+                if (!field_inversion || !lconfig->interlaced) {
+                        osd_write(sd, fb_base_phys & ~0x1F, OSD_VIDWIN0ADR);
+                        osd_write(sd, fb_base_phys & ~0x1F, OSD_PPVWIN0ADR);
+                        osd_modify(sd, OSD_MISCCTL_PPSW | OSD_MISCCTL_PPRV, 0,
+                                   OSD_MISCCTL);
+                        return 0;
+                } else {
+                        unsigned miscctl = OSD_MISCCTL_PPRV;
+
+                        osd_write(sd,
+                                (fb_base_phys & ~0x1F) - lconfig->line_length,
+                                OSD_VIDWIN0ADR);
+                        osd_write(sd,
+                                (fb_base_phys & ~0x1F) + lconfig->line_length,
+                                OSD_PPVWIN0ADR);
+                        osd_modify(sd,
+                                OSD_MISCCTL_PPSW | OSD_MISCCTL_PPRV, miscctl,
+                                OSD_MISCCTL);
+
+                        return 1;
+                }
+        }
+
+        return 0;
+}
+
+static void _osd_set_field_inversion(struct osd_state *sd, int enable)
+{
+        unsigned fsinv = 0;
+
+        if (enable)
+                fsinv = OSD_MODE_FSINV;
+
+        osd_modify(sd, OSD_MODE_FSINV, fsinv, OSD_MODE);
+}
+
+static void _osd_set_blink_attribute(struct osd_state *sd, int enable,
+                                     enum osd_blink_interval blink)
+{
+        u32 osdatrmd = 0;
+
+        if (enable) {
+                osdatrmd |= OSD_OSDATRMD_BLNK;
+                osdatrmd |= blink << OSD_OSDATRMD_BLNKINT_SHIFT;
+        }
+        /* caller must ensure that OSD1 is configured in attribute mode */
+        osd_modify(sd, OSD_OSDATRMD_BLNKINT | OSD_OSDATRMD_BLNK, osdatrmd,
+                  OSD_OSDATRMD);
+}
+
+static void _osd_set_rom_clut(struct osd_state *sd,
+                              enum osd_rom_clut rom_clut)
+{
+        if (rom_clut == ROM_CLUT0)
+                osd_clear(sd, OSD_MISCCTL_RSEL, OSD_MISCCTL);
+        else
+                osd_set(sd, OSD_MISCCTL_RSEL, OSD_MISCCTL);
+}
+
+static void _osd_set_palette_map(struct osd_state *sd,
+                                 enum osd_win_layer osdwin,
+                                 unsigned char pixel_value,
+                                 unsigned char clut_index,
+                                 enum osd_pix_format pixfmt)
+{
+        static const int map_2bpp[] = { 0, 5, 10, 15 };
+        static const int map_1bpp[] = { 0, 15 };
+        int bmp_offset;
+        int bmp_shift;
+        int bmp_mask;
+        int bmp_reg;
+
+        switch (pixfmt) {
+        case PIXFMT_1BPP:
+                bmp_reg = map_1bpp[pixel_value & 0x1];
+                break;
+        case PIXFMT_2BPP:
+                bmp_reg = map_2bpp[pixel_value & 0x3];
+                break;
+        case PIXFMT_4BPP:
+                bmp_reg = pixel_value & 0xf;
+                break;
+        default:
+                return;
+        }
+
+        switch (osdwin) {
+        case OSDWIN_OSD0:
+                bmp_offset = OSD_W0BMP01 + (bmp_reg >> 1) * sizeof(u32);
+                break;
+        case OSDWIN_OSD1:
+                bmp_offset = OSD_W1BMP01 + (bmp_reg >> 1) * sizeof(u32);
+                break;
+        default:
+                return;
+        }
+
+        if (bmp_reg & 1) {
+                bmp_shift = 8;
+                bmp_mask = 0xff << 8;
+        } else {
+                bmp_shift = 0;
+                bmp_mask = 0xff;
+        }
+
+        osd_modify(sd, bmp_mask, clut_index << bmp_shift, bmp_offset);
+}
+
+static void _osd_set_rec601_attenuation(struct osd_state *sd,
+                                        enum osd_win_layer osdwin, int enable)
+{
+        switch (osdwin) {
+        case OSDWIN_OSD0:
+                osd_modify(sd, OSD_OSDWIN0MD_ATN0E,
+                          enable ? OSD_OSDWIN0MD_ATN0E : 0,
+                          OSD_OSDWIN0MD);
+                break;
+        case OSDWIN_OSD1:
+                osd_modify(sd, OSD_OSDWIN1MD_ATN1E,
+                          enable ? OSD_OSDWIN1MD_ATN1E : 0,
+                          OSD_OSDWIN1MD);
+                break;
+        }
+}
+
+static void _osd_set_blending_factor(struct osd_state *sd,
+                                     enum osd_win_layer osdwin,
+                                     enum osd_blending_factor blend)
+{
+        switch (osdwin) {
+        case OSDWIN_OSD0:
+                osd_modify(sd, OSD_OSDWIN0MD_BLND0,
+                          blend << OSD_OSDWIN0MD_BLND0_SHIFT, OSD_OSDWIN0MD);
+                break;
+        case OSDWIN_OSD1:
+                osd_modify(sd, OSD_OSDWIN1MD_BLND1,
+                          blend << OSD_OSDWIN1MD_BLND1_SHIFT, OSD_OSDWIN1MD);
+                break;
+        }
+}
+
+static void _osd_enable_color_key(struct osd_state *sd,
+                                  enum osd_win_layer osdwin,
+                                  unsigned colorkey,
+                                  enum osd_pix_format pixfmt)
+{
+        switch (pixfmt) {
+        case PIXFMT_RGB565:
+                osd_write(sd, colorkey & OSD_TRANSPVAL_RGBTRANS,
+                          OSD_TRANSPVAL);
+                break;
+        default:
+                break;
+        }
+
+        switch (osdwin) {
+        case OSDWIN_OSD0:
+                osd_set(sd, OSD_OSDWIN0MD_TE0, OSD_OSDWIN0MD);
+                break;
+        case OSDWIN_OSD1:
+                osd_set(sd, OSD_OSDWIN1MD_TE1, OSD_OSDWIN1MD);
+                break;
+        }
+}
+
+static void _osd_disable_color_key(struct osd_state *sd,
+                                   enum osd_win_layer osdwin)
+{
+        switch (osdwin) {
+        case OSDWIN_OSD0:
+                osd_clear(sd, OSD_OSDWIN0MD_TE0, OSD_OSDWIN0MD);
+                break;
+        case OSDWIN_OSD1:
+                osd_clear(sd, OSD_OSDWIN1MD_TE1, OSD_OSDWIN1MD);
+                break;
+        }
+}
+
+static void _osd_set_osd_clut(struct osd_state *sd,
+                              enum osd_win_layer osdwin,
+                              enum osd_clut clut)
+{
+        u32 winmd = 0;
+
+        switch (osdwin) {
+        case OSDWIN_OSD0:
+                if (clut == RAM_CLUT)
+                        winmd |= OSD_OSDWIN0MD_CLUTS0;
+                osd_modify(sd, OSD_OSDWIN0MD_CLUTS0, winmd, OSD_OSDWIN0MD);
+                break;
+        case OSDWIN_OSD1:
+                if (clut == RAM_CLUT)
+                        winmd |= OSD_OSDWIN1MD_CLUTS1;
+                osd_modify(sd, OSD_OSDWIN1MD_CLUTS1, winmd, OSD_OSDWIN1MD);
+                break;
+        }
+}
+
+static void _osd_set_zoom(struct osd_state *sd, enum osd_layer layer,
+                          enum osd_zoom_factor h_zoom,
+                          enum osd_zoom_factor v_zoom)
+{
+        u32 winmd = 0;
+
+        switch (layer) {
+        case WIN_OSD0:
+                winmd |= (h_zoom << OSD_OSDWIN0MD_OHZ0_SHIFT);
+                winmd |= (v_zoom << OSD_OSDWIN0MD_OVZ0_SHIFT);
+                osd_modify(sd, OSD_OSDWIN0MD_OHZ0 | OSD_OSDWIN0MD_OVZ0, winmd,
+                          OSD_OSDWIN0MD);
+                break;
+        case WIN_VID0:
+                winmd |= (h_zoom << OSD_VIDWINMD_VHZ0_SHIFT);
+                winmd |= (v_zoom << OSD_VIDWINMD_VVZ0_SHIFT);
+                osd_modify(sd, OSD_VIDWINMD_VHZ0 | OSD_VIDWINMD_VVZ0, winmd,
+                          OSD_VIDWINMD);
+                break;
+        case WIN_OSD1:
+                winmd |= (h_zoom << OSD_OSDWIN1MD_OHZ1_SHIFT);
+                winmd |= (v_zoom << OSD_OSDWIN1MD_OVZ1_SHIFT);
+                osd_modify(sd, OSD_OSDWIN1MD_OHZ1 | OSD_OSDWIN1MD_OVZ1, winmd,
+                          OSD_OSDWIN1MD);
+                break;
+        case WIN_VID1:
+                winmd |= (h_zoom << OSD_VIDWINMD_VHZ1_SHIFT);
+                winmd |= (v_zoom << OSD_VIDWINMD_VVZ1_SHIFT);
+                osd_modify(sd, OSD_VIDWINMD_VHZ1 | OSD_VIDWINMD_VVZ1, winmd,
+                          OSD_VIDWINMD);
+                break;
+        }
+}
+
+static void _osd_disable_layer(struct osd_state *sd, enum osd_layer layer)
+{
+        switch (layer) {
+        case WIN_OSD0:
+                osd_clear(sd, OSD_OSDWIN0MD_OACT0, OSD_OSDWIN0MD);
+                break;
+        case WIN_VID0:
+                osd_clear(sd, OSD_VIDWINMD_ACT0, OSD_VIDWINMD);
+                break;
+        case WIN_OSD1:
+                /* disable attribute mode as well as disabling the window */
+                osd_clear(sd, OSD_OSDWIN1MD_OASW | OSD_OSDWIN1MD_OACT1,
+                          OSD_OSDWIN1MD);
+                break;
+        case WIN_VID1:
+                osd_clear(sd, OSD_VIDWINMD_ACT1, OSD_VIDWINMD);
+                break;
+        }
+}
+
+static void osd_disable_layer(struct osd_state *sd, enum osd_layer layer)
+{
+        struct osd_state *osd = sd;
+        struct osd_window_state *win = &osd->win[layer];
+        unsigned long flags;
+
+        spin_lock_irqsave(&osd->lock, flags);
+
+        if (!win->is_enabled) {
+                spin_unlock_irqrestore(&osd->lock, flags);
+                return;
+        }
+        win->is_enabled = 0;
+
+        _osd_disable_layer(sd, layer);
+
+        spin_unlock_irqrestore(&osd->lock, flags);
+}
+
+static void _osd_enable_attribute_mode(struct osd_state *sd)
+{
+        /* enable attribute mode for OSD1 */
+        osd_set(sd, OSD_OSDWIN1MD_OASW, OSD_OSDWIN1MD);
+}
+
+static void _osd_enable_layer(struct osd_state *sd, enum osd_layer layer)
+{
+        switch (layer) {
+        case WIN_OSD0:
+                osd_set(sd, OSD_OSDWIN0MD_OACT0, OSD_OSDWIN0MD);
+                break;
+        case WIN_VID0:
+                osd_set(sd, OSD_VIDWINMD_ACT0, OSD_VIDWINMD);
+                break;
+        case WIN_OSD1:
+                /* enable OSD1 and disable attribute mode */
+                osd_modify(sd, OSD_OSDWIN1MD_OASW | OSD_OSDWIN1MD_OACT1,
+                          OSD_OSDWIN1MD_OACT1, OSD_OSDWIN1MD);
+                break;
+        case WIN_VID1:
+                osd_set(sd, OSD_VIDWINMD_ACT1, OSD_VIDWINMD);
+                break;
+        }
+}
+
+static int osd_enable_layer(struct osd_state *sd, enum osd_layer layer,
+                            int otherwin)
+{
+        struct osd_state *osd = sd;
+        struct osd_window_state *win = &osd->win[layer];
+        struct osd_layer_config *cfg = &win->lconfig;
+        unsigned long flags;
+
+        spin_lock_irqsave(&osd->lock, flags);
+
+        /*
+         * use otherwin flag to know this is the other vid window
+         * in YUV420 mode, if is, skip this check
+         */
+        if (!otherwin && (!win->is_allocated ||
+                        !win->fb_base_phys ||
+                        !cfg->line_length ||
+                        !cfg->xsize ||
+                        !cfg->ysize)) {
+                spin_unlock_irqrestore(&osd->lock, flags);
+                return -1;
+        }
+
+        if (win->is_enabled) {
+                spin_unlock_irqrestore(&osd->lock, flags);
+                return 0;
+        }
+        win->is_enabled = 1;
+
+        if (cfg->pixfmt != PIXFMT_OSD_ATTR)
+                _osd_enable_layer(sd, layer);
+        else {
+                _osd_enable_attribute_mode(sd);
+                _osd_set_blink_attribute(sd, osd->is_blinking, osd->blink);
+        }
+
+        spin_unlock_irqrestore(&osd->lock, flags);
+
+        return 0;
+}
+
+static void _osd_start_layer(struct osd_state *sd, enum osd_layer layer,
+                             unsigned long fb_base_phys,
+                             unsigned long cbcr_ofst)
+{
+        switch (layer) {
+        case WIN_OSD0:
+                osd_write(sd, fb_base_phys & ~0x1F, OSD_OSDWIN0ADR);
+                break;
+        case WIN_VID0:
+                osd_write(sd, fb_base_phys & ~0x1F, OSD_VIDWIN0ADR);
+                break;
+        case WIN_OSD1:
+                osd_write(sd, fb_base_phys & ~0x1F, OSD_OSDWIN1ADR);
+                break;
+        case WIN_VID1:
+                osd_write(sd, fb_base_phys & ~0x1F, OSD_VIDWIN1ADR);
+                break;
+        }
+}
+
+static void osd_start_layer(struct osd_state *sd, enum osd_layer layer,
+                            unsigned long fb_base_phys,
+                            unsigned long cbcr_ofst)
+{
+        struct osd_state *osd = sd;
+        struct osd_window_state *win = &osd->win[layer];
+        struct osd_layer_config *cfg = &win->lconfig;
+        unsigned long flags;
+
+        spin_lock_irqsave(&osd->lock, flags);
+
+        win->fb_base_phys = fb_base_phys & ~0x1F;
+        _osd_start_layer(sd, layer, fb_base_phys, cbcr_ofst);
+
+        if (layer == WIN_VID0) {
+                osd->pingpong =
+                    _osd_dm6446_vid0_pingpong(sd, osd->field_inversion,
+                                                       win->fb_base_phys,
+                                                       cfg);
+        }
+
+        spin_unlock_irqrestore(&osd->lock, flags);
+}
+
+static void osd_get_layer_config(struct osd_state *sd, enum osd_layer layer,
+                                 struct osd_layer_config *lconfig)
+{
+        struct osd_state *osd = sd;
+        struct osd_window_state *win = &osd->win[layer];
+        unsigned long flags;
+
+        spin_lock_irqsave(&osd->lock, flags);
+
+        *lconfig = win->lconfig;
+
+        spin_unlock_irqrestore(&osd->lock, flags);
+}
+
+/**
+ * try_layer_config() - Try a specific configuration for the layer
+ * @sd  - ptr to struct osd_state
+ * @layer - layer to configure
+ * @lconfig - layer configuration to try
+ *
+ * If the requested lconfig is completely rejected and the value of lconfig on
+ * exit is the current lconfig, then try_layer_config() returns 1.  Otherwise,
+ * try_layer_config() returns 0.  A return value of 0 does not necessarily mean
+ * that the value of lconfig on exit is identical to the value of lconfig on
+ * entry, but merely that it represents a change from the current lconfig.
+ */
+static int try_layer_config(struct osd_state *sd, enum osd_layer layer,
+                            struct osd_layer_config *lconfig)
+{
+        struct osd_state *osd = sd;
+        struct osd_window_state *win = &osd->win[layer];
+        int bad_config;
+
+        /* verify that the pixel format is compatible with the layer */
+        switch (lconfig->pixfmt) {
+        case PIXFMT_1BPP:
+        case PIXFMT_2BPP:
+        case PIXFMT_4BPP:
+        case PIXFMT_8BPP:
+        case PIXFMT_RGB565:
+                bad_config = !is_osd_win(layer);
+                break;
+        case PIXFMT_YCbCrI:
+        case PIXFMT_YCrCbI:
+                bad_config = !is_vid_win(layer);
+                break;
+        case PIXFMT_RGB888:
+                bad_config = !is_vid_win(layer);
+                break;
+        case PIXFMT_NV12:
+                bad_config = 1;
+                break;
+        case PIXFMT_OSD_ATTR:
+                bad_config = (layer != WIN_OSD1);
+                break;
+        default:
+                bad_config = 1;
+                break;
+        }
+        if (bad_config) {
+                /*
+                 * The requested pixel format is incompatible with the layer,
+                 * so keep the current layer configuration.
+                 */
+                *lconfig = win->lconfig;
+                return bad_config;
+        }
+
+        /* DM6446: */
+        /* only one OSD window at a time can use RGB pixel formats */
+        if (is_osd_win(layer) && is_rgb_pixfmt(lconfig->pixfmt)) {
+                enum osd_pix_format pixfmt;
+                if (layer == WIN_OSD0)
+                        pixfmt = osd->win[WIN_OSD1].lconfig.pixfmt;
+                else
+                        pixfmt = osd->win[WIN_OSD0].lconfig.pixfmt;
+
+                if (is_rgb_pixfmt(pixfmt)) {
+                        /*
+                         * The other OSD window is already configured for an
+                         * RGB, so keep the current layer configuration.
+                         */
+                        *lconfig = win->lconfig;
+                        return 1;
+                }
+        }
+
+        /* DM6446: only one video window at a time can use RGB888 */
+        if (is_vid_win(layer) && lconfig->pixfmt == PIXFMT_RGB888) {
+                enum osd_pix_format pixfmt;
+
+                if (layer == WIN_VID0)
+                        pixfmt = osd->win[WIN_VID1].lconfig.pixfmt;
+                else
+                        pixfmt = osd->win[WIN_VID0].lconfig.pixfmt;
+
+                if (pixfmt == PIXFMT_RGB888) {
+                        /*
+                         * The other video window is already configured for
+                         * RGB888, so keep the current layer configuration.
+                         */
+                        *lconfig = win->lconfig;
+                        return 1;
+                }
+        }
+
+        /* window dimensions must be non-zero */
+        if (!lconfig->line_length || !lconfig->xsize || !lconfig->ysize) {
+                *lconfig = win->lconfig;
+                return 1;
+        }
+
+        /* round line_length up to a multiple of 32 */
+        lconfig->line_length = ((lconfig->line_length + 31) / 32) * 32;
+        lconfig->line_length =
+            min(lconfig->line_length, (unsigned)MAX_LINE_LENGTH);
+        lconfig->xsize = min(lconfig->xsize, (unsigned)MAX_WIN_SIZE);
+        lconfig->ysize = min(lconfig->ysize, (unsigned)MAX_WIN_SIZE);
+        lconfig->xpos = min(lconfig->xpos, (unsigned)MAX_WIN_SIZE);
+        lconfig->ypos = min(lconfig->ypos, (unsigned)MAX_WIN_SIZE);
+        lconfig->interlaced = (lconfig->interlaced != 0);
+        if (lconfig->interlaced) {
+                /* ysize and ypos must be even for interlaced displays */
+                lconfig->ysize &= ~1;
+                lconfig->ypos &= ~1;
+        }
+
+        return 0;
+}
+
+static void _osd_disable_vid_rgb888(struct osd_state *sd)
+{
+        /*
+         * The DM6446 supports RGB888 pixel format in a single video window.
+         * This routine disables RGB888 pixel format for both video windows.
+         * The caller must ensure that neither video window is currently
+         * configured for RGB888 pixel format.
+         */
+        osd_clear(sd, OSD_MISCCTL_RGBEN, OSD_MISCCTL);
+}
+
+static void _osd_enable_vid_rgb888(struct osd_state *sd,
+                                   enum osd_layer layer)
+{
+        /*
+         * The DM6446 supports RGB888 pixel format in a single video window.
+         * This routine enables RGB888 pixel format for the specified video
+         * window.  The caller must ensure that the other video window is not
+         * currently configured for RGB888 pixel format, as this routine will
+         * disable RGB888 pixel format for the other window.
+         */
+        if (layer == WIN_VID0) {
+                osd_modify(sd, OSD_MISCCTL_RGBEN | OSD_MISCCTL_RGBWIN,
+                          OSD_MISCCTL_RGBEN, OSD_MISCCTL);
+        } else if (layer == WIN_VID1) {
+                osd_modify(sd, OSD_MISCCTL_RGBEN | OSD_MISCCTL_RGBWIN,
+                          OSD_MISCCTL_RGBEN | OSD_MISCCTL_RGBWIN,
+                          OSD_MISCCTL);
+        }
+}
+
+static void _osd_set_cbcr_order(struct osd_state *sd,
+                                enum osd_pix_format pixfmt)
+{
+        /*
+         * The caller must ensure that all windows using YC pixfmt use the same
+         * Cb/Cr order.
+         */
+        if (pixfmt == PIXFMT_YCbCrI)
+                osd_clear(sd, OSD_MODE_CS, OSD_MODE);
+        else if (pixfmt == PIXFMT_YCrCbI)
+                osd_set(sd, OSD_MODE_CS, OSD_MODE);
+}
+
+static void _osd_set_layer_config(struct osd_state *sd, enum osd_layer layer,
+                                  const struct osd_layer_config *lconfig)
+{
+        u32 winmd = 0, winmd_mask = 0, bmw = 0;
+
+        _osd_set_cbcr_order(sd, lconfig->pixfmt);
+
+        switch (layer) {
+        case WIN_OSD0:
+                winmd_mask |= OSD_OSDWIN0MD_RGB0E;
+                if (lconfig->pixfmt == PIXFMT_RGB565)
+                        winmd |= OSD_OSDWIN0MD_RGB0E;
+
+                winmd_mask |= OSD_OSDWIN0MD_BMW0 | OSD_OSDWIN0MD_OFF0;
+
+                switch (lconfig->pixfmt) {
+                case PIXFMT_1BPP:
+                        bmw = 0;
+                        break;
+                case PIXFMT_2BPP:
+                        bmw = 1;
+                        break;
+                case PIXFMT_4BPP:
+                        bmw = 2;
+                        break;
+                case PIXFMT_8BPP:
+                        bmw = 3;
+                        break;
+                default:
+                        break;
+                }
+                winmd |= (bmw << OSD_OSDWIN0MD_BMW0_SHIFT);
+
+                if (lconfig->interlaced)
+                        winmd |= OSD_OSDWIN0MD_OFF0;
+
+                osd_modify(sd, winmd_mask, winmd, OSD_OSDWIN0MD);
+                osd_write(sd, lconfig->line_length >> 5, OSD_OSDWIN0OFST);
+                osd_write(sd, lconfig->xpos, OSD_OSDWIN0XP);
+                osd_write(sd, lconfig->xsize, OSD_OSDWIN0XL);
+                if (lconfig->interlaced) {
+                        osd_write(sd, lconfig->ypos >> 1, OSD_OSDWIN0YP);
+                        osd_write(sd, lconfig->ysize >> 1, OSD_OSDWIN0YL);
+                } else {
+                        osd_write(sd, lconfig->ypos, OSD_OSDWIN0YP);
+                        osd_write(sd, lconfig->ysize, OSD_OSDWIN0YL);
+                }
+                break;
+        case WIN_VID0:
+                winmd_mask |= OSD_VIDWINMD_VFF0;
+                if (lconfig->interlaced)
+                        winmd |= OSD_VIDWINMD_VFF0;
+
+                osd_modify(sd, winmd_mask, winmd, OSD_VIDWINMD);
+                osd_write(sd, lconfig->line_length >> 5, OSD_VIDWIN0OFST);
+                osd_write(sd, lconfig->xpos, OSD_VIDWIN0XP);
+                osd_write(sd, lconfig->xsize, OSD_VIDWIN0XL);
+                /*
+                 * For YUV420P format the register contents are
+                 * duplicated in both VID registers
+                 */
+                if (lconfig->interlaced) {
+                        osd_write(sd, lconfig->ypos >> 1, OSD_VIDWIN0YP);
+                        osd_write(sd, lconfig->ysize >> 1, OSD_VIDWIN0YL);
+                } else {
+                        osd_write(sd, lconfig->ypos, OSD_VIDWIN0YP);
+                        osd_write(sd, lconfig->ysize, OSD_VIDWIN0YL);
+                }
+                break;
+        case WIN_OSD1:
+                /*
+                 * The caller must ensure that OSD1 is disabled prior to
+                 * switching from a normal mode to attribute mode or from
+                 * attribute mode to a normal mode.
+                 */
+                if (lconfig->pixfmt == PIXFMT_OSD_ATTR) {
+                        winmd_mask |=
+                            OSD_OSDWIN1MD_ATN1E | OSD_OSDWIN1MD_RGB1E |
+                            OSD_OSDWIN1MD_CLUTS1 |
+                            OSD_OSDWIN1MD_BLND1 | OSD_OSDWIN1MD_TE1;
+                } else {
+                        winmd_mask |= OSD_OSDWIN1MD_RGB1E;
+                        if (lconfig->pixfmt == PIXFMT_RGB565)
+                                winmd |= OSD_OSDWIN1MD_RGB1E;
+
+                        winmd_mask |= OSD_OSDWIN1MD_BMW1;
+                        switch (lconfig->pixfmt) {
+                        case PIXFMT_1BPP:
+                                bmw = 0;
+                                break;
+                        case PIXFMT_2BPP:
+                                bmw = 1;
+                                break;
+                        case PIXFMT_4BPP:
+                                bmw = 2;
+                                break;
+                        case PIXFMT_8BPP:
+                                bmw = 3;
+                                break;
+                        default:
+                                break;
+                        }
+                        winmd |= (bmw << OSD_OSDWIN1MD_BMW1_SHIFT);
+                }
+
+                winmd_mask |= OSD_OSDWIN1MD_OFF1;
+                if (lconfig->interlaced)
+                        winmd |= OSD_OSDWIN1MD_OFF1;
+
+                osd_modify(sd, winmd_mask, winmd, OSD_OSDWIN1MD);
+                osd_write(sd, lconfig->line_length >> 5, OSD_OSDWIN1OFST);
+                osd_write(sd, lconfig->xpos, OSD_OSDWIN1XP);
+                osd_write(sd, lconfig->xsize, OSD_OSDWIN1XL);
+                if (lconfig->interlaced) {
+                        osd_write(sd, lconfig->ypos >> 1, OSD_OSDWIN1YP);
+                        osd_write(sd, lconfig->ysize >> 1, OSD_OSDWIN1YL);
+                } else {
+                        osd_write(sd, lconfig->ypos, OSD_OSDWIN1YP);
+                        osd_write(sd, lconfig->ysize, OSD_OSDWIN1YL);
+                }
+                break;
+        case WIN_VID1:
+                winmd_mask |= OSD_VIDWINMD_VFF1;
+                if (lconfig->interlaced)
+                        winmd |= OSD_VIDWINMD_VFF1;
+
+                osd_modify(sd, winmd_mask, winmd, OSD_VIDWINMD);
+                osd_write(sd, lconfig->line_length >> 5, OSD_VIDWIN1OFST);
+                osd_write(sd, lconfig->xpos, OSD_VIDWIN1XP);
+                osd_write(sd, lconfig->xsize, OSD_VIDWIN1XL);
+                /*
+                 * For YUV420P format the register contents are
+                 * duplicated in both VID registers
+                 */
+                osd_modify(sd, OSD_MISCCTL_S420D, ~OSD_MISCCTL_S420D,
+                           OSD_MISCCTL);
+
+                if (lconfig->interlaced) {
+                        osd_write(sd, lconfig->ypos >> 1, OSD_VIDWIN1YP);
+                        osd_write(sd, lconfig->ysize >> 1, OSD_VIDWIN1YL);
+                } else {
+                        osd_write(sd, lconfig->ypos, OSD_VIDWIN1YP);
+                        osd_write(sd, lconfig->ysize, OSD_VIDWIN1YL);
+                }
+                break;
+        }
+}
+
+static int osd_set_layer_config(struct osd_state *sd, enum osd_layer layer,
+                                struct osd_layer_config *lconfig)
+{
+        struct osd_state *osd = sd;
+        struct osd_window_state *win = &osd->win[layer];
+        struct osd_layer_config *cfg = &win->lconfig;
+        unsigned long flags;
+        int reject_config;
+
+        spin_lock_irqsave(&osd->lock, flags);
+
+        reject_config = try_layer_config(sd, layer, lconfig);
+        if (reject_config) {
+                spin_unlock_irqrestore(&osd->lock, flags);
+                return reject_config;
+        }
+
+        /* update the current Cb/Cr order */
+        if (is_yc_pixfmt(lconfig->pixfmt))
+                osd->yc_pixfmt = lconfig->pixfmt;
+
+        /*
+         * If we are switching OSD1 from normal mode to attribute mode or from
+         * attribute mode to normal mode, then we must disable the window.
+         */
+        if (layer == WIN_OSD1) {
+                if (((lconfig->pixfmt == PIXFMT_OSD_ATTR) &&
+                  (cfg->pixfmt != PIXFMT_OSD_ATTR)) ||
+                  ((lconfig->pixfmt != PIXFMT_OSD_ATTR) &&
+                  (cfg->pixfmt == PIXFMT_OSD_ATTR))) {
+                        win->is_enabled = 0;
+                        _osd_disable_layer(sd, layer);
+                }
+        }
+
+        _osd_set_layer_config(sd, layer, lconfig);
+
+        if (layer == WIN_OSD1) {
+                struct osd_osdwin_state *osdwin_state =
+                    &osd->osdwin[OSDWIN_OSD1];
+
+                if ((lconfig->pixfmt != PIXFMT_OSD_ATTR) &&
+                  (cfg->pixfmt == PIXFMT_OSD_ATTR)) {
+                        /*
+                         * We just switched OSD1 from attribute mode to normal
+                         * mode, so we must initialize the CLUT select, the
+                         * blend factor, transparency colorkey enable, and
+                         * attenuation enable (DM6446 only) bits in the
+                         * OSDWIN1MD register.
+                         */
+                        _osd_set_osd_clut(sd, OSDWIN_OSD1,
+                                                   osdwin_state->clut);
+                        _osd_set_blending_factor(sd, OSDWIN_OSD1,
+                                                          osdwin_state->blend);
+                        if (osdwin_state->colorkey_blending) {
+                                _osd_enable_color_key(sd, OSDWIN_OSD1,
+                                                               osdwin_state->
+                                                               colorkey,
+                                                               lconfig->pixfmt);
+                        } else
+                                _osd_disable_color_key(sd, OSDWIN_OSD1);
+                        _osd_set_rec601_attenuation(sd, OSDWIN_OSD1,
+                                                    osdwin_state->
+                                                    rec601_attenuation);
+                } else if ((lconfig->pixfmt == PIXFMT_OSD_ATTR) &&
+                  (cfg->pixfmt != PIXFMT_OSD_ATTR)) {
+                        /*
+                         * We just switched OSD1 from normal mode to attribute
+                         * mode, so we must initialize the blink enable and
+                         * blink interval bits in the OSDATRMD register.
+                         */
+                        _osd_set_blink_attribute(sd, osd->is_blinking,
+                                                          osd->blink);
+                }
+        }
+
+        /*
+         * If we just switched to a 1-, 2-, or 4-bits-per-pixel bitmap format
+         * then configure a default palette map.
+         */
+        if ((lconfig->pixfmt != cfg->pixfmt) &&
+          ((lconfig->pixfmt == PIXFMT_1BPP) ||
+          (lconfig->pixfmt == PIXFMT_2BPP) ||
+          (lconfig->pixfmt == PIXFMT_4BPP))) {
+                enum osd_win_layer osdwin =
+                    ((layer == WIN_OSD0) ? OSDWIN_OSD0 : OSDWIN_OSD1);
+                struct osd_osdwin_state *osdwin_state =
+                    &osd->osdwin[osdwin];
+                unsigned char clut_index;
+                unsigned char clut_entries = 0;
+
+                switch (lconfig->pixfmt) {
+                case PIXFMT_1BPP:
+                        clut_entries = 2;
+                        break;
+                case PIXFMT_2BPP:
+                        clut_entries = 4;
+                        break;
+                case PIXFMT_4BPP:
+                        clut_entries = 16;
+                        break;
+                default:
+                        break;
+                }
+                /*
+                 * The default palette map maps the pixel value to the clut
+                 * index, i.e. pixel value 0 maps to clut entry 0, pixel value
+                 * 1 maps to clut entry 1, etc.
+                 */
+                for (clut_index = 0; clut_index < 16; clut_index++) {
+                        osdwin_state->palette_map[clut_index] = clut_index;
+                        if (clut_index < clut_entries) {
+                                _osd_set_palette_map(sd, osdwin, clut_index,
+                                                     clut_index,
+                                                     lconfig->pixfmt);
+                        }
+                }
+        }
+
+        *cfg = *lconfig;
+        /* DM6446: configure the RGB888 enable and window selection */
+        if (osd->win[WIN_VID0].lconfig.pixfmt == PIXFMT_RGB888)
+                _osd_enable_vid_rgb888(sd, WIN_VID0);
+        else if (osd->win[WIN_VID1].lconfig.pixfmt == PIXFMT_RGB888)
+                _osd_enable_vid_rgb888(sd, WIN_VID1);
+        else
+                _osd_disable_vid_rgb888(sd);
+
+        if (layer == WIN_VID0) {
+                osd->pingpong =
+                    _osd_dm6446_vid0_pingpong(sd, osd->field_inversion,
+                                                       win->fb_base_phys,
+                                                       cfg);
+        }
+
+        spin_unlock_irqrestore(&osd->lock, flags);
+
+        return 0;
+}
+
+static void osd_init_layer(struct osd_state *sd, enum osd_layer layer)
+{
+        struct osd_state *osd = sd;
+        struct osd_window_state *win = &osd->win[layer];
+        enum osd_win_layer osdwin;
+        struct osd_osdwin_state *osdwin_state;
+        struct osd_layer_config *cfg = &win->lconfig;
+        unsigned long flags;
+
+        spin_lock_irqsave(&osd->lock, flags);
+
+        win->is_enabled = 0;
+        _osd_disable_layer(sd, layer);
+
+        win->h_zoom = ZOOM_X1;
+        win->v_zoom = ZOOM_X1;
+        _osd_set_zoom(sd, layer, win->h_zoom, win->v_zoom);
+
+        win->fb_base_phys = 0;
+        _osd_start_layer(sd, layer, win->fb_base_phys, 0);
+
+        cfg->line_length = 0;
+        cfg->xsize = 0;
+        cfg->ysize = 0;
+        cfg->xpos = 0;
+        cfg->ypos = 0;
+        cfg->interlaced = 0;
+        switch (layer) {
+        case WIN_OSD0:
+        case WIN_OSD1:
+                osdwin = (layer == WIN_OSD0) ? OSDWIN_OSD0 : OSDWIN_OSD1;
+                osdwin_state = &osd->osdwin[osdwin];
+                /*
+                 * Other code relies on the fact that OSD windows default to a
+                 * bitmap pixel format when they are deallocated, so don't
+                 * change this default pixel format.
+                 */
+                cfg->pixfmt = PIXFMT_8BPP;
+                _osd_set_layer_config(sd, layer, cfg);
+                osdwin_state->clut = RAM_CLUT;
+                _osd_set_osd_clut(sd, osdwin, osdwin_state->clut);
+                osdwin_state->colorkey_blending = 0;
+                _osd_disable_color_key(sd, osdwin);
+                osdwin_state->blend = OSD_8_VID_0;
+                _osd_set_blending_factor(sd, osdwin, osdwin_state->blend);
+                osdwin_state->rec601_attenuation = 0;
+                _osd_set_rec601_attenuation(sd, osdwin,
+                                                     osdwin_state->
+                                                     rec601_attenuation);
+                if (osdwin == OSDWIN_OSD1) {
+                        osd->is_blinking = 0;
+                        osd->blink = BLINK_X1;
+                }
+                break;
+        case WIN_VID0:
+        case WIN_VID1:
+                cfg->pixfmt = osd->yc_pixfmt;
+                _osd_set_layer_config(sd, layer, cfg);
+                break;
+        }
+
+        spin_unlock_irqrestore(&osd->lock, flags);
+}
+
+static void osd_release_layer(struct osd_state *sd, enum osd_layer layer)
+{
+        struct osd_state *osd = sd;
+        struct osd_window_state *win = &osd->win[layer];
+        unsigned long flags;
+
+        spin_lock_irqsave(&osd->lock, flags);
+
+        if (!win->is_allocated) {
+                spin_unlock_irqrestore(&osd->lock, flags);
+                return;
+        }
+
+        spin_unlock_irqrestore(&osd->lock, flags);
+        osd_init_layer(sd, layer);
+        spin_lock_irqsave(&osd->lock, flags);
+
+        win->is_allocated = 0;
+
+        spin_unlock_irqrestore(&osd->lock, flags);
+}
+
+static int osd_request_layer(struct osd_state *sd, enum osd_layer layer)
+{
+        struct osd_state *osd = sd;
+        struct osd_window_state *win = &osd->win[layer];
+        unsigned long flags;
+
+        spin_lock_irqsave(&osd->lock, flags);
+
+        if (win->is_allocated) {
+                spin_unlock_irqrestore(&osd->lock, flags);
+                return -1;
+        }
+        win->is_allocated = 1;
+
+        spin_unlock_irqrestore(&osd->lock, flags);
+
+        return 0;
+}
+
+static void _osd_init(struct osd_state *sd)
+{
+        osd_write(sd, 0, OSD_MODE);
+        osd_write(sd, 0, OSD_VIDWINMD);
+        osd_write(sd, 0, OSD_OSDWIN0MD);
+        osd_write(sd, 0, OSD_OSDWIN1MD);
+        osd_write(sd, 0, OSD_RECTCUR);
+        osd_write(sd, 0, OSD_MISCCTL);
+}
+
+static void osd_set_left_margin(struct osd_state *sd, u32 val)
+{
+        osd_write(sd, val, OSD_BASEPX);
+}
+
+static void osd_set_top_margin(struct osd_state *sd, u32 val)
+{
+        osd_write(sd, val, OSD_BASEPY);
+}
+
+static int osd_initialize(struct osd_state *osd)
+{
+        if (osd == NULL)
+                return -ENODEV;
+        _osd_init(osd);
+
+        /* set default Cb/Cr order */
+        osd->yc_pixfmt = PIXFMT_YCbCrI;
+
+        _osd_set_field_inversion(osd, osd->field_inversion);
+        _osd_set_rom_clut(osd, osd->rom_clut);
+
+        osd_init_layer(osd, WIN_OSD0);
+        osd_init_layer(osd, WIN_VID0);
+        osd_init_layer(osd, WIN_OSD1);
+        osd_init_layer(osd, WIN_VID1);
+
+        return 0;
+}
+
+static const struct vpbe_osd_ops osd_ops = {
+        .initialize = osd_initialize,
+        .request_layer = osd_request_layer,
+        .release_layer = osd_release_layer,
+        .enable_layer = osd_enable_layer,
+        .disable_layer = osd_disable_layer,
+        .set_layer_config = osd_set_layer_config,
+        .get_layer_config = osd_get_layer_config,
+        .start_layer = osd_start_layer,
+        .set_left_margin = osd_set_left_margin,
+        .set_top_margin = osd_set_top_margin,
+};
+
+static int osd_probe(struct platform_device *pdev)
+{
+        struct osd_platform_data *pdata;
+        struct osd_state *osd;
+        struct resource *res;
+        int ret = 0;
+
+        osd = kzalloc(sizeof(struct osd_state), GFP_KERNEL);
+        if (osd == NULL)
+                return -ENOMEM;
+
+        osd->dev = &pdev->dev;
+        pdata = (struct osd_platform_data *)pdev->dev.platform_data;
+        osd->vpbe_type = (enum vpbe_version)pdata->vpbe_type;
+        if (NULL == pdev->dev.platform_data) {
+                dev_err(osd->dev, "No platform data defined for OSD"
+                        " sub device\n");
+                ret = -ENOENT;
+                goto free_mem;
+        }
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!res) {
+                dev_err(osd->dev, "Unable to get OSD register address map\n");
+                ret = -ENODEV;
+                goto free_mem;
+        }
+        osd->osd_base_phys = res->start;
+        osd->osd_size = res->end - res->start + 1;
+        if (!request_mem_region(osd->osd_base_phys, osd->osd_size,
+                                MODULE_NAME)) {
+                dev_err(osd->dev, "Unable to reserve OSD MMIO region\n");
+                ret = -ENODEV;
+                goto free_mem;
+        }
+        osd->osd_base = (unsigned long)ioremap_nocache(res->start,
+                                                        osd->osd_size);
+        if (!osd->osd_base) {
+                dev_err(osd->dev, "Unable to map the OSD region\n");
+                ret = -ENODEV;
+                goto release_mem_region;
+        }
+        spin_lock_init(&osd->lock);
+        osd->ops = osd_ops;
+        platform_set_drvdata(pdev, osd);
+        dev_notice(osd->dev, "OSD sub device probe success\n");
+        return ret;
+
+release_mem_region:
+        release_mem_region(osd->osd_base_phys, osd->osd_size);
+free_mem:
+        kfree(osd);
+        return ret;
+}
+
+static int osd_remove(struct platform_device *pdev)
+{
+        struct osd_state *osd = platform_get_drvdata(pdev);
+
+        iounmap((void *)osd->osd_base);
+        release_mem_region(osd->osd_base_phys, osd->osd_size);
+        kfree(osd);
+        return 0;
+}
+
+static struct platform_driver osd_driver = {
+        .probe          = osd_probe,
+        .remove         = osd_remove,
+        .driver         = {
+                .name   = MODULE_NAME,
+                .owner  = THIS_MODULE,
+        },
+};
+
+static int osd_init(void)
+{
+        if (platform_driver_register(&osd_driver)) {
+                printk(KERN_ERR "Unable to register davinci osd driver\n");
+                return -ENODEV;
+        }
+
+        return 0;
+}
+
+static void osd_exit(void)
+{
+        platform_driver_unregister(&osd_driver);
+}
+
+module_init(osd_init);
+module_exit(osd_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("DaVinci OSD Manager Driver");
+MODULE_AUTHOR("Texas Instruments");
diff --git a/drivers/media/video/davinci/vpbe_osd_regs.h b/drivers/media/video/davinci/vpbe_osd_regs.h
new file mode 100644
index 00000000000..584520f3af6
--- /dev/null
+++ b/drivers/media/video/davinci/vpbe_osd_regs.h
@@ -0,0 +1,364 @@
+/*
+ * Copyright (C) 2006-2010 Texas Instruments 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 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
+ */
+#ifndef _VPBE_OSD_REGS_H
+#define _VPBE_OSD_REGS_H
+
+/* VPBE Global Registers */
+#define VPBE_PID                                0x0
+#define VPBE_PCR                                0x4
+
+/* VPSS CLock Registers */
+#define VPSSCLK_PID                             0x00
+#define VPSSCLK_CLKCTRL                         0x04
+
+/* VPSS Buffer Logic Registers */
+#define VPSSBL_PID                              0x00
+#define VPSSBL_PCR                              0x04
+#define VPSSBL_BCR                              0x08
+#define VPSSBL_INTSTAT                          0x0C
+#define VPSSBL_INTSEL                           0x10
+#define VPSSBL_EVTSEL                           0x14
+#define VPSSBL_MEMCTRL                          0x18
+#define VPSSBL_CCDCMUX                          0x1C
+
+/* DM365 ISP5 system configuration */
+#define ISP5_PID                                0x0
+#define ISP5_PCCR                               0x4
+#define ISP5_BCR                                0x8
+#define ISP5_INTSTAT                            0xC
+#define ISP5_INTSEL1                            0x10
+#define ISP5_INTSEL2                            0x14
+#define ISP5_INTSEL3                            0x18
+#define ISP5_EVTSEL                             0x1c
+#define ISP5_CCDCMUX                            0x20
+
+/* VPBE On-Screen Display Subsystem Registers (OSD) */
+#define OSD_MODE                                0x00
+#define OSD_VIDWINMD                            0x04
+#define OSD_OSDWIN0MD                           0x08
+#define OSD_OSDWIN1MD                           0x0C
+#define OSD_OSDATRMD                            0x0C
+#define OSD_RECTCUR                             0x10
+#define OSD_VIDWIN0OFST                         0x18
+#define OSD_VIDWIN1OFST                         0x1C
+#define OSD_OSDWIN0OFST                         0x20
+#define OSD_OSDWIN1OFST                         0x24
+#define OSD_VIDWINADH                           0x28
+#define OSD_VIDWIN0ADL                          0x2C
+#define OSD_VIDWIN0ADR                          0x2C
+#define OSD_VIDWIN1ADL                          0x30
+#define OSD_VIDWIN1ADR                          0x30
+#define OSD_OSDWINADH                           0x34
+#define OSD_OSDWIN0ADL                          0x38
+#define OSD_OSDWIN0ADR                          0x38
+#define OSD_OSDWIN1ADL                          0x3C
+#define OSD_OSDWIN1ADR                          0x3C
+#define OSD_BASEPX                              0x40
+#define OSD_BASEPY                              0x44
+#define OSD_VIDWIN0XP                           0x48
+#define OSD_VIDWIN0YP                           0x4C
+#define OSD_VIDWIN0XL                           0x50
+#define OSD_VIDWIN0YL                           0x54
+#define OSD_VIDWIN1XP                           0x58
+#define OSD_VIDWIN1YP                           0x5C
+#define OSD_VIDWIN1XL                           0x60
+#define OSD_VIDWIN1YL                           0x64
+#define OSD_OSDWIN0XP                           0x68
+#define OSD_OSDWIN0YP                           0x6C
+#define OSD_OSDWIN0XL                           0x70
+#define OSD_OSDWIN0YL                           0x74
+#define OSD_OSDWIN1XP                           0x78
+#define OSD_OSDWIN1YP                           0x7C
+#define OSD_OSDWIN1XL                           0x80
+#define OSD_OSDWIN1YL                           0x84
+#define OSD_CURXP                               0x88
+#define OSD_CURYP                               0x8C
+#define OSD_CURXL                               0x90
+#define OSD_CURYL                               0x94
+#define OSD_W0BMP01                             0xA0
+#define OSD_W0BMP23                             0xA4
+#define OSD_W0BMP45                             0xA8
+#define OSD_W0BMP67                             0xAC
+#define OSD_W0BMP89                             0xB0
+#define OSD_W0BMPAB                             0xB4
+#define OSD_W0BMPCD                             0xB8
+#define OSD_W0BMPEF                             0xBC
+#define OSD_W1BMP01                             0xC0
+#define OSD_W1BMP23                             0xC4
+#define OSD_W1BMP45                             0xC8
+#define OSD_W1BMP67                             0xCC
+#define OSD_W1BMP89                             0xD0
+#define OSD_W1BMPAB                             0xD4
+#define OSD_W1BMPCD                             0xD8
+#define OSD_W1BMPEF                             0xDC
+#define OSD_VBNDRY                              0xE0
+#define OSD_EXTMODE                             0xE4
+#define OSD_MISCCTL                             0xE8
+#define OSD_CLUTRAMYCB                          0xEC
+#define OSD_CLUTRAMCR                           0xF0
+#define OSD_TRANSPVAL                           0xF4
+#define OSD_TRANSPVALL                          0xF4
+#define OSD_TRANSPVALU                          0xF8
+#define OSD_TRANSPBMPIDX                        0xFC
+#define OSD_PPVWIN0ADR                          0xFC
+
+/* bit definitions */
+#define VPBE_PCR_VENC_DIV                       (1 << 1)
+#define VPBE_PCR_CLK_OFF                        (1 << 0)
+
+#define VPSSBL_INTSTAT_HSSIINT                  (1 << 14)
+#define VPSSBL_INTSTAT_CFALDINT                 (1 << 13)
+#define VPSSBL_INTSTAT_IPIPE_INT5               (1 << 12)
+#define VPSSBL_INTSTAT_IPIPE_INT4               (1 << 11)
+#define VPSSBL_INTSTAT_IPIPE_INT3               (1 << 10)
+#define VPSSBL_INTSTAT_IPIPE_INT2               (1 << 9)
+#define VPSSBL_INTSTAT_IPIPE_INT1               (1 << 8)
+#define VPSSBL_INTSTAT_IPIPE_INT0               (1 << 7)
+#define VPSSBL_INTSTAT_IPIPEIFINT               (1 << 6)
+#define VPSSBL_INTSTAT_OSDINT                   (1 << 5)
+#define VPSSBL_INTSTAT_VENCINT                  (1 << 4)
+#define VPSSBL_INTSTAT_H3AINT                   (1 << 3)
+#define VPSSBL_INTSTAT_CCDC_VDINT2              (1 << 2)
+#define VPSSBL_INTSTAT_CCDC_VDINT1              (1 << 1)
+#define VPSSBL_INTSTAT_CCDC_VDINT0              (1 << 0)
+
+/* DM365 ISP5 bit definitions */
+#define ISP5_INTSTAT_VENCINT                    (1 << 21)
+#define ISP5_INTSTAT_OSDINT                     (1 << 20)
+
+/* VMOD TVTYP options for HDMD=0 */
+#define SDTV_NTSC                               0
+#define SDTV_PAL                                1
+/* VMOD TVTYP options for HDMD=1 */
+#define HDTV_525P                               0
+#define HDTV_625P                               1
+#define HDTV_1080I                              2
+#define HDTV_720P                               3
+
+#define OSD_MODE_CS                             (1 << 15)
+#define OSD_MODE_OVRSZ                          (1 << 14)
+#define OSD_MODE_OHRSZ                          (1 << 13)
+#define OSD_MODE_EF                             (1 << 12)
+#define OSD_MODE_VVRSZ                          (1 << 11)
+#define OSD_MODE_VHRSZ                          (1 << 10)
+#define OSD_MODE_FSINV                          (1 << 9)
+#define OSD_MODE_BCLUT                          (1 << 8)
+#define OSD_MODE_CABG_SHIFT                     0
+#define OSD_MODE_CABG                           (0xff << 0)
+
+#define OSD_VIDWINMD_VFINV                      (1 << 15)
+#define OSD_VIDWINMD_V1EFC                      (1 << 14)
+#define OSD_VIDWINMD_VHZ1_SHIFT                 12
+#define OSD_VIDWINMD_VHZ1                       (3 << 12)
+#define OSD_VIDWINMD_VVZ1_SHIFT                 10
+#define OSD_VIDWINMD_VVZ1                       (3 << 10)
+#define OSD_VIDWINMD_VFF1                       (1 << 9)
+#define OSD_VIDWINMD_ACT1                       (1 << 8)
+#define OSD_VIDWINMD_V0EFC                      (1 << 6)
+#define OSD_VIDWINMD_VHZ0_SHIFT                 4
+#define OSD_VIDWINMD_VHZ0                       (3 << 4)
+#define OSD_VIDWINMD_VVZ0_SHIFT                 2
+#define OSD_VIDWINMD_VVZ0                       (3 << 2)
+#define OSD_VIDWINMD_VFF0                       (1 << 1)
+#define OSD_VIDWINMD_ACT0                       (1 << 0)
+
+#define OSD_OSDWIN0MD_ATN0E                     (1 << 14)
+#define OSD_OSDWIN0MD_RGB0E                     (1 << 13)
+#define OSD_OSDWIN0MD_BMP0MD_SHIFT              13
+#define OSD_OSDWIN0MD_BMP0MD                    (3 << 13)
+#define OSD_OSDWIN0MD_CLUTS0                    (1 << 12)
+#define OSD_OSDWIN0MD_OHZ0_SHIFT                10
+#define OSD_OSDWIN0MD_OHZ0                      (3 << 10)
+#define OSD_OSDWIN0MD_OVZ0_SHIFT                8
+#define OSD_OSDWIN0MD_OVZ0                      (3 << 8)
+#define OSD_OSDWIN0MD_BMW0_SHIFT                6
+#define OSD_OSDWIN0MD_BMW0                      (3 << 6)
+#define OSD_OSDWIN0MD_BLND0_SHIFT               3
+#define OSD_OSDWIN0MD_BLND0                     (7 << 3)
+#define OSD_OSDWIN0MD_TE0                       (1 << 2)
+#define OSD_OSDWIN0MD_OFF0                      (1 << 1)
+#define OSD_OSDWIN0MD_OACT0                     (1 << 0)
+
+#define OSD_OSDWIN1MD_OASW                      (1 << 15)
+#define OSD_OSDWIN1MD_ATN1E                     (1 << 14)
+#define OSD_OSDWIN1MD_RGB1E                     (1 << 13)
+#define OSD_OSDWIN1MD_BMP1MD_SHIFT              13
+#define OSD_OSDWIN1MD_BMP1MD                    (3 << 13)
+#define OSD_OSDWIN1MD_CLUTS1                    (1 << 12)
+#define OSD_OSDWIN1MD_OHZ1_SHIFT                10
+#define OSD_OSDWIN1MD_OHZ1                      (3 << 10)
+#define OSD_OSDWIN1MD_OVZ1_SHIFT                8
+#define OSD_OSDWIN1MD_OVZ1                      (3 << 8)
+#define OSD_OSDWIN1MD_BMW1_SHIFT                6
+#define OSD_OSDWIN1MD_BMW1                      (3 << 6)
+#define OSD_OSDWIN1MD_BLND1_SHIFT               3
+#define OSD_OSDWIN1MD_BLND1                     (7 << 3)
+#define OSD_OSDWIN1MD_TE1                       (1 << 2)
+#define OSD_OSDWIN1MD_OFF1                      (1 << 1)
+#define OSD_OSDWIN1MD_OACT1                     (1 << 0)
+
+#define OSD_OSDATRMD_OASW                       (1 << 15)
+#define OSD_OSDATRMD_OHZA_SHIFT                 10
+#define OSD_OSDATRMD_OHZA                       (3 << 10)
+#define OSD_OSDATRMD_OVZA_SHIFT                 8
+#define OSD_OSDATRMD_OVZA                       (3 << 8)
+#define OSD_OSDATRMD_BLNKINT_SHIFT              6
+#define OSD_OSDATRMD_BLNKINT                    (3 << 6)
+#define OSD_OSDATRMD_OFFA                       (1 << 1)
+#define OSD_OSDATRMD_BLNK                       (1 << 0)
+
+#define OSD_RECTCUR_RCAD_SHIFT                  8
+#define OSD_RECTCUR_RCAD                        (0xff << 8)
+#define OSD_RECTCUR_CLUTSR                      (1 << 7)
+#define OSD_RECTCUR_RCHW_SHIFT                  4
+#define OSD_RECTCUR_RCHW                        (7 << 4)
+#define OSD_RECTCUR_RCVW_SHIFT                  1
+#define OSD_RECTCUR_RCVW                        (7 << 1)
+#define OSD_RECTCUR_RCACT                       (1 << 0)
+
+#define OSD_VIDWIN0OFST_V0LO                    (0x1ff << 0)
+
+#define OSD_VIDWIN1OFST_V1LO                    (0x1ff << 0)
+
+#define OSD_OSDWIN0OFST_O0LO                    (0x1ff << 0)
+
+#define OSD_OSDWIN1OFST_O1LO                    (0x1ff << 0)
+
+#define OSD_WINOFST_AH_SHIFT                    9
+
+#define OSD_VIDWIN0OFST_V0AH                    (0xf << 9)
+#define OSD_VIDWIN1OFST_V1AH                    (0xf << 9)
+#define OSD_OSDWIN0OFST_O0AH                    (0xf << 9)
+#define OSD_OSDWIN1OFST_O1AH                    (0xf << 9)
+
+#define OSD_VIDWINADH_V1AH_SHIFT                8
+#define OSD_VIDWINADH_V1AH                      (0x7f << 8)
+#define OSD_VIDWINADH_V0AH_SHIFT                0
+#define OSD_VIDWINADH_V0AH                      (0x7f << 0)
+
+#define OSD_VIDWIN0ADL_V0AL                     (0xffff << 0)
+
+#define OSD_VIDWIN1ADL_V1AL                     (0xffff << 0)
+
+#define OSD_OSDWINADH_O1AH_SHIFT                8
+#define OSD_OSDWINADH_O1AH                      (0x7f << 8)
+#define OSD_OSDWINADH_O0AH_SHIFT                0
+#define OSD_OSDWINADH_O0AH                      (0x7f << 0)
+
+#define OSD_OSDWIN0ADL_O0AL                     (0xffff << 0)
+
+#define OSD_OSDWIN1ADL_O1AL                     (0xffff << 0)
+
+#define OSD_BASEPX_BPX                          (0x3ff << 0)
+
+#define OSD_BASEPY_BPY                          (0x1ff << 0)
+
+#define OSD_VIDWIN0XP_V0X                       (0x7ff << 0)
+
+#define OSD_VIDWIN0YP_V0Y                       (0x7ff << 0)
+
+#define OSD_VIDWIN0XL_V0W                       (0x7ff << 0)
+
+#define OSD_VIDWIN0YL_V0H                       (0x7ff << 0)
+
+#define OSD_VIDWIN1XP_V1X                       (0x7ff << 0)
+
+#define OSD_VIDWIN1YP_V1Y                       (0x7ff << 0)
+
+#define OSD_VIDWIN1XL_V1W                       (0x7ff << 0)
+
+#define OSD_VIDWIN1YL_V1H                       (0x7ff << 0)
+
+#define OSD_OSDWIN0XP_W0X                       (0x7ff << 0)
+
+#define OSD_OSDWIN0YP_W0Y                       (0x7ff << 0)
+
+#define OSD_OSDWIN0XL_W0W                       (0x7ff << 0)
+
+#define OSD_OSDWIN0YL_W0H                       (0x7ff << 0)
+
+#define OSD_OSDWIN1XP_W1X                       (0x7ff << 0)
+
+#define OSD_OSDWIN1YP_W1Y                       (0x7ff << 0)
+
+#define OSD_OSDWIN1XL_W1W                       (0x7ff << 0)
+
+#define OSD_OSDWIN1YL_W1H                       (0x7ff << 0)
+
+#define OSD_CURXP_RCSX                          (0x7ff << 0)
+
+#define OSD_CURYP_RCSY                          (0x7ff << 0)
+
+#define OSD_CURXL_RCSW                          (0x7ff << 0)
+
+#define OSD_CURYL_RCSH                          (0x7ff << 0)
+
+#define OSD_EXTMODE_EXPMDSEL                    (1 << 15)
+#define OSD_EXTMODE_SCRNHEXP_SHIFT              13
+#define OSD_EXTMODE_SCRNHEXP                    (3 << 13)
+#define OSD_EXTMODE_SCRNVEXP                    (1 << 12)
+#define OSD_EXTMODE_OSD1BLDCHR                  (1 << 11)
+#define OSD_EXTMODE_OSD0BLDCHR                  (1 << 10)
+#define OSD_EXTMODE_ATNOSD1EN                   (1 << 9)
+#define OSD_EXTMODE_ATNOSD0EN                   (1 << 8)
+#define OSD_EXTMODE_OSDHRSZ15                   (1 << 7)
+#define OSD_EXTMODE_VIDHRSZ15                   (1 << 6)
+#define OSD_EXTMODE_ZMFILV1HEN                  (1 << 5)
+#define OSD_EXTMODE_ZMFILV1VEN                  (1 << 4)
+#define OSD_EXTMODE_ZMFILV0HEN                  (1 << 3)
+#define OSD_EXTMODE_ZMFILV0VEN                  (1 << 2)
+#define OSD_EXTMODE_EXPFILHEN                   (1 << 1)
+#define OSD_EXTMODE_EXPFILVEN                   (1 << 0)
+
+#define OSD_MISCCTL_BLDSEL                      (1 << 15)
+#define OSD_MISCCTL_S420D                       (1 << 14)
+#define OSD_MISCCTL_BMAPT                       (1 << 13)
+#define OSD_MISCCTL_DM365M                      (1 << 12)
+#define OSD_MISCCTL_RGBEN                       (1 << 7)
+#define OSD_MISCCTL_RGBWIN                      (1 << 6)
+#define OSD_MISCCTL_DMANG                       (1 << 6)
+#define OSD_MISCCTL_TMON                        (1 << 5)
+#define OSD_MISCCTL_RSEL                        (1 << 4)
+#define OSD_MISCCTL_CPBSY                       (1 << 3)
+#define OSD_MISCCTL_PPSW                        (1 << 2)
+#define OSD_MISCCTL_PPRV                        (1 << 1)
+
+#define OSD_CLUTRAMYCB_Y_SHIFT                  8
+#define OSD_CLUTRAMYCB_Y                        (0xff << 8)
+#define OSD_CLUTRAMYCB_CB_SHIFT                 0
+#define OSD_CLUTRAMYCB_CB                       (0xff << 0)
+
+#define OSD_CLUTRAMCR_CR_SHIFT                  8
+#define OSD_CLUTRAMCR_CR                        (0xff << 8)
+#define OSD_CLUTRAMCR_CADDR_SHIFT               0
+#define OSD_CLUTRAMCR_CADDR                     (0xff << 0)
+
+#define OSD_TRANSPVAL_RGBTRANS                  (0xffff << 0)
+
+#define OSD_TRANSPVALL_RGBL                     (0xffff << 0)
+
+#define OSD_TRANSPVALU_Y_SHIFT                  8
+#define OSD_TRANSPVALU_Y                        (0xff << 8)
+#define OSD_TRANSPVALU_RGBU_SHIFT               0
+#define OSD_TRANSPVALU_RGBU                     (0xff << 0)
+
+#define OSD_TRANSPBMPIDX_BMP1_SHIFT             8
+#define OSD_TRANSPBMPIDX_BMP1                   (0xff << 8)
+#define OSD_TRANSPBMPIDX_BMP0_SHIFT             0
+#define OSD_TRANSPBMPIDX_BMP0                   0xff
+
+#endif                          /* _DAVINCI_VPBE_H_ */
diff --git a/drivers/media/video/davinci/vpbe_venc.c b/drivers/media/video/davinci/vpbe_venc.c
new file mode 100644
index 00000000000..03a3e5c65ee
--- /dev/null
+++ b/drivers/media/video/davinci/vpbe_venc.c
@@ -0,0 +1,566 @@
+/*
+ * Copyright (C) 2010 Texas Instruments 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 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
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/ctype.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+#include <linux/slab.h>
+
+#include <mach/hardware.h>
+#include <mach/mux.h>
+#include <mach/io.h>
+#include <mach/i2c.h>
+
+#include <linux/io.h>
+
+#include <media/davinci/vpbe_types.h>
+#include <media/davinci/vpbe_venc.h>
+#include <media/davinci/vpss.h>
+#include <media/v4l2-device.h>
+
+#include "vpbe_venc_regs.h"
+
+#define MODULE_NAME     VPBE_VENC_SUBDEV_NAME
+
+static int debug = 2;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Debug level 0-2");
+
+struct venc_state {
+        struct v4l2_subdev sd;
+        struct venc_callback *callback;
+        struct venc_platform_data *pdata;
+        struct device *pdev;
+        u32 output;
+        v4l2_std_id std;
+        spinlock_t lock;
+        void __iomem *venc_base;
+        void __iomem *vdaccfg_reg;
+};
+
+static inline struct venc_state *to_state(struct v4l2_subdev *sd)
+{
+        return container_of(sd, struct venc_state, sd);
+}
+
+static inline u32 venc_read(struct v4l2_subdev *sd, u32 offset)
+{
+        struct venc_state *venc = to_state(sd);
+
+        return readl(venc->venc_base + offset);
+}
+
+static inline u32 venc_write(struct v4l2_subdev *sd, u32 offset, u32 val)
+{
+        struct venc_state *venc = to_state(sd);
+
+        writel(val, (venc->venc_base + offset));
+
+        return val;
+}
+
+static inline u32 venc_modify(struct v4l2_subdev *sd, u32 offset,
+                                 u32 val, u32 mask)
+{
+        u32 new_val = (venc_read(sd, offset) & ~mask) | (val & mask);
+
+        venc_write(sd, offset, new_val);
+
+        return new_val;
+}
+
+static inline u32 vdaccfg_write(struct v4l2_subdev *sd, u32 val)
+{
+        struct venc_state *venc = to_state(sd);
+
+        writel(val, venc->vdaccfg_reg);
+
+        val = readl(venc->vdaccfg_reg);
+
+        return val;
+}
+
+/* This function sets the dac of the VPBE for various outputs
+ */
+static int venc_set_dac(struct v4l2_subdev *sd, u32 out_index)
+{
+        switch (out_index) {
+        case 0:
+                v4l2_dbg(debug, 1, sd, "Setting output to Composite\n");
+                venc_write(sd, VENC_DACSEL, 0);
+                break;
+        case 1:
+                v4l2_dbg(debug, 1, sd, "Setting output to S-Video\n");
+                venc_write(sd, VENC_DACSEL, 0x210);
+                break;
+        case  2:
+                venc_write(sd, VENC_DACSEL, 0x543);
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+static void venc_enabledigitaloutput(struct v4l2_subdev *sd, int benable)
+{
+        v4l2_dbg(debug, 2, sd, "venc_enabledigitaloutput\n");
+
+        if (benable) {
+                venc_write(sd, VENC_VMOD, 0);
+                venc_write(sd, VENC_CVBS, 0);
+                venc_write(sd, VENC_LCDOUT, 0);
+                venc_write(sd, VENC_HSPLS, 0);
+                venc_write(sd, VENC_HSTART, 0);
+                venc_write(sd, VENC_HVALID, 0);
+                venc_write(sd, VENC_HINT, 0);
+                venc_write(sd, VENC_VSPLS, 0);
+                venc_write(sd, VENC_VSTART, 0);
+                venc_write(sd, VENC_VVALID, 0);
+                venc_write(sd, VENC_VINT, 0);
+                venc_write(sd, VENC_YCCCTL, 0);
+                venc_write(sd, VENC_DACSEL, 0);
+
+        } else {
+                venc_write(sd, VENC_VMOD, 0);
+                /* disable VCLK output pin enable */
+                venc_write(sd, VENC_VIDCTL, 0x141);
+
+                /* Disable output sync pins */
+                venc_write(sd, VENC_SYNCCTL, 0);
+
+                /* Disable DCLOCK */
+                venc_write(sd, VENC_DCLKCTL, 0);
+                venc_write(sd, VENC_DRGBX1, 0x0000057C);
+
+                /* Disable LCD output control (accepting default polarity) */
+                venc_write(sd, VENC_LCDOUT, 0);
+                venc_write(sd, VENC_CMPNT, 0x100);
+                venc_write(sd, VENC_HSPLS, 0);
+                venc_write(sd, VENC_HINT, 0);
+                venc_write(sd, VENC_HSTART, 0);
+                venc_write(sd, VENC_HVALID, 0);
+
+                venc_write(sd, VENC_VSPLS, 0);
+                venc_write(sd, VENC_VINT, 0);
+                venc_write(sd, VENC_VSTART, 0);
+                venc_write(sd, VENC_VVALID, 0);
+
+                venc_write(sd, VENC_HSDLY, 0);
+                venc_write(sd, VENC_VSDLY, 0);
+
+                venc_write(sd, VENC_YCCCTL, 0);
+                venc_write(sd, VENC_VSTARTA, 0);
+
+                /* Set OSD clock and OSD Sync Adavance registers */
+                venc_write(sd, VENC_OSDCLK0, 1);
+                venc_write(sd, VENC_OSDCLK1, 2);
+        }
+}
+
+/*
+ * setting NTSC mode
+ */
+static int venc_set_ntsc(struct v4l2_subdev *sd)
+{
+        struct venc_state *venc = to_state(sd);
+        struct venc_platform_data *pdata = venc->pdata;
+
+        v4l2_dbg(debug, 2, sd, "venc_set_ntsc\n");
+
+        /* Setup clock at VPSS & VENC for SD */
+        vpss_enable_clock(VPSS_VENC_CLOCK_SEL, 1);
+        if (pdata->setup_clock(VPBE_ENC_STD, V4L2_STD_525_60) < 0)
+                return -EINVAL;
+
+        venc_enabledigitaloutput(sd, 0);
+
+        /* to set VENC CLK DIV to 1 - final clock is 54 MHz */
+        venc_modify(sd, VENC_VIDCTL, 0, 1 << 1);
+        /* Set REC656 Mode */
+        venc_write(sd, VENC_YCCCTL, 0x1);
+        venc_modify(sd, VENC_VDPRO, 0, VENC_VDPRO_DAFRQ);
+        venc_modify(sd, VENC_VDPRO, 0, VENC_VDPRO_DAUPS);
+
+        venc_write(sd, VENC_VMOD, 0);
+        venc_modify(sd, VENC_VMOD, (1 << VENC_VMOD_VIE_SHIFT),
+                        VENC_VMOD_VIE);
+        venc_modify(sd, VENC_VMOD, (0 << VENC_VMOD_VMD), VENC_VMOD_VMD);
+        venc_modify(sd, VENC_VMOD, (0 << VENC_VMOD_TVTYP_SHIFT),
+                        VENC_VMOD_TVTYP);
+        venc_write(sd, VENC_DACTST, 0x0);
+        venc_modify(sd, VENC_VMOD, VENC_VMOD_VENC, VENC_VMOD_VENC);
+
+        return 0;
+}
+
+/*
+ * setting PAL mode
+ */
+static int venc_set_pal(struct v4l2_subdev *sd)
+{
+        struct venc_state *venc = to_state(sd);
+
+        v4l2_dbg(debug, 2, sd, "venc_set_pal\n");
+
+        /* Setup clock at VPSS & VENC for SD */
+        vpss_enable_clock(VPSS_VENC_CLOCK_SEL, 1);
+        if (venc->pdata->setup_clock(VPBE_ENC_STD, V4L2_STD_625_50) < 0)
+                return -EINVAL;
+
+        venc_enabledigitaloutput(sd, 0);
+
+        /* to set VENC CLK DIV to 1 - final clock is 54 MHz */
+        venc_modify(sd, VENC_VIDCTL, 0, 1 << 1);
+        /* Set REC656 Mode */
+        venc_write(sd, VENC_YCCCTL, 0x1);
+
+        venc_modify(sd, VENC_SYNCCTL, 1 << VENC_SYNCCTL_OVD_SHIFT,
+                        VENC_SYNCCTL_OVD);
+        venc_write(sd, VENC_VMOD, 0);
+        venc_modify(sd, VENC_VMOD,
+                        (1 << VENC_VMOD_VIE_SHIFT),
+                        VENC_VMOD_VIE);
+        venc_modify(sd, VENC_VMOD,
+                        (0 << VENC_VMOD_VMD), VENC_VMOD_VMD);
+        venc_modify(sd, VENC_VMOD,
+                        (1 << VENC_VMOD_TVTYP_SHIFT),
+                        VENC_VMOD_TVTYP);
+        venc_write(sd, VENC_DACTST, 0x0);
+        venc_modify(sd, VENC_VMOD, VENC_VMOD_VENC, VENC_VMOD_VENC);
+
+        return 0;
+}
+
+/*
+ * venc_set_480p59_94
+ *
+ * This function configures the video encoder to EDTV(525p) component setting.
+ */
+static int venc_set_480p59_94(struct v4l2_subdev *sd)
+{
+        struct venc_state *venc = to_state(sd);
+        struct venc_platform_data *pdata = venc->pdata;
+
+        v4l2_dbg(debug, 2, sd, "venc_set_480p59_94\n");
+
+        /* Setup clock at VPSS & VENC for SD */
+        if (pdata->setup_clock(VPBE_ENC_DV_PRESET, V4L2_DV_480P59_94) < 0)
+                return -EINVAL;
+
+        venc_enabledigitaloutput(sd, 0);
+
+        venc_write(sd, VENC_OSDCLK0, 0);
+        venc_write(sd, VENC_OSDCLK1, 1);
+        venc_modify(sd, VENC_VDPRO, VENC_VDPRO_DAFRQ,
+                    VENC_VDPRO_DAFRQ);
+        venc_modify(sd, VENC_VDPRO, VENC_VDPRO_DAUPS,
+                    VENC_VDPRO_DAUPS);
+        venc_write(sd, VENC_VMOD, 0);
+        venc_modify(sd, VENC_VMOD, (1 << VENC_VMOD_VIE_SHIFT),
+                    VENC_VMOD_VIE);
+        venc_modify(sd, VENC_VMOD, VENC_VMOD_HDMD, VENC_VMOD_HDMD);
+        venc_modify(sd, VENC_VMOD, (HDTV_525P << VENC_VMOD_TVTYP_SHIFT),
+                    VENC_VMOD_TVTYP);
+        venc_modify(sd, VENC_VMOD, VENC_VMOD_VDMD_YCBCR8 <<
+                    VENC_VMOD_VDMD_SHIFT, VENC_VMOD_VDMD);
+
+        venc_modify(sd, VENC_VMOD, VENC_VMOD_VENC, VENC_VMOD_VENC);
+
+        return 0;
+}
+
+/*
+ * venc_set_625p
+ *
+ * This function configures the video encoder to HDTV(625p) component setting
+ */
+static int venc_set_576p50(struct v4l2_subdev *sd)
+{
+        struct venc_state *venc = to_state(sd);
+        struct venc_platform_data *pdata = venc->pdata;
+
+        v4l2_dbg(debug, 2, sd, "venc_set_576p50\n");
+
+        /* Setup clock at VPSS & VENC for SD */
+        if (pdata->setup_clock(VPBE_ENC_DV_PRESET, V4L2_DV_576P50) < 0)
+                return -EINVAL;
+
+        venc_enabledigitaloutput(sd, 0);
+
+        venc_write(sd, VENC_OSDCLK0, 0);
+        venc_write(sd, VENC_OSDCLK1, 1);
+
+        venc_modify(sd, VENC_VDPRO, VENC_VDPRO_DAFRQ,
+                    VENC_VDPRO_DAFRQ);
+        venc_modify(sd, VENC_VDPRO, VENC_VDPRO_DAUPS,
+                    VENC_VDPRO_DAUPS);
+
+        venc_write(sd, VENC_VMOD, 0);
+        venc_modify(sd, VENC_VMOD, (1 << VENC_VMOD_VIE_SHIFT),
+                    VENC_VMOD_VIE);
+        venc_modify(sd, VENC_VMOD, VENC_VMOD_HDMD, VENC_VMOD_HDMD);
+        venc_modify(sd, VENC_VMOD, (HDTV_625P << VENC_VMOD_TVTYP_SHIFT),
+                    VENC_VMOD_TVTYP);
+
+        venc_modify(sd, VENC_VMOD, VENC_VMOD_VDMD_YCBCR8 <<
+                    VENC_VMOD_VDMD_SHIFT, VENC_VMOD_VDMD);
+        venc_modify(sd, VENC_VMOD, VENC_VMOD_VENC, VENC_VMOD_VENC);
+
+        return 0;
+}
+
+static int venc_s_std_output(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+        v4l2_dbg(debug, 1, sd, "venc_s_std_output\n");
+
+        if (norm & V4L2_STD_525_60)
+                return venc_set_ntsc(sd);
+        else if (norm & V4L2_STD_625_50)
+                return venc_set_pal(sd);
+
+        return -EINVAL;
+}
+
+static int venc_s_dv_preset(struct v4l2_subdev *sd,
+                            struct v4l2_dv_preset *dv_preset)
+{
+        v4l2_dbg(debug, 1, sd, "venc_s_dv_preset\n");
+
+        if (dv_preset->preset == V4L2_DV_576P50)
+                return venc_set_576p50(sd);
+        else if (dv_preset->preset == V4L2_DV_480P59_94)
+                return venc_set_480p59_94(sd);
+
+        return -EINVAL;
+}
+
+static int venc_s_routing(struct v4l2_subdev *sd, u32 input, u32 output,
+                          u32 config)
+{
+        struct venc_state *venc = to_state(sd);
+        int ret;
+
+        v4l2_dbg(debug, 1, sd, "venc_s_routing\n");
+
+        ret = venc_set_dac(sd, output);
+        if (!ret)
+                venc->output = output;
+
+        return ret;
+}
+
+static long venc_ioctl(struct v4l2_subdev *sd,
+                        unsigned int cmd,
+                        void *arg)
+{
+        u32 val;
+
+        switch (cmd) {
+        case VENC_GET_FLD:
+                val = venc_read(sd, VENC_VSTAT);
+                *((int *)arg) = ((val & VENC_VSTAT_FIDST) ==
+                VENC_VSTAT_FIDST);
+                break;
+        default:
+                v4l2_err(sd, "Wrong IOCTL cmd\n");
+                break;
+        }
+
+        return 0;
+}
+
+static const struct v4l2_subdev_core_ops venc_core_ops = {
+        .ioctl      = venc_ioctl,
+};
+
+static const struct v4l2_subdev_video_ops venc_video_ops = {
+        .s_routing = venc_s_routing,
+        .s_std_output = venc_s_std_output,
+        .s_dv_preset = venc_s_dv_preset,
+};
+
+static const struct v4l2_subdev_ops venc_ops = {
+        .core = &venc_core_ops,
+        .video = &venc_video_ops,
+};
+
+static int venc_initialize(struct v4l2_subdev *sd)
+{
+        struct venc_state *venc = to_state(sd);
+        int ret;
+
+        /* Set default to output to composite and std to NTSC */
+        venc->output = 0;
+        venc->std = V4L2_STD_525_60;
+
+        ret = venc_s_routing(sd, 0, venc->output, 0);
+        if (ret < 0) {
+                v4l2_err(sd, "Error setting output during init\n");
+                return -EINVAL;
+        }
+
+        ret = venc_s_std_output(sd, venc->std);
+        if (ret < 0) {
+                v4l2_err(sd, "Error setting std during init\n");
+                return -EINVAL;
+        }
+
+        return ret;
+}
+
+static int venc_device_get(struct device *dev, void *data)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct venc_state **venc = data;
+
+        if (strcmp(MODULE_NAME, pdev->name) == 0)
+                *venc = platform_get_drvdata(pdev);
+
+        return 0;
+}
+
+struct v4l2_subdev *venc_sub_dev_init(struct v4l2_device *v4l2_dev,
+                const char *venc_name)
+{
+        struct venc_state *venc;
+        int err;
+
+        err = bus_for_each_dev(&platform_bus_type, NULL, &venc,
+                        venc_device_get);
+        if (venc == NULL)
+                return NULL;
+
+        v4l2_subdev_init(&venc->sd, &venc_ops);
+
+        strcpy(venc->sd.name, venc_name);
+        if (v4l2_device_register_subdev(v4l2_dev, &venc->sd) < 0) {
+                v4l2_err(v4l2_dev,
+                        "vpbe unable to register venc sub device\n");
+                return NULL;
+        }
+        if (venc_initialize(&venc->sd)) {
+                v4l2_err(v4l2_dev,
+                        "vpbe venc initialization failed\n");
+                return NULL;
+        }
+
+        return &venc->sd;
+}
+EXPORT_SYMBOL(venc_sub_dev_init);
+
+static int venc_probe(struct platform_device *pdev)
+{
+        struct venc_state *venc;
+        struct resource *res;
+        int ret;
+
+        venc = kzalloc(sizeof(struct venc_state), GFP_KERNEL);
+        if (venc == NULL)
+                return -ENOMEM;
+
+        venc->pdev = &pdev->dev;
+        venc->pdata = pdev->dev.platform_data;
+        if (NULL == venc->pdata) {
+                dev_err(venc->pdev, "Unable to get platform data for"
+                        " VENC sub device");
+                ret = -ENOENT;
+                goto free_mem;
+        }
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!res) {
+                dev_err(venc->pdev,
+                        "Unable to get VENC register address map\n");
+                ret = -ENODEV;
+                goto free_mem;
+        }
+
+        if (!request_mem_region(res->start, resource_size(res), "venc")) {
+                dev_err(venc->pdev, "Unable to reserve VENC MMIO region\n");
+                ret = -ENODEV;
+                goto free_mem;
+        }
+
+        venc->venc_base = ioremap_nocache(res->start, resource_size(res));
+        if (!venc->venc_base) {
+                dev_err(venc->pdev, "Unable to map VENC IO space\n");
+                ret = -ENODEV;
+                goto release_venc_mem_region;
+        }
+
+        spin_lock_init(&venc->lock);
+        platform_set_drvdata(pdev, venc);
+        dev_notice(venc->pdev, "VENC sub device probe success\n");
+        return 0;
+
+release_venc_mem_region:
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        release_mem_region(res->start, resource_size(res));
+free_mem:
+        kfree(venc);
+        return ret;
+}
+
+static int venc_remove(struct platform_device *pdev)
+{
+        struct venc_state *venc = platform_get_drvdata(pdev);
+        struct resource *res;
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        iounmap((void *)venc->venc_base);
+        release_mem_region(res->start, resource_size(res));
+        kfree(venc);
+
+        return 0;
+}
+
+static struct platform_driver venc_driver = {
+        .probe          = venc_probe,
+        .remove         = venc_remove,
+        .driver         = {
+                .name   = MODULE_NAME,
+                .owner  = THIS_MODULE,
+        },
+};
+
+static int venc_init(void)
+{
+        if (platform_driver_register(&venc_driver)) {
+                printk(KERN_ERR "Unable to register venc driver\n");
+                return -ENODEV;
+        }
+        return 0;
+}
+
+static void venc_exit(void)
+{
+        platform_driver_unregister(&venc_driver);
+        return;
+}
+
+module_init(venc_init);
+module_exit(venc_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("VPBE VENC Driver");
+MODULE_AUTHOR("Texas Instruments");
diff --git a/drivers/media/video/davinci/vpbe_venc_regs.h b/drivers/media/video/davinci/vpbe_venc_regs.h
new file mode 100644
index 00000000000..947cb151077
--- /dev/null
+++ b/drivers/media/video/davinci/vpbe_venc_regs.h
@@ -0,0 +1,177 @@
+/*
+ * Copyright (C) 2006-2010 Texas Instruments 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 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
+ */
+#ifndef _VPBE_VENC_REGS_H
+#define _VPBE_VENC_REGS_H
+
+/* VPBE Video Encoder / Digital LCD Subsystem Registers (VENC) */
+#define VENC_VMOD                               0x00
+#define VENC_VIDCTL                             0x04
+#define VENC_VDPRO                              0x08
+#define VENC_SYNCCTL                            0x0C
+#define VENC_HSPLS                              0x10
+#define VENC_VSPLS                              0x14
+#define VENC_HINT                               0x18
+#define VENC_HSTART                             0x1C
+#define VENC_HVALID                             0x20
+#define VENC_VINT                               0x24
+#define VENC_VSTART                             0x28
+#define VENC_VVALID                             0x2C
+#define VENC_HSDLY                              0x30
+#define VENC_VSDLY                              0x34
+#define VENC_YCCCTL                             0x38
+#define VENC_RGBCTL                             0x3C
+#define VENC_RGBCLP                             0x40
+#define VENC_LINECTL                            0x44
+#define VENC_CULLLINE                           0x48
+#define VENC_LCDOUT                             0x4C
+#define VENC_BRTS                               0x50
+#define VENC_BRTW                               0x54
+#define VENC_ACCTL                              0x58
+#define VENC_PWMP                               0x5C
+#define VENC_PWMW                               0x60
+#define VENC_DCLKCTL                            0x64
+#define VENC_DCLKPTN0                           0x68
+#define VENC_DCLKPTN1                           0x6C
+#define VENC_DCLKPTN2                           0x70
+#define VENC_DCLKPTN3                           0x74
+#define VENC_DCLKPTN0A                          0x78
+#define VENC_DCLKPTN1A                          0x7C
+#define VENC_DCLKPTN2A                          0x80
+#define VENC_DCLKPTN3A                          0x84
+#define VENC_DCLKHS                             0x88
+#define VENC_DCLKHSA                            0x8C
+#define VENC_DCLKHR                             0x90
+#define VENC_DCLKVS                             0x94
+#define VENC_DCLKVR                             0x98
+#define VENC_CAPCTL                             0x9C
+#define VENC_CAPDO                              0xA0
+#define VENC_CAPDE                              0xA4
+#define VENC_ATR0                               0xA8
+#define VENC_ATR1                               0xAC
+#define VENC_ATR2                               0xB0
+#define VENC_VSTAT                              0xB8
+#define VENC_RAMADR                             0xBC
+#define VENC_RAMPORT                            0xC0
+#define VENC_DACTST                             0xC4
+#define VENC_YCOLVL                             0xC8
+#define VENC_SCPROG                             0xCC
+#define VENC_CVBS                               0xDC
+#define VENC_CMPNT                              0xE0
+#define VENC_ETMG0                              0xE4
+#define VENC_ETMG1                              0xE8
+#define VENC_ETMG2                              0xEC
+#define VENC_ETMG3                              0xF0
+#define VENC_DACSEL                             0xF4
+#define VENC_ARGBX0                             0x100
+#define VENC_ARGBX1                             0x104
+#define VENC_ARGBX2                             0x108
+#define VENC_ARGBX3                             0x10C
+#define VENC_ARGBX4                             0x110
+#define VENC_DRGBX0                             0x114
+#define VENC_DRGBX1                             0x118
+#define VENC_DRGBX2                             0x11C
+#define VENC_DRGBX3                             0x120
+#define VENC_DRGBX4                             0x124
+#define VENC_VSTARTA                            0x128
+#define VENC_OSDCLK0                            0x12C
+#define VENC_OSDCLK1                            0x130
+#define VENC_HVLDCL0                            0x134
+#define VENC_HVLDCL1                            0x138
+#define VENC_OSDHADV                            0x13C
+#define VENC_CLKCTL                             0x140
+#define VENC_GAMCTL                             0x144
+#define VENC_XHINTVL                            0x174
+
+/* bit definitions */
+#define VPBE_PCR_VENC_DIV                       (1 << 1)
+#define VPBE_PCR_CLK_OFF                        (1 << 0)
+
+#define VENC_VMOD_VDMD_SHIFT                    12
+#define VENC_VMOD_VDMD_YCBCR16                  0
+#define VENC_VMOD_VDMD_YCBCR8                   1
+#define VENC_VMOD_VDMD_RGB666                   2
+#define VENC_VMOD_VDMD_RGB8                     3
+#define VENC_VMOD_VDMD_EPSON                    4
+#define VENC_VMOD_VDMD_CASIO                    5
+#define VENC_VMOD_VDMD_UDISPQVGA                6
+#define VENC_VMOD_VDMD_STNLCD                   7
+#define VENC_VMOD_VIE_SHIFT                     1
+#define VENC_VMOD_VDMD                          (7 << 12)
+#define VENC_VMOD_ITLCL                         (1 << 11)
+#define VENC_VMOD_ITLC                          (1 << 10)
+#define VENC_VMOD_NSIT                          (1 << 9)
+#define VENC_VMOD_HDMD                          (1 << 8)
+#define VENC_VMOD_TVTYP_SHIFT                   6
+#define VENC_VMOD_TVTYP                         (3 << 6)
+#define VENC_VMOD_SLAVE                         (1 << 5)
+#define VENC_VMOD_VMD                           (1 << 4)
+#define VENC_VMOD_BLNK                          (1 << 3)
+#define VENC_VMOD_VIE                           (1 << 1)
+#define VENC_VMOD_VENC                          (1 << 0)
+
+/* VMOD TVTYP options for HDMD=0 */
+#define SDTV_NTSC                               0
+#define SDTV_PAL                                1
+/* VMOD TVTYP options for HDMD=1 */
+#define HDTV_525P                               0
+#define HDTV_625P                               1
+#define HDTV_1080I                              2
+#define HDTV_720P                               3
+
+#define VENC_VIDCTL_VCLKP                       (1 << 14)
+#define VENC_VIDCTL_VCLKE_SHIFT                 13
+#define VENC_VIDCTL_VCLKE                       (1 << 13)
+#define VENC_VIDCTL_VCLKZ_SHIFT                 12
+#define VENC_VIDCTL_VCLKZ                       (1 << 12)
+#define VENC_VIDCTL_SYDIR_SHIFT                 8
+#define VENC_VIDCTL_SYDIR                       (1 << 8)
+#define VENC_VIDCTL_DOMD_SHIFT                  4
+#define VENC_VIDCTL_DOMD                        (3 << 4)
+#define VENC_VIDCTL_YCDIR_SHIFT                 0
+#define VENC_VIDCTL_YCDIR                       (1 << 0)
+
+#define VENC_VDPRO_ATYCC_SHIFT                  5
+#define VENC_VDPRO_ATYCC                        (1 << 5)
+#define VENC_VDPRO_ATCOM_SHIFT                  4
+#define VENC_VDPRO_ATCOM                        (1 << 4)
+#define VENC_VDPRO_DAFRQ                        (1 << 3)
+#define VENC_VDPRO_DAUPS                        (1 << 2)
+#define VENC_VDPRO_CUPS                         (1 << 1)
+#define VENC_VDPRO_YUPS                         (1 << 0)
+
+#define VENC_SYNCCTL_VPL_SHIFT                  3
+#define VENC_SYNCCTL_VPL                        (1 << 3)
+#define VENC_SYNCCTL_HPL_SHIFT                  2
+#define VENC_SYNCCTL_HPL                        (1 << 2)
+#define VENC_SYNCCTL_SYEV_SHIFT                 1
+#define VENC_SYNCCTL_SYEV                       (1 << 1)
+#define VENC_SYNCCTL_SYEH_SHIFT                 0
+#define VENC_SYNCCTL_SYEH                       (1 << 0)
+#define VENC_SYNCCTL_OVD_SHIFT                  14
+#define VENC_SYNCCTL_OVD                        (1 << 14)
+
+#define VENC_DCLKCTL_DCKEC_SHIFT                11
+#define VENC_DCLKCTL_DCKEC                      (1 << 11)
+#define VENC_DCLKCTL_DCKPW_SHIFT                0
+#define VENC_DCLKCTL_DCKPW                      (0x3f << 0)
+
+#define VENC_VSTAT_FIDST                        (1 << 4)
+
+#define VENC_CMPNT_MRGB_SHIFT                   14
+#define VENC_CMPNT_MRGB                         (1 << 14)
+
+#endif                          /* _VPBE_VENC_REGS_H */
diff --git a/drivers/media/video/davinci/vpif_capture.c b/drivers/media/video/davinci/vpif_capture.c
index d93ad74a34c..49e4deb5004 100644
--- a/drivers/media/video/davinci/vpif_capture.c
+++ b/drivers/media/video/davinci/vpif_capture.c
@@ -33,7 +33,6 @@
 #include <linux/i2c.h>
 #include <linux/platform_device.h>
 #include <linux/io.h>
-#include <linux/version.h>
 #include <linux/slab.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
@@ -44,6 +43,7 @@
 
 MODULE_DESCRIPTION("TI DaVinci VPIF Capture driver");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(VPIF_CAPTURE_VERSION);
 
 #define vpif_err(fmt, arg...)   v4l2_err(&vpif_obj.v4l2_dev, fmt, ## arg)
 #define vpif_dbg(level, debug, fmt, arg...)     \
@@ -1677,7 +1677,6 @@ static int vpif_querycap(struct file *file, void  *priv,
 {
         struct vpif_capture_config *config = vpif_dev->platform_data;
 
-        cap->version = VPIF_CAPTURE_VERSION_CODE;
         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
         strlcpy(cap->driver, "vpif capture", sizeof(cap->driver));
         strlcpy(cap->bus_info, "DM646x Platform", sizeof(cap->bus_info));
@@ -2211,10 +2210,8 @@ static __init int vpif_probe(struct platform_device *pdev)
                 vfd->v4l2_dev = &vpif_obj.v4l2_dev;
                 vfd->release = video_device_release;
                 snprintf(vfd->name, sizeof(vfd->name),
-                         "DM646x_VPIFCapture_DRIVER_V%d.%d.%d",
-                         (VPIF_CAPTURE_VERSION_CODE >> 16) & 0xff,
-                         (VPIF_CAPTURE_VERSION_CODE >> 8) & 0xff,
-                         (VPIF_CAPTURE_VERSION_CODE) & 0xff);
+                         "DM646x_VPIFCapture_DRIVER_V%s",
+                         VPIF_CAPTURE_VERSION);
                 /* Set video_dev to the video device */
                 ch->video_dev = vfd;
         }
diff --git a/drivers/media/video/davinci/vpif_capture.h b/drivers/media/video/davinci/vpif_capture.h
index 7a4196dfdce..064550f5ce4 100644
--- a/drivers/media/video/davinci/vpif_capture.h
+++ b/drivers/media/video/davinci/vpif_capture.h
@@ -23,7 +23,6 @@
 
 /* Header files */
 #include <linux/videodev2.h>
-#include <linux/version.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
 #include <media/videobuf-core.h>
@@ -33,11 +32,7 @@
 #include "vpif.h"
 
 /* Macros */
-#define VPIF_MAJOR_RELEASE              0
-#define VPIF_MINOR_RELEASE              0
-#define VPIF_BUILD                      1
-#define VPIF_CAPTURE_VERSION_CODE       ((VPIF_MAJOR_RELEASE << 16) | \
-        (VPIF_MINOR_RELEASE << 8) | VPIF_BUILD)
+#define VPIF_CAPTURE_VERSION            "0.0.2"
 
 #define VPIF_VALID_FIELD(field)         (((V4L2_FIELD_ANY == field) || \
         (V4L2_FIELD_NONE == field)) || \
diff --git a/drivers/media/video/davinci/vpif_display.c b/drivers/media/video/davinci/vpif_display.c
index cdf659abdc2..286f0291004 100644
--- a/drivers/media/video/davinci/vpif_display.c
+++ b/drivers/media/video/davinci/vpif_display.c
@@ -29,7 +29,6 @@
 #include <linux/i2c.h>
 #include <linux/platform_device.h>
 #include <linux/io.h>
-#include <linux/version.h>
 #include <linux/slab.h>
 
 #include <asm/irq.h>
@@ -47,6 +46,7 @@
 
 MODULE_DESCRIPTION("TI DaVinci VPIF Display driver");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(VPIF_DISPLAY_VERSION);
 
 #define DM646X_V4L2_STD (V4L2_STD_525_60 | V4L2_STD_625_50)
 
@@ -701,7 +701,6 @@ static int vpif_querycap(struct file *file, void  *priv,
 {
         struct vpif_display_config *config = vpif_dev->platform_data;
 
-        cap->version = VPIF_DISPLAY_VERSION_CODE;
         cap->capabilities = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
         strlcpy(cap->driver, "vpif display", sizeof(cap->driver));
         strlcpy(cap->bus_info, "Platform", sizeof(cap->bus_info));
@@ -1740,10 +1739,8 @@ static __init int vpif_probe(struct platform_device *pdev)
                 vfd->v4l2_dev = &vpif_obj.v4l2_dev;
                 vfd->release = video_device_release;
                 snprintf(vfd->name, sizeof(vfd->name),
-                         "DM646x_VPIFDisplay_DRIVER_V%d.%d.%d",
-                         (VPIF_DISPLAY_VERSION_CODE >> 16) & 0xff,
-                         (VPIF_DISPLAY_VERSION_CODE >> 8) & 0xff,
-                         (VPIF_DISPLAY_VERSION_CODE) & 0xff);
+                         "DM646x_VPIFDisplay_DRIVER_V%s",
+                         VPIF_DISPLAY_VERSION);
 
                 /* Set video_dev to the video device */
                 ch->video_dev = vfd;
diff --git a/drivers/media/video/davinci/vpif_display.h b/drivers/media/video/davinci/vpif_display.h
index b53aaa88307..5d1936dafed 100644
--- a/drivers/media/video/davinci/vpif_display.h
+++ b/drivers/media/video/davinci/vpif_display.h
@@ -18,7 +18,6 @@
 
 /* Header files */
 #include <linux/videodev2.h>
-#include <linux/version.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
 #include <media/videobuf-core.h>
@@ -27,12 +26,7 @@
 #include "vpif.h"
 
 /* Macros */
-#define VPIF_MAJOR_RELEASE      (0)
-#define VPIF_MINOR_RELEASE      (0)
-#define VPIF_BUILD              (1)
-
-#define VPIF_DISPLAY_VERSION_CODE \
-        ((VPIF_MAJOR_RELEASE << 16) | (VPIF_MINOR_RELEASE << 8) | VPIF_BUILD)
+#define VPIF_DISPLAY_VERSION    "0.0.2"
 
 #define VPIF_VALID_FIELD(field) \
         (((V4L2_FIELD_ANY == field) || (V4L2_FIELD_NONE == field)) || \
diff --git a/drivers/media/video/em28xx/Kconfig b/drivers/media/video/em28xx/Kconfig
index 3cb78f26df9..281ee427c2a 100644
--- a/drivers/media/video/em28xx/Kconfig
+++ b/drivers/media/video/em28xx/Kconfig
@@ -3,7 +3,6 @@ config VIDEO_EM28XX
         depends on VIDEO_DEV && I2C
         select VIDEO_TUNER
         select VIDEO_TVEEPROM
-        depends on RC_CORE
         select VIDEOBUF_VMALLOC
         select VIDEO_SAA711X if VIDEO_HELPER_CHIPS_AUTO
         select VIDEO_TVP5150 if VIDEO_HELPER_CHIPS_AUTO
@@ -40,7 +39,18 @@ config VIDEO_EM28XX_DVB
         select DVB_S921 if !DVB_FE_CUSTOMISE
         select DVB_DRXD if !DVB_FE_CUSTOMISE
         select DVB_CXD2820R if !DVB_FE_CUSTOMISE
+        select DVB_DRXK if !DVB_FE_CUSTOMISE
+        select DVB_TDA18271C2DD if !DVB_FE_CUSTOMISE
         select VIDEOBUF_DVB
         ---help---
           This adds support for DVB cards based on the
           Empiatech em28xx chips.
+
+config VIDEO_EM28XX_RC
+        bool "EM28XX Remote Controller support"
+        depends on RC_CORE
+        depends on VIDEO_EM28XX
+        depends on !(RC_CORE=m && VIDEO_EM28XX=y)
+        default y
+        ---help---
+          Enables Remote Controller support on em28xx driver.
diff --git a/drivers/media/video/em28xx/Makefile b/drivers/media/video/em28xx/Makefile
index d0f093d1d0d..38aaa004f57 100644
--- a/drivers/media/video/em28xx/Makefile
+++ b/drivers/media/video/em28xx/Makefile
@@ -1,5 +1,7 @@
-em28xx-objs     := em28xx-video.o em28xx-i2c.o em28xx-cards.o em28xx-core.o \
-                   em28xx-input.o em28xx-vbi.o
+em28xx-y :=     em28xx-video.o em28xx-i2c.o em28xx-cards.o
+em28xx-y +=     em28xx-core.o  em28xx-vbi.o
+
+em28xx-$(CONFIG_VIDEO_EM28XX_RC) += em28xx-input.o
 
 em28xx-alsa-objs := em28xx-audio.o
 
diff --git a/drivers/media/video/em28xx/em28xx-audio.c b/drivers/media/video/em28xx/em28xx-audio.c
index 3c48a72eb7d..cff0768afbf 100644
--- a/drivers/media/video/em28xx/em28xx-audio.c
+++ b/drivers/media/video/em28xx/em28xx-audio.c
@@ -3,9 +3,9 @@
  *
  *  Copyright (C) 2006 Markus Rechberger <mrechberger@gmail.com>
  *
- *  Copyright (C) 2007 Mauro Carvalho Chehab <mchehab@infradead.org>
+ *  Copyright (C) 2007-2011 Mauro Carvalho Chehab <mchehab@redhat.com>
  *      - Port to work with the in-kernel driver
- *      - Several cleanups
+ *      - Cleanups, fixes, alsa-controls, etc.
  *
  *  This driver is based on my previous au600 usb pstn audio driver
  *  and inherits all the copyrights
@@ -41,6 +41,7 @@
 #include <sound/info.h>
 #include <sound/initval.h>
 #include <sound/control.h>
+#include <sound/tlv.h>
 #include <media/v4l2-common.h>
 #include "em28xx.h"
 
@@ -212,9 +213,12 @@ static int em28xx_init_audio_isoc(struct em28xx *dev)
         for (i = 0; i < EM28XX_AUDIO_BUFS; i++) {
                 errCode = usb_submit_urb(dev->adev.urb[i], GFP_ATOMIC);
                 if (errCode) {
+                        em28xx_errdev("submit of audio urb failed\n");
                         em28xx_deinit_isoc_audio(dev);
+                        atomic_set(&dev->stream_started, 0);
                         return errCode;
                 }
+
         }
 
         return 0;
@@ -245,6 +249,7 @@ static struct snd_pcm_hardware snd_em28xx_hw_capture = {
         .info = SNDRV_PCM_INFO_BLOCK_TRANSFER |
                 SNDRV_PCM_INFO_MMAP           |
                 SNDRV_PCM_INFO_INTERLEAVED    |
+                SNDRV_PCM_INFO_BATCH          |
                 SNDRV_PCM_INFO_MMAP_VALID,
 
         .formats = SNDRV_PCM_FMTBIT_S16_LE,
@@ -276,24 +281,27 @@ static int snd_em28xx_capture_open(struct snd_pcm_substream *substream)
                 return -ENODEV;
         }
 
-        /* Sets volume, mute, etc */
+        runtime->hw = snd_em28xx_hw_capture;
+        if ((dev->alt == 0 || dev->audio_ifnum) && dev->adev.users == 0) {
+                if (dev->audio_ifnum)
+                        dev->alt = 1;
+                else
+                        dev->alt = 7;
 
-        dev->mute = 0;
-        mutex_lock(&dev->lock);
-        ret = em28xx_audio_analog_set(dev);
-        if (ret < 0)
-                goto err;
+                dprintk("changing alternate number on interface %d to %d\n",
+                        dev->audio_ifnum, dev->alt);
+                usb_set_interface(dev->udev, dev->audio_ifnum, dev->alt);
 
-        runtime->hw = snd_em28xx_hw_capture;
-        if (dev->alt == 0 && dev->adev.users == 0) {
-                int errCode;
-                dev->alt = 7;
-                dprintk("changing alternate number to 7\n");
-                errCode = usb_set_interface(dev->udev, 0, 7);
-        }
+                /* Sets volume, mute, etc */
+                dev->mute = 0;
+                mutex_lock(&dev->lock);
+                ret = em28xx_audio_analog_set(dev);
+                if (ret < 0)
+                        goto err;
 
-        dev->adev.users++;
-        mutex_unlock(&dev->lock);
+                dev->adev.users++;
+                mutex_unlock(&dev->lock);
+        }
 
         snd_pcm_hw_constraint_integer(runtime, SNDRV_PCM_HW_PARAM_PERIODS);
         dev->adev.capture_pcm_substream = substream;
@@ -342,6 +350,8 @@ static int snd_em28xx_hw_capture_params(struct snd_pcm_substream *substream,
 
         ret = snd_pcm_alloc_vmalloc_buffer(substream,
                                 params_buffer_bytes(hw_params));
+        if (ret < 0)
+                return ret;
         format = params_format(hw_params);
         rate = params_rate(hw_params);
         channels = params_channels(hw_params);
@@ -393,20 +403,24 @@ static int snd_em28xx_capture_trigger(struct snd_pcm_substream *substream,
                                       int cmd)
 {
         struct em28xx *dev = snd_pcm_substream_chip(substream);
-        int retval;
+        int retval = 0;
 
         switch (cmd) {
+        case SNDRV_PCM_TRIGGER_PAUSE_RELEASE: /* fall through */
+        case SNDRV_PCM_TRIGGER_RESUME: /* fall through */
         case SNDRV_PCM_TRIGGER_START:
                 atomic_set(&dev->stream_started, 1);
                 break;
+        case SNDRV_PCM_TRIGGER_PAUSE_PUSH: /* fall through */
+        case SNDRV_PCM_TRIGGER_SUSPEND: /* fall through */
         case SNDRV_PCM_TRIGGER_STOP:
-                atomic_set(&dev->stream_started, 1);
+                atomic_set(&dev->stream_started, 0);
                 break;
         default:
                 retval = -EINVAL;
         }
         schedule_work(&dev->wq_trigger);
-        return 0;
+        return retval;
 }
 
 static snd_pcm_uframes_t snd_em28xx_capture_pointer(struct snd_pcm_substream
@@ -432,6 +446,179 @@ static struct page *snd_pcm_get_vmalloc_page(struct snd_pcm_substream *subs,
         return vmalloc_to_page(pageptr);
 }
 
+/*
+ * AC97 volume control support
+ */
+static int em28xx_vol_info(struct snd_kcontrol *kcontrol,
+                                struct snd_ctl_elem_info *info)
+{
+        info->type = SNDRV_CTL_ELEM_TYPE_INTEGER;
+        info->count = 2;
+        info->value.integer.min = 0;
+        info->value.integer.max = 0x1f;
+
+        return 0;
+}
+
+static int em28xx_vol_put(struct snd_kcontrol *kcontrol,
+                               struct snd_ctl_elem_value *value)
+{
+        struct em28xx *dev = snd_kcontrol_chip(kcontrol);
+        u16 val = (0x1f - (value->value.integer.value[0] & 0x1f)) |
+                  (0x1f - (value->value.integer.value[1] & 0x1f)) << 8;
+        int rc;
+
+        mutex_lock(&dev->lock);
+        rc = em28xx_read_ac97(dev, kcontrol->private_value);
+        if (rc < 0)
+                goto err;
+
+        val |= rc & 0x8000;     /* Preserve the mute flag */
+
+        rc = em28xx_write_ac97(dev, kcontrol->private_value, val);
+        if (rc < 0)
+                goto err;
+
+        dprintk("%sleft vol %d, right vol %d (0x%04x) to ac97 volume control 0x%04x\n",
+                (val & 0x8000) ? "muted " : "",
+                0x1f - ((val >> 8) & 0x1f), 0x1f - (val & 0x1f),
+                val, (int)kcontrol->private_value);
+
+err:
+        mutex_unlock(&dev->lock);
+        return rc;
+}
+
+static int em28xx_vol_get(struct snd_kcontrol *kcontrol,
+                               struct snd_ctl_elem_value *value)
+{
+        struct em28xx *dev = snd_kcontrol_chip(kcontrol);
+        int val;
+
+        mutex_lock(&dev->lock);
+        val = em28xx_read_ac97(dev, kcontrol->private_value);
+        mutex_unlock(&dev->lock);
+        if (val < 0)
+                return val;
+
+        dprintk("%sleft vol %d, right vol %d (0x%04x) from ac97 volume control 0x%04x\n",
+                (val & 0x8000) ? "muted " : "",
+                0x1f - ((val >> 8) & 0x1f), 0x1f - (val & 0x1f),
+                val, (int)kcontrol->private_value);
+
+        value->value.integer.value[0] = 0x1f - (val & 0x1f);
+        value->value.integer.value[1] = 0x1f - ((val << 8) & 0x1f);
+
+        return 0;
+}
+
+static int em28xx_vol_put_mute(struct snd_kcontrol *kcontrol,
+                               struct snd_ctl_elem_value *value)
+{
+        struct em28xx *dev = snd_kcontrol_chip(kcontrol);
+        u16 val = value->value.integer.value[0];
+        int rc;
+
+        mutex_lock(&dev->lock);
+        rc = em28xx_read_ac97(dev, kcontrol->private_value);
+        if (rc < 0)
+                goto err;
+
+        if (val)
+                rc &= 0x1f1f;
+        else
+                rc |= 0x8000;
+
+        rc = em28xx_write_ac97(dev, kcontrol->private_value, rc);
+        if (rc < 0)
+                goto err;
+
+        dprintk("%sleft vol %d, right vol %d (0x%04x) to ac97 volume control 0x%04x\n",
+                (val & 0x8000) ? "muted " : "",
+                0x1f - ((val >> 8) & 0x1f), 0x1f - (val & 0x1f),
+                val, (int)kcontrol->private_value);
+
+err:
+        mutex_unlock(&dev->lock);
+        return rc;
+}
+
+static int em28xx_vol_get_mute(struct snd_kcontrol *kcontrol,
+                               struct snd_ctl_elem_value *value)
+{
+        struct em28xx *dev = snd_kcontrol_chip(kcontrol);
+        int val;
+
+        mutex_lock(&dev->lock);
+        val = em28xx_read_ac97(dev, kcontrol->private_value);
+        mutex_unlock(&dev->lock);
+        if (val < 0)
+                return val;
+
+        if (val & 0x8000)
+                value->value.integer.value[0] = 0;
+        else
+                value->value.integer.value[0] = 1;
+
+        dprintk("%sleft vol %d, right vol %d (0x%04x) from ac97 volume control 0x%04x\n",
+                (val & 0x8000) ? "muted " : "",
+                0x1f - ((val >> 8) & 0x1f), 0x1f - (val & 0x1f),
+                val, (int)kcontrol->private_value);
+
+        return 0;
+}
+
+static const DECLARE_TLV_DB_SCALE(em28xx_db_scale, -3450, 150, 0);
+
+static int em28xx_cvol_new(struct snd_card *card, struct em28xx *dev,
+                           char *name, int id)
+{
+        int err;
+        char ctl_name[44];
+        struct snd_kcontrol *kctl;
+        struct snd_kcontrol_new tmp;
+
+        memset (&tmp, 0, sizeof(tmp));
+        tmp.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+        tmp.private_value = id,
+        tmp.name  = ctl_name,
+
+        /* Add Mute Control */
+        sprintf(ctl_name, "%s Switch", name);
+        tmp.get  = em28xx_vol_get_mute;
+        tmp.put  = em28xx_vol_put_mute;
+        tmp.info = snd_ctl_boolean_mono_info;
+        kctl = snd_ctl_new1(&tmp, dev);
+        err = snd_ctl_add(card, kctl);
+        if (err < 0)
+                return err;
+        dprintk("Added control %s for ac97 volume control 0x%04x\n",
+                ctl_name, id);
+
+        memset (&tmp, 0, sizeof(tmp));
+        tmp.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+        tmp.private_value = id,
+        tmp.name  = ctl_name,
+
+        /* Add Volume Control */
+        sprintf(ctl_name, "%s Volume", name);
+        tmp.get   = em28xx_vol_get;
+        tmp.put   = em28xx_vol_put;
+        tmp.info  = em28xx_vol_info;
+        tmp.tlv.p = em28xx_db_scale,
+        kctl = snd_ctl_new1(&tmp, dev);
+        err = snd_ctl_add(card, kctl);
+        if (err < 0)
+                return err;
+        dprintk("Added control %s for ac97 volume control 0x%04x\n",
+                ctl_name, id);
+
+        return 0;
+}
+
+/*
+ * register/unregister code and data
+ */
 static struct snd_pcm_ops snd_em28xx_pcm_capture = {
         .open      = snd_em28xx_capture_open,
         .close     = snd_em28xx_pcm_close,
@@ -452,17 +639,17 @@ static int em28xx_audio_init(struct em28xx *dev)
         static int          devnr;
         int                 err;
 
-        if (dev->has_alsa_audio != 1) {
+        if (!dev->has_alsa_audio || dev->audio_ifnum < 0) {
                 /* This device does not support the extension (in this case
                    the device is expecting the snd-usb-audio module or
                    doesn't have analog audio support at all) */
                 return 0;
         }
 
-        printk(KERN_INFO "em28xx-audio.c: probing for em28x1 "
-                         "non standard usbaudio\n");
+        printk(KERN_INFO "em28xx-audio.c: probing for em28xx Audio Vendor Class\n");
         printk(KERN_INFO "em28xx-audio.c: Copyright (C) 2006 Markus "
                          "Rechberger\n");
+        printk(KERN_INFO "em28xx-audio.c: Copyright (C) 2007-2011 Mauro Carvalho Chehab\n");
 
         err = snd_card_create(index[devnr], "Em28xx Audio", THIS_MODULE, 0,
                               &card);
@@ -488,6 +675,22 @@ static int em28xx_audio_init(struct em28xx *dev)
 
         INIT_WORK(&dev->wq_trigger, audio_trigger);
 
+        if (dev->audio_mode.ac97 != EM28XX_NO_AC97) {
+                em28xx_cvol_new(card, dev, "Video", AC97_VIDEO_VOL);
+                em28xx_cvol_new(card, dev, "Line In", AC97_LINEIN_VOL);
+                em28xx_cvol_new(card, dev, "Phone", AC97_PHONE_VOL);
+                em28xx_cvol_new(card, dev, "Microphone", AC97_PHONE_VOL);
+                em28xx_cvol_new(card, dev, "CD", AC97_CD_VOL);
+                em28xx_cvol_new(card, dev, "AUX", AC97_AUX_VOL);
+                em28xx_cvol_new(card, dev, "PCM", AC97_PCM_OUT_VOL);
+
+                em28xx_cvol_new(card, dev, "Master", AC97_MASTER_VOL);
+                em28xx_cvol_new(card, dev, "Line", AC97_LINE_LEVEL_VOL);
+                em28xx_cvol_new(card, dev, "Mono", AC97_MASTER_MONO_VOL);
+                em28xx_cvol_new(card, dev, "LFE", AC97_LFE_MASTER_VOL);
+                em28xx_cvol_new(card, dev, "Surround", AC97_SURR_MASTER_VOL);
+        }
+
         err = snd_card_register(card);
         if (err < 0) {
                 snd_card_free(card);
@@ -538,7 +741,7 @@ static void __exit em28xx_alsa_unregister(void)
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Markus Rechberger <mrechberger@gmail.com>");
-MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@infradead.org>");
+MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
 MODULE_DESCRIPTION("Em28xx Audio driver");
 
 module_init(em28xx_alsa_register);
diff --git a/drivers/media/video/em28xx/em28xx-cards.c b/drivers/media/video/em28xx/em28xx-cards.c
index 4e37375decf..3e3959fee41 100644
--- a/drivers/media/video/em28xx/em28xx-cards.c
+++ b/drivers/media/video/em28xx/em28xx-cards.c
@@ -289,7 +289,7 @@ static struct em28xx_reg_seq leadership_reset[] = {
         {       -1,             -1,     -1,     -1},
 };
 
-/* 2013:024f PCTV Systems nanoStick T2 290e
+/* 2013:024f PCTV nanoStick T2 290e
  * GPIO_6 - demod reset
  * GPIO_7 - LED
  */
@@ -300,6 +300,23 @@ static struct em28xx_reg_seq pctv_290e[] = {
         {-1,                    -1,     -1,             -1},
 };
 
+#if 0
+static struct em28xx_reg_seq terratec_h5_gpio[] = {
+        {EM28XX_R08_GPIO,       0xff,   0xff,   10},
+        {EM2874_R80_GPIO,       0xf6,   0xff,   100},
+        {EM2874_R80_GPIO,       0xf2,   0xff,   50},
+        {EM2874_R80_GPIO,       0xf6,   0xff,   50},
+        { -1,                   -1,     -1,     -1},
+};
+
+static struct em28xx_reg_seq terratec_h5_digital[] = {
+        {EM2874_R80_GPIO,       0xf6,   0xff,   10},
+        {EM2874_R80_GPIO,       0xe6,   0xff,   100},
+        {EM2874_R80_GPIO,       0xa6,   0xff,   10},
+        { -1,                   -1,     -1,     -1},
+};
+#endif
+
 /*
  *  Board definitions
  */
@@ -843,6 +860,19 @@ struct em28xx_board em28xx_boards[] = {
                         .gpio     = terratec_cinergy_USB_XS_FR_analog,
                 } },
         },
+        [EM2884_BOARD_TERRATEC_H5] = {
+                .name         = "Terratec Cinergy H5",
+                .has_dvb      = 1,
+#if 0
+                .tuner_type   = TUNER_PHILIPS_TDA8290,
+                .tuner_addr   = 0x41,
+                .dvb_gpio     = terratec_h5_digital, /* FIXME: probably wrong */
+                .tuner_gpio   = terratec_h5_gpio,
+#endif
+                .i2c_speed    = EM2874_I2C_SECONDARY_BUS_SELECT |
+                                EM28XX_I2C_CLK_WAIT_ENABLE |
+                                EM28XX_I2C_FREQ_400_KHZ,
+        },
         [EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900] = {
                 .name         = "Hauppauge WinTV HVR 900",
                 .tda9887_conf = TDA9887_PRESENT,
@@ -1259,7 +1289,7 @@ struct em28xx_board em28xx_boards[] = {
                 } },
         },
 
-        [EM2874_LEADERSHIP_ISDBT] = {
+        [EM2874_BOARD_LEADERSHIP_ISDBT] = {
                 .i2c_speed      = EM2874_I2C_SECONDARY_BUS_SELECT |
                                   EM28XX_I2C_CLK_WAIT_ENABLE |
                                   EM28XX_I2C_FREQ_100_KHZ,
@@ -1319,7 +1349,6 @@ struct em28xx_board em28xx_boards[] = {
         },
         [EM2880_BOARD_KWORLD_DVB_305U] = {
                 .name         = "KWorld DVB-T 305U",
-                .valid        = EM28XX_BOARD_NOT_VALIDATED,
                 .tuner_type   = TUNER_XC2028,
                 .tuner_gpio   = default_tuner_gpio,
                 .decoder      = EM28XX_TVP5150,
@@ -1770,16 +1799,16 @@ struct em28xx_board em28xx_boards[] = {
                 .dvb_gpio   = kworld_a340_digital,
                 .tuner_gpio = default_tuner_gpio,
         },
-        /* 2013:024f PCTV Systems nanoStick T2 290e.
+        /* 2013:024f PCTV nanoStick T2 290e.
          * Empia EM28174, Sony CXD2820R and NXP TDA18271HD/C2 */
         [EM28174_BOARD_PCTV_290E] = {
+                .name          = "PCTV nanoStick T2 290e",
                 .i2c_speed      = EM2874_I2C_SECONDARY_BUS_SELECT |
                         EM28XX_I2C_CLK_WAIT_ENABLE | EM28XX_I2C_FREQ_100_KHZ,
-                .xclk          = EM28XX_XCLK_FREQUENCY_12MHZ,
-                .name          = "PCTV Systems nanoStick T2 290e",
                 .tuner_type    = TUNER_ABSENT,
                 .tuner_gpio    = pctv_290e,
                 .has_dvb       = 1,
+                .ir_codes      = RC_MAP_PINNACLE_PCTV_HD,
         },
 };
 const unsigned int em28xx_bcount = ARRAY_SIZE(em28xx_boards);
@@ -1855,8 +1884,10 @@ struct usb_device_id em28xx_id_table[] = {
         { USB_DEVICE(0x0ccd, 0x0042),
                         .driver_info = EM2882_BOARD_TERRATEC_HYBRID_XS },
         { USB_DEVICE(0x0ccd, 0x0043),
-                        .driver_info = EM2870_BOARD_TERRATEC_XS },
-        { USB_DEVICE(0x0ccd, 0x0047),
+                        .driver_info = EM2884_BOARD_TERRATEC_H5 },
+        { USB_DEVICE(0x0ccd, 0x10a2),   /* Rev. 1 */
+                        .driver_info = EM2884_BOARD_TERRATEC_H5 },
+        { USB_DEVICE(0x0ccd, 0x10ad),   /* Rev. 2 */
                         .driver_info = EM2880_BOARD_TERRATEC_PRODIGY_XS },
         { USB_DEVICE(0x0ccd, 0x0084),
                         .driver_info = EM2860_BOARD_TERRATEC_AV350 },
@@ -1937,7 +1968,7 @@ static struct em28xx_hash_table em28xx_i2c_hash[] = {
         {0x77800080, EM2860_BOARD_TVP5150_REFERENCE_DESIGN, TUNER_ABSENT},
         {0xc51200e3, EM2820_BOARD_GADMEI_TVR200, TUNER_LG_PAL_NEW_TAPC},
         {0x4ba50080, EM2861_BOARD_GADMEI_UTV330PLUS, TUNER_TNF_5335MF},
-        {0x6b800080, EM2874_LEADERSHIP_ISDBT, TUNER_ABSENT},
+        {0x6b800080, EM2874_BOARD_LEADERSHIP_ISDBT, TUNER_ABSENT},
 };
 
 /* I2C possible address to saa7115, tvp5150, msp3400, tvaudio */
@@ -2660,10 +2691,9 @@ void em28xx_card_setup(struct em28xx *dev)
                         .addr = 0xba >> 1,
                         .platform_data = &pdata,
                 };
-                struct v4l2_subdev *sd;
 
                 pdata.xtal = dev->sensor_xtal;
-                sd = v4l2_i2c_new_subdev_board(&dev->v4l2_dev, &dev->i2c_adap,
+                v4l2_i2c_new_subdev_board(&dev->v4l2_dev, &dev->i2c_adap,
                                 &mt9v011_info, NULL);
         }
 
@@ -2842,11 +2872,26 @@ static int em28xx_init_dev(struct em28xx **devhandle, struct usb_device *udev,
                         em28xx_info("chip ID is em2882/em2883\n");
                         dev->wait_after_write = 0;
                         break;
+                case CHIP_ID_EM2884:
+                        em28xx_info("chip ID is em2884\n");
+                        dev->reg_gpio_num = EM2874_R80_GPIO;
+                        dev->wait_after_write = 0;
+                        break;
                 default:
                         em28xx_info("em28xx chip ID = %d\n", dev->chip_id);
                 }
         }
 
+        if (dev->is_audio_only) {
+                errCode = em28xx_audio_setup(dev);
+                if (errCode)
+                        return -ENODEV;
+                em28xx_add_into_devlist(dev);
+                em28xx_init_extension(dev);
+
+                return 0;
+        }
+
         /* Prepopulate cached GPO register content */
         retval = em28xx_read_reg(dev, dev->reg_gpo_num);
         if (retval >= 0)
@@ -2947,6 +2992,9 @@ fail_reg_devices:
         return retval;
 }
 
+/* high bandwidth multiplier, as encoded in highspeed endpoint descriptors */
+#define hb_mult(wMaxPacketSize) (1 + (((wMaxPacketSize) >> 11) & 0x03))
+
 /*
  * em28xx_usb_probe()
  * checks for supported devices
@@ -2956,15 +3004,15 @@ static int em28xx_usb_probe(struct usb_interface *interface,
 {
         const struct usb_endpoint_descriptor *endpoint;
         struct usb_device *udev;
-        struct usb_interface *uif;
         struct em28xx *dev = NULL;
         int retval;
-        int i, nr, ifnum, isoc_pipe;
+        bool is_audio_only = false, has_audio = false;
+        int i, nr, isoc_pipe;
+        const int ifnum = interface->altsetting[0].desc.bInterfaceNumber;
         char *speed;
         char descr[255] = "";
 
         udev = usb_get_dev(interface_to_usbdev(interface));
-        ifnum = interface->altsetting[0].desc.bInterfaceNumber;
 
         /* Check to see next free device and mark as used */
         nr = find_first_zero_bit(&em28xx_devused, EM28XX_MAXBOARDS);
@@ -2984,6 +3032,19 @@ static int em28xx_usb_probe(struct usb_interface *interface,
                 goto err;
         }
 
+        /* Get endpoints */
+        for (i = 0; i < interface->num_altsetting; i++) {
+                int ep;
+
+                for (ep = 0; ep < interface->altsetting[i].desc.bNumEndpoints; ep++) {
+                        struct usb_host_endpoint        *e;
+                        e = &interface->altsetting[i].endpoint[ep];
+
+                        if (e->desc.bEndpointAddress == 0x83)
+                                has_audio = true;
+                }
+        }
+
         endpoint = &interface->cur_altsetting->endpoint[0].desc;
 
         /* check if the device has the iso in endpoint at the correct place */
@@ -3003,19 +3064,22 @@ static int em28xx_usb_probe(struct usb_interface *interface,
                         check_interface = 0;
 
                 if (!check_interface) {
-                        em28xx_err(DRIVER_NAME " video device (%04x:%04x): "
-                                "interface %i, class %i found.\n",
-                                le16_to_cpu(udev->descriptor.idVendor),
-                                le16_to_cpu(udev->descriptor.idProduct),
-                                ifnum,
-                                interface->altsetting[0].desc.bInterfaceClass);
-
-                        em28xx_err(DRIVER_NAME " This is an anciliary "
-                                "interface not used by the driver\n");
-
-                        em28xx_devused &= ~(1<<nr);
-                        retval = -ENODEV;
-                        goto err;
+                        if (has_audio) {
+                                is_audio_only = true;
+                        } else {
+                                em28xx_err(DRIVER_NAME " video device (%04x:%04x): "
+                                        "interface %i, class %i found.\n",
+                                        le16_to_cpu(udev->descriptor.idVendor),
+                                        le16_to_cpu(udev->descriptor.idProduct),
+                                        ifnum,
+                                        interface->altsetting[0].desc.bInterfaceClass);
+                                em28xx_err(DRIVER_NAME " This is an anciliary "
+                                        "interface not used by the driver\n");
+
+                                em28xx_devused &= ~(1<<nr);
+                                retval = -ENODEV;
+                                goto err;
+                        }
                 }
         }
 
@@ -3045,8 +3109,8 @@ static int em28xx_usb_probe(struct usb_interface *interface,
         if (*descr)
                 strlcat(descr, " ", sizeof(descr));
 
-        printk(DRIVER_NAME ": New device %s@ %s Mbps "
-                "(%04x:%04x, interface %d, class %d)\n",
+        printk(KERN_INFO DRIVER_NAME
+                ": New device %s@ %s Mbps (%04x:%04x, interface %d, class %d)\n",
                 descr,
                 speed,
                 le16_to_cpu(udev->descriptor.idVendor),
@@ -3054,6 +3118,11 @@ static int em28xx_usb_probe(struct usb_interface *interface,
                 ifnum,
                 interface->altsetting->desc.bInterfaceNumber);
 
+        if (has_audio)
+                printk(KERN_INFO DRIVER_NAME
+                       ": Audio Vendor Class interface %i found\n",
+                       ifnum);
+
         /*
          * Make sure we have 480 Mbps of bandwidth, otherwise things like
          * video stream wouldn't likely work, since 12 Mbps is generally
@@ -3089,10 +3158,13 @@ static int em28xx_usb_probe(struct usb_interface *interface,
         dev->devno = nr;
         dev->model = id->driver_info;
         dev->alt   = -1;
+        dev->is_audio_only = is_audio_only;
+        dev->has_alsa_audio = has_audio;
+        dev->audio_ifnum = ifnum;
 
         /* Checks if audio is provided by some interface */
         for (i = 0; i < udev->config->desc.bNumInterfaces; i++) {
-                uif = udev->config->interface[i];
+                struct usb_interface *uif = udev->config->interface[i];
                 if (uif->altsetting[0].desc.bInterfaceClass == USB_CLASS_AUDIO) {
                         dev->has_audio_class = 1;
                         break;
@@ -3100,9 +3172,7 @@ static int em28xx_usb_probe(struct usb_interface *interface,
         }
 
         /* compute alternate max packet sizes */
-        uif = udev->actconfig->interface[0];
-
-        dev->num_alt = uif->num_altsetting;
+        dev->num_alt = interface->num_altsetting;
         dev->alt_max_pkt_size = kmalloc(32 * dev->num_alt, GFP_KERNEL);
 
         if (dev->alt_max_pkt_size == NULL) {
@@ -3114,14 +3184,21 @@ static int em28xx_usb_probe(struct usb_interface *interface,
         }
 
         for (i = 0; i < dev->num_alt ; i++) {
-                u16 tmp = le16_to_cpu(uif->altsetting[i].endpoint[isoc_pipe].desc.wMaxPacketSize);
-                dev->alt_max_pkt_size[i] =
-                    (tmp & 0x07ff) * (((tmp & 0x1800) >> 11) + 1);
+                u16 tmp = le16_to_cpu(interface->altsetting[i].endpoint[isoc_pipe].desc.wMaxPacketSize);
+                unsigned int size = tmp & 0x7ff;
+
+                if (udev->speed == USB_SPEED_HIGH)
+                        size = size * hb_mult(tmp);
+
+                dev->alt_max_pkt_size[i] = size;
         }
 
         if ((card[nr] >= 0) && (card[nr] < em28xx_bcount))
                 dev->model = card[nr];
 
+        /* save our data pointer in this interface device */
+        usb_set_intfdata(interface, dev);
+
         /* allocate device struct */
         mutex_init(&dev->lock);
         mutex_lock(&dev->lock);
@@ -3133,9 +3210,6 @@ static int em28xx_usb_probe(struct usb_interface *interface,
                 goto err;
         }
 
-        /* save our data pointer in this interface device */
-        usb_set_intfdata(interface, dev);
-
         request_modules(dev);
 
         /* Should be the last thing to do, to avoid newer udev's to
@@ -3164,6 +3238,13 @@ static void em28xx_usb_disconnect(struct usb_interface *interface)
         if (!dev)
                 return;
 
+        if (dev->is_audio_only) {
+                mutex_lock(&dev->lock);
+                em28xx_close_extension(dev);
+                mutex_unlock(&dev->lock);
+                return;
+        }
+
         em28xx_info("disconnecting %s\n", dev->vdev->name);
 
         flush_request_modules(dev);
diff --git a/drivers/media/video/em28xx/em28xx-core.c b/drivers/media/video/em28xx/em28xx-core.c
index e33f145d867..57b1b5c6d88 100644
--- a/drivers/media/video/em28xx/em28xx-core.c
+++ b/drivers/media/video/em28xx/em28xx-core.c
@@ -211,6 +211,7 @@ int em28xx_write_reg(struct em28xx *dev, u16 reg, u8 val)
 {
         return em28xx_write_regs(dev, reg, &val, 1);
 }
+EXPORT_SYMBOL_GPL(em28xx_write_reg);
 
 /*
  * em28xx_write_reg_bits()
@@ -286,6 +287,7 @@ int em28xx_read_ac97(struct em28xx *dev, u8 reg)
                 return ret;
         return le16_to_cpu(val);
 }
+EXPORT_SYMBOL_GPL(em28xx_read_ac97);
 
 /*
  * em28xx_write_ac97()
@@ -313,13 +315,14 @@ int em28xx_write_ac97(struct em28xx *dev, u8 reg, u16 val)
 
         return 0;
 }
+EXPORT_SYMBOL_GPL(em28xx_write_ac97);
 
-struct em28xx_vol_table {
+struct em28xx_vol_itable {
         enum em28xx_amux mux;
         u8               reg;
 };
 
-static struct em28xx_vol_table inputs[] = {
+static struct em28xx_vol_itable inputs[] = {
         { EM28XX_AMUX_VIDEO,    AC97_VIDEO_VOL   },
         { EM28XX_AMUX_LINE_IN,  AC97_LINEIN_VOL  },
         { EM28XX_AMUX_PHONE,    AC97_PHONE_VOL   },
@@ -403,7 +406,12 @@ static int em28xx_set_audio_source(struct em28xx *dev)
         return ret;
 }
 
-static const struct em28xx_vol_table outputs[] = {
+struct em28xx_vol_otable {
+        enum em28xx_aout mux;
+        u8               reg;
+};
+
+static const struct em28xx_vol_otable outputs[] = {
         { EM28XX_AOUT_MASTER, AC97_MASTER_VOL      },
         { EM28XX_AOUT_LINE,   AC97_LINE_LEVEL_VOL  },
         { EM28XX_AOUT_MONO,   AC97_MASTER_MONO_VOL },
@@ -492,17 +500,13 @@ int em28xx_audio_setup(struct em28xx *dev)
         if (dev->chip_id == CHIP_ID_EM2870 || dev->chip_id == CHIP_ID_EM2874
                 || dev->chip_id == CHIP_ID_EM28174) {
                 /* Digital only device - don't load any alsa module */
-                dev->audio_mode.has_audio = 0;
-                dev->has_audio_class = 0;
-                dev->has_alsa_audio = 0;
+                dev->audio_mode.has_audio = false;
+                dev->has_audio_class = false;
+                dev->has_alsa_audio = false;
                 return 0;
         }
 
-        /* If device doesn't support Usb Audio Class, use vendor class */
-        if (!dev->has_audio_class)
-                dev->has_alsa_audio = 1;
-
-        dev->audio_mode.has_audio = 1;
+        dev->audio_mode.has_audio = true;
 
         /* See how this device is configured */
         cfg = em28xx_read_reg(dev, EM28XX_R00_CHIPCFG);
@@ -512,8 +516,8 @@ int em28xx_audio_setup(struct em28xx *dev)
                 cfg = EM28XX_CHIPCFG_AC97; /* Be conservative */
         } else if ((cfg & EM28XX_CHIPCFG_AUDIOMASK) == 0x00) {
                 /* The device doesn't have vendor audio at all */
-                dev->has_alsa_audio = 0;
-                dev->audio_mode.has_audio = 0;
+                dev->has_alsa_audio = false;
+                dev->audio_mode.has_audio = false;
                 return 0;
         } else if ((cfg & EM28XX_CHIPCFG_AUDIOMASK) ==
                    EM28XX_CHIPCFG_I2S_3_SAMPRATES) {
@@ -542,8 +546,8 @@ int em28xx_audio_setup(struct em28xx *dev)
                  */
                 em28xx_warn("AC97 chip type couldn't be determined\n");
                 dev->audio_mode.ac97 = EM28XX_NO_AC97;
-                dev->has_alsa_audio = 0;
-                dev->audio_mode.has_audio = 0;
+                dev->has_alsa_audio = false;
+                dev->audio_mode.has_audio = false;
                 goto init_audio;
         }
 
@@ -615,7 +619,9 @@ int em28xx_capture_start(struct em28xx *dev, int start)
 {
         int rc;
 
-        if (dev->chip_id == CHIP_ID_EM2874 || dev->chip_id == CHIP_ID_EM28174) {
+        if (dev->chip_id == CHIP_ID_EM2874 ||
+            dev->chip_id == CHIP_ID_EM2884 ||
+            dev->chip_id == CHIP_ID_EM28174) {
                 /* The Transport Stream Enable Register moved in em2874 */
                 if (!start) {
                         rc = em28xx_write_reg_bits(dev, EM2874_R5F_TS_ENABLE,
@@ -884,6 +890,7 @@ int em28xx_gpio_set(struct em28xx *dev, struct em28xx_reg_seq *gpio)
         }
         return rc;
 }
+EXPORT_SYMBOL_GPL(em28xx_gpio_set);
 
 int em28xx_set_mode(struct em28xx *dev, enum em28xx_mode set_mode)
 {
@@ -917,7 +924,7 @@ EXPORT_SYMBOL_GPL(em28xx_set_mode);
 static void em28xx_irq_callback(struct urb *urb)
 {
         struct em28xx *dev = urb->context;
-        int rc, i;
+        int i;
 
         switch (urb->status) {
         case 0:             /* success */
@@ -934,7 +941,7 @@ static void em28xx_irq_callback(struct urb *urb)
 
         /* Copy data from URB */
         spin_lock(&dev->slock);
-        rc = dev->isoc_ctl.isoc_copy(dev, urb);
+        dev->isoc_ctl.isoc_copy(dev, urb);
         spin_unlock(&dev->slock);
 
         /* Reset urb buffers */
@@ -1106,17 +1113,19 @@ EXPORT_SYMBOL_GPL(em28xx_init_isoc);
 int em28xx_isoc_dvb_max_packetsize(struct em28xx *dev)
 {
         unsigned int chip_cfg2;
-        unsigned int packet_size = 564;
-
-        if (dev->chip_id == CHIP_ID_EM2874) {
-                /* FIXME - for now assume 564 like it was before, but the
-                   em2874 code should be added to return the proper value... */
-                packet_size = 564;
-        } else if (dev->chip_id == CHIP_ID_EM28174) {
-                /* FIXME same as em2874. 564 was enough for 22 Mbit DVB-T
-                   but too much for 44 Mbit DVB-C. */
-                packet_size = 752;
-        } else {
+        unsigned int packet_size;
+
+        switch (dev->chip_id) {
+        case CHIP_ID_EM2710:
+        case CHIP_ID_EM2750:
+        case CHIP_ID_EM2800:
+        case CHIP_ID_EM2820:
+        case CHIP_ID_EM2840:
+        case CHIP_ID_EM2860:
+                /* No DVB support */
+                return -EINVAL;
+        case CHIP_ID_EM2870:
+        case CHIP_ID_EM2883:
                 /* TS max packet size stored in bits 1-0 of R01 */
                 chip_cfg2 = em28xx_read_reg(dev, EM28XX_R01_CHIPCFG2);
                 switch (chip_cfg2 & EM28XX_CHIPCFG2_TS_PACKETSIZE_MASK) {
@@ -1133,9 +1142,24 @@ int em28xx_isoc_dvb_max_packetsize(struct em28xx *dev)
                         packet_size = 752;
                         break;
                 }
+                break;
+        case CHIP_ID_EM2874:
+                /*
+                 * FIXME: for now assumes 564 like it was before, but the
+                 * em2874 code should be added to return the proper value
+                 */
+                packet_size = 564;
+                break;
+        case CHIP_ID_EM2884:
+        case CHIP_ID_EM28174:
+        default:
+                /*
+                 * FIXME: same as em2874. 564 was enough for 22 Mbit DVB-T
+                 * but not enough for 44 Mbit DVB-C.
+                 */
+                packet_size = 752;
         }
 
-        em28xx_coredbg("dvb max packet size=%d\n", packet_size);
         return packet_size;
 }
 EXPORT_SYMBOL_GPL(em28xx_isoc_dvb_max_packetsize);
diff --git a/drivers/media/video/em28xx/em28xx-dvb.c b/drivers/media/video/em28xx/em28xx-dvb.c
index 7904ca4b691..e5916dee409 100644
--- a/drivers/media/video/em28xx/em28xx-dvb.c
+++ b/drivers/media/video/em28xx/em28xx-dvb.c
@@ -1,7 +1,7 @@
 /*
  DVB device driver for em28xx
 
- (c) 2008 Mauro Carvalho Chehab <mchehab@infradead.org>
+ (c) 2008-2011 Mauro Carvalho Chehab <mchehab@infradead.org>
 
  (c) 2008 Devin Heitmueller <devin.heitmueller@gmail.com>
         - Fixes for the driver to properly work with HVR-950
@@ -40,6 +40,8 @@
 #include "s921.h"
 #include "drxd.h"
 #include "cxd2820r.h"
+#include "tda18271c2dd.h"
+#include "drxk.h"
 
 MODULE_DESCRIPTION("driver for em28xx based DVB cards");
 MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@infradead.org>");
@@ -73,6 +75,11 @@ struct em28xx_dvb {
         struct dmx_frontend        fe_hw;
         struct dmx_frontend        fe_mem;
         struct dvb_net             net;
+
+        /* Due to DRX-K - probably need changes */
+        int (*gate_ctrl)(struct dvb_frontend *, int);
+        struct semaphore      pll_mutex;
+        bool                    dont_attach_fe1;
 };
 
 
@@ -160,6 +167,11 @@ static int start_streaming(struct em28xx_dvb *dvb)
                 return rc;
 
         max_dvb_packet_size = em28xx_isoc_dvb_max_packetsize(dev);
+        if (max_dvb_packet_size < 0)
+                return max_dvb_packet_size;
+        dprintk(1, "Using %d buffers each with %d bytes\n",
+                EM28XX_DVB_NUM_BUFS,
+                max_dvb_packet_size);
 
         return em28xx_init_isoc(dev, EM28XX_DVB_MAX_PACKETS,
                                 EM28XX_DVB_NUM_BUFS, max_dvb_packet_size,
@@ -295,6 +307,79 @@ static struct drxd_config em28xx_drxd = {
         .disable_i2c_gate_ctrl = 1,
 };
 
+struct drxk_config terratec_h5_drxk = {
+        .adr = 0x29,
+        .single_master = 1,
+        .no_i2c_bridge = 1,
+        .microcode_name = "dvb-usb-terratec-h5-drxk.fw",
+};
+
+static int drxk_gate_ctrl(struct dvb_frontend *fe, int enable)
+{
+        struct em28xx_dvb *dvb = fe->sec_priv;
+        int status;
+
+        if (!dvb)
+                return -EINVAL;
+
+        if (enable) {
+                down(&dvb->pll_mutex);
+                status = dvb->gate_ctrl(fe, 1);
+        } else {
+                status = dvb->gate_ctrl(fe, 0);
+                up(&dvb->pll_mutex);
+        }
+        return status;
+}
+
+static void terratec_h5_init(struct em28xx *dev)
+{
+        int i;
+        struct em28xx_reg_seq terratec_h5_init[] = {
+                {EM28XX_R08_GPIO,       0xff,   0xff,   10},
+                {EM2874_R80_GPIO,       0xf6,   0xff,   100},
+                {EM2874_R80_GPIO,       0xf2,   0xff,   50},
+                {EM2874_R80_GPIO,       0xf6,   0xff,   100},
+                { -1,                   -1,     -1,     -1},
+        };
+        struct em28xx_reg_seq terratec_h5_end[] = {
+                {EM2874_R80_GPIO,       0xe6,   0xff,   100},
+                {EM2874_R80_GPIO,       0xa6,   0xff,   50},
+                {EM2874_R80_GPIO,       0xe6,   0xff,   100},
+                { -1,                   -1,     -1,     -1},
+        };
+        struct {
+                unsigned char r[4];
+                int len;
+        } regs[] = {
+                {{ 0x06, 0x02, 0x00, 0x31 }, 4},
+                {{ 0x01, 0x02 }, 2},
+                {{ 0x01, 0x02, 0x00, 0xc6 }, 4},
+                {{ 0x01, 0x00 }, 2},
+                {{ 0x01, 0x00, 0xff, 0xaf }, 4},
+                {{ 0x01, 0x00, 0x03, 0xa0 }, 4},
+                {{ 0x01, 0x00 }, 2},
+                {{ 0x01, 0x00, 0x73, 0xaf }, 4},
+                {{ 0x04, 0x00 }, 2},
+                {{ 0x00, 0x04 }, 2},
+                {{ 0x00, 0x04, 0x00, 0x0a }, 4},
+                {{ 0x04, 0x14 }, 2},
+                {{ 0x04, 0x14, 0x00, 0x00 }, 4},
+        };
+
+        em28xx_gpio_set(dev, terratec_h5_init);
+        em28xx_write_reg(dev, EM28XX_R06_I2C_CLK, 0x40);
+        msleep(10);
+        em28xx_write_reg(dev, EM28XX_R06_I2C_CLK, 0x45);
+        msleep(10);
+
+        dev->i2c_client.addr = 0x82 >> 1;
+
+        for (i = 0; i < ARRAY_SIZE(regs); i++)
+                i2c_master_send(&dev->i2c_client, regs[i].r, regs[i].len);
+        em28xx_gpio_set(dev, terratec_h5_end);
+};
+
 static int mt352_terratec_xs_init(struct dvb_frontend *fe)
 {
         /* Values extracted from a USB trace of the Terratec Windows driver */
@@ -516,7 +601,7 @@ static void unregister_dvb(struct em28xx_dvb *dvb)
         if (dvb->fe[1])
                 dvb_unregister_frontend(dvb->fe[1]);
         dvb_unregister_frontend(dvb->fe[0]);
-        if (dvb->fe[1])
+        if (dvb->fe[1] && !dvb->dont_attach_fe1)
                 dvb_frontend_detach(dvb->fe[1]);
         dvb_frontend_detach(dvb->fe[0]);
         dvb_unregister_adapter(&dvb->adapter);
@@ -546,7 +631,7 @@ static int dvb_init(struct em28xx *dev)
         em28xx_set_mode(dev, EM28XX_DIGITAL_MODE);
         /* init frontend */
         switch (dev->model) {
-        case EM2874_LEADERSHIP_ISDBT:
+        case EM2874_BOARD_LEADERSHIP_ISDBT:
                 dvb->fe[0] = dvb_attach(s921_attach,
                                 &sharp_isdbt, &dev->i2c_adap);
 
@@ -689,6 +774,41 @@ static int dvb_init(struct em28xx *dev)
                         }
                 }
                 break;
+        case EM2884_BOARD_TERRATEC_H5:
+                terratec_h5_init(dev);
+
+                dvb->dont_attach_fe1 = 1;
+
+                dvb->fe[0] = dvb_attach(drxk_attach, &terratec_h5_drxk, &dev->i2c_adap, &dvb->fe[1]);
+                if (!dvb->fe[0]) {
+                        result = -EINVAL;
+                        goto out_free;
+                }
+
+                /* FIXME: do we need a pll semaphore? */
+                dvb->fe[0]->sec_priv = dvb;
+                sema_init(&dvb->pll_mutex, 1);
+                dvb->gate_ctrl = dvb->fe[0]->ops.i2c_gate_ctrl;
+                dvb->fe[0]->ops.i2c_gate_ctrl = drxk_gate_ctrl;
+                dvb->fe[1]->id = 1;
+
+                /* Attach tda18271 to DVB-C frontend */
+                if (dvb->fe[0]->ops.i2c_gate_ctrl)
+                        dvb->fe[0]->ops.i2c_gate_ctrl(dvb->fe[0], 1);
+                if (!dvb_attach(tda18271c2dd_attach, dvb->fe[0], &dev->i2c_adap, 0x60)) {
+                        result = -EINVAL;
+                        goto out_free;
+                }
+                if (dvb->fe[0]->ops.i2c_gate_ctrl)
+                        dvb->fe[0]->ops.i2c_gate_ctrl(dvb->fe[0], 0);
+
+                /* Hack - needed by drxk/tda18271c2dd */
+                dvb->fe[1]->tuner_priv = dvb->fe[0]->tuner_priv;
+                memcpy(&dvb->fe[1]->ops.tuner_ops,
+                       &dvb->fe[0]->ops.tuner_ops,
+                       sizeof(dvb->fe[0]->ops.tuner_ops));
+
+                break;
         default:
                 em28xx_errdev("/2: The frontend of your DVB/ATSC card"
                                 " isn't supported yet\n");
diff --git a/drivers/media/video/em28xx/em28xx-i2c.c b/drivers/media/video/em28xx/em28xx-i2c.c
index 4739fc7e6eb..36f5a9bc8b7 100644
--- a/drivers/media/video/em28xx/em28xx-i2c.c
+++ b/drivers/media/video/em28xx/em28xx-i2c.c
@@ -181,16 +181,25 @@ static int em2800_i2c_recv_bytes(struct em28xx *dev, unsigned char addr,
 
 /*
  * em28xx_i2c_send_bytes()
- * untested for more than 4 bytes
  */
 static int em28xx_i2c_send_bytes(void *data, unsigned char addr, char *buf,
                                  short len, int stop)
 {
         int wrcount = 0;
         struct em28xx *dev = (struct em28xx *)data;
+        int write_timeout, ret;
 
         wrcount = dev->em28xx_write_regs_req(dev, stop ? 2 : 3, addr, buf, len);
 
+        /* Seems to be required after a write */
+        for (write_timeout = EM2800_I2C_WRITE_TIMEOUT; write_timeout > 0;
+             write_timeout -= 5) {
+                ret = dev->em28xx_read_reg(dev, 0x05);
+                if (!ret)
+                        break;
+                msleep(5);
+        }
+
         return wrcount;
 }
 
@@ -218,9 +227,7 @@ static int em28xx_i2c_recv_bytes(struct em28xx *dev, unsigned char addr,
  */
 static int em28xx_i2c_check_for_device(struct em28xx *dev, unsigned char addr)
 {
-        char msg;
         int ret;
-        msg = addr;
 
         ret = dev->em28xx_read_reg_req(dev, 2, addr);
         if (ret < 0) {
@@ -332,7 +339,9 @@ static int em28xx_i2c_eeprom(struct em28xx *dev, unsigned char *eedata, int len)
         struct em28xx_eeprom *em_eeprom = (void *)eedata;
         int i, err, size = len, block;
 
-        if (dev->chip_id == CHIP_ID_EM2874 || dev->chip_id == CHIP_ID_EM28174) {
+        if (dev->chip_id == CHIP_ID_EM2874 ||
+            dev->chip_id == CHIP_ID_EM28174 ||
+            dev->chip_id == CHIP_ID_EM2884) {
                 /* Empia switched to a 16-bit addressable eeprom in newer
                    devices.  While we could certainly write a routine to read
                    the eeprom, there is nothing of use in there that cannot be
diff --git a/drivers/media/video/em28xx/em28xx-input.c b/drivers/media/video/em28xx/em28xx-input.c
index ba1ba8648c8..5d12b14282e 100644
--- a/drivers/media/video/em28xx/em28xx-input.c
+++ b/drivers/media/video/em28xx/em28xx-input.c
@@ -372,6 +372,7 @@ int em28xx_ir_change_protocol(struct rc_dev *rc_dev, u64 rc_type)
                 ir->get_key = default_polling_getkey;
                 break;
         case CHIP_ID_EM2874:
+        case CHIP_ID_EM28174:
                 ir->get_key = em2874_polling_getkey;
                 em28xx_write_regs(dev, EM2874_R50_IR_CONFIG, &ir_config, 1);
                 break;
diff --git a/drivers/media/video/em28xx/em28xx-reg.h b/drivers/media/video/em28xx/em28xx-reg.h
index e92a28ede43..66f792361b9 100644
--- a/drivers/media/video/em28xx/em28xx-reg.h
+++ b/drivers/media/video/em28xx/em28xx-reg.h
@@ -201,6 +201,7 @@ enum em28xx_chip_id {
         CHIP_ID_EM2870 = 35,
         CHIP_ID_EM2883 = 36,
         CHIP_ID_EM2874 = 65,
+        CHIP_ID_EM2884 = 68,
         CHIP_ID_EM28174 = 113,
 };
 
diff --git a/drivers/media/video/em28xx/em28xx-video.c b/drivers/media/video/em28xx/em28xx-video.c
index 7b6461d2d1f..d176dc0394e 100644
--- a/drivers/media/video/em28xx/em28xx-video.c
+++ b/drivers/media/video/em28xx/em28xx-video.c
@@ -32,7 +32,6 @@
 #include <linux/bitmap.h>
 #include <linux/usb.h>
 #include <linux/i2c.h>
-#include <linux/version.h>
 #include <linux/mm.h>
 #include <linux/mutex.h>
 #include <linux/slab.h>
@@ -50,7 +49,8 @@
                       "Sascha Sommer <saschasommer@freenet.de>"
 
 #define DRIVER_DESC         "Empia em28xx based USB video device driver"
-#define EM28XX_VERSION_CODE  KERNEL_VERSION(0, 1, 2)
+
+#define EM28XX_VERSION "0.1.3"
 
 #define em28xx_videodbg(fmt, arg...) do {\
         if (video_debug) \
@@ -72,6 +72,7 @@ do {\
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
+MODULE_VERSION(EM28XX_VERSION);
 
 static unsigned int video_nr[] = {[0 ... (EM28XX_MAXBOARDS - 1)] = UNSET };
 static unsigned int vbi_nr[]   = {[0 ... (EM28XX_MAXBOARDS - 1)] = UNSET };
@@ -1757,8 +1758,6 @@ static int vidioc_querycap(struct file *file, void  *priv,
         strlcpy(cap->card, em28xx_boards[dev->model].name, sizeof(cap->card));
         usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
 
-        cap->version = EM28XX_VERSION_CODE;
-
         cap->capabilities =
                         V4L2_CAP_SLICED_VBI_CAPTURE |
                         V4L2_CAP_VIDEO_CAPTURE |
@@ -1976,7 +1975,6 @@ static int radio_querycap(struct file *file, void  *priv,
         strlcpy(cap->card, em28xx_boards[dev->model].name, sizeof(cap->card));
         usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
 
-        cap->version = EM28XX_VERSION_CODE;
         cap->capabilities = V4L2_CAP_TUNER;
         return 0;
 }
@@ -2450,10 +2448,8 @@ int em28xx_register_analog_devices(struct em28xx *dev)
       u8 val;
         int ret;
 
-        printk(KERN_INFO "%s: v4l2 driver version %d.%d.%d\n",
-                dev->name,
-                (EM28XX_VERSION_CODE >> 16) & 0xff,
-                (EM28XX_VERSION_CODE >> 8) & 0xff, EM28XX_VERSION_CODE & 0xff);
+        printk(KERN_INFO "%s: v4l2 driver version %s\n",
+                dev->name, EM28XX_VERSION);
 
         /* set default norm */
         dev->norm = em28xx_video_template.current_norm;
diff --git a/drivers/media/video/em28xx/em28xx.h b/drivers/media/video/em28xx/em28xx.h
index 3cca3312245..d80658bf3da 100644
--- a/drivers/media/video/em28xx/em28xx.h
+++ b/drivers/media/video/em28xx/em28xx.h
@@ -117,9 +117,9 @@
 #define EM2800_BOARD_VC211A                       74
 #define EM2882_BOARD_DIKOM_DK300                  75
 #define EM2870_BOARD_KWORLD_A340                  76
-#define EM2874_LEADERSHIP_ISDBT                   77
+#define EM2874_BOARD_LEADERSHIP_ISDBT             77
 #define EM28174_BOARD_PCTV_290E                   78
-
+#define EM2884_BOARD_TERRATEC_H5                  79
 
 /* Limits minimum and default number of buffers */
 #define EM28XX_MIN_BUF 4
@@ -487,6 +487,8 @@ struct em28xx {
         int devno;              /* marks the number of this device */
         enum em28xx_chip_id chip_id;
 
+        int audio_ifnum;
+
         struct v4l2_device v4l2_dev;
         struct em28xx_board board;
 
@@ -503,6 +505,7 @@ struct em28xx {
 
         unsigned int has_audio_class:1;
         unsigned int has_alsa_audio:1;
+        unsigned int is_audio_only:1;
 
         /* Controls audio streaming */
         struct work_struct wq_trigger;              /* Trigger to start/stop audio for alsa module */
@@ -697,6 +700,9 @@ int em28xx_tuner_callback(void *ptr, int component, int command, int arg);
 void em28xx_release_resources(struct em28xx *dev);
 
 /* Provided by em28xx-input.c */
+
+#ifdef CONFIG_VIDEO_EM28XX_RC
+
 int em28xx_get_key_terratec(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw);
 int em28xx_get_key_em_haup(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw);
 int em28xx_get_key_pinnacle_usb_grey(struct IR_i2c *ir, u32 *ir_key,
@@ -709,6 +715,20 @@ void em28xx_deregister_snapshot_button(struct em28xx *dev);
 int em28xx_ir_init(struct em28xx *dev);
 int em28xx_ir_fini(struct em28xx *dev);
 
+#else
+
+#define em28xx_get_key_terratec                 NULL
+#define em28xx_get_key_em_haup                  NULL
+#define em28xx_get_key_pinnacle_usb_grey        NULL
+#define em28xx_get_key_winfast_usbii_deluxe     NULL
+
+static inline void em28xx_register_snapshot_button(struct em28xx *dev) {}
+static inline void em28xx_deregister_snapshot_button(struct em28xx *dev) {}
+static inline int em28xx_ir_init(struct em28xx *dev) { return 0; }
+static inline int em28xx_ir_fini(struct em28xx *dev) { return 0; }
+
+#endif
+
 /* Provided by em28xx-vbi.c */
 extern struct videobuf_queue_ops em28xx_vbi_qops;
 
diff --git a/drivers/media/video/et61x251/et61x251.h b/drivers/media/video/et61x251/et61x251.h
index bf66189cb26..14bb907d650 100644
--- a/drivers/media/video/et61x251/et61x251.h
+++ b/drivers/media/video/et61x251/et61x251.h
@@ -21,7 +21,6 @@
 #ifndef _ET61X251_H_
 #define _ET61X251_H_
 
-#include <linux/version.h>
 #include <linux/usb.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-common.h>
diff --git a/drivers/media/video/et61x251/et61x251_core.c b/drivers/media/video/et61x251/et61x251_core.c
index a982750dcef..9a1e80a1e14 100644
--- a/drivers/media/video/et61x251/et61x251_core.c
+++ b/drivers/media/video/et61x251/et61x251_core.c
@@ -18,6 +18,7 @@
  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.               *
  ***************************************************************************/
 
+#include <linux/version.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/kernel.h>
@@ -48,8 +49,7 @@
 #define ET61X251_MODULE_AUTHOR  "(C) 2006-2007 Luca Risolia"
 #define ET61X251_AUTHOR_EMAIL   "<luca.risolia@studio.unibo.it>"
 #define ET61X251_MODULE_LICENSE "GPL"
-#define ET61X251_MODULE_VERSION "1:1.09"
-#define ET61X251_MODULE_VERSION_CODE  KERNEL_VERSION(1, 1, 9)
+#define ET61X251_MODULE_VERSION "1.1.10"
 
 /*****************************************************************************/
 
@@ -1579,7 +1579,7 @@ et61x251_vidioc_querycap(struct et61x251_device* cam, void __user * arg)
 {
         struct v4l2_capability cap = {
                 .driver = "et61x251",
-                .version = ET61X251_MODULE_VERSION_CODE,
+                .version = LINUX_VERSION_CODE,
                 .capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
                                 V4L2_CAP_STREAMING,
         };
@@ -2480,16 +2480,8 @@ static long et61x251_ioctl_v4l2(struct file *filp,
         case VIDIOC_S_PARM:
                 return et61x251_vidioc_s_parm(cam, arg);
 
-        case VIDIOC_G_STD:
-        case VIDIOC_S_STD:
-        case VIDIOC_QUERYSTD:
-        case VIDIOC_ENUMSTD:
-        case VIDIOC_QUERYMENU:
-        case VIDIOC_ENUM_FRAMEINTERVALS:
-                return -EINVAL;
-
         default:
-                return -EINVAL;
+                return -ENOTTY;
 
         }
 }
diff --git a/drivers/media/video/fsl-viu.c b/drivers/media/video/fsl-viu.c
index 908d7012c3f..27cb197d0bd 100644
--- a/drivers/media/video/fsl-viu.c
+++ b/drivers/media/video/fsl-viu.c
@@ -23,19 +23,13 @@
 #include <linux/io.h>
 #include <linux/of_platform.h>
 #include <linux/slab.h>
-#include <linux/version.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 #include <media/videobuf-dma-contig.h>
 
 #define DRV_NAME                "fsl_viu"
-#define VIU_MAJOR_VERSION       0
-#define VIU_MINOR_VERSION       5
-#define VIU_RELEASE             0
-#define VIU_VERSION             KERNEL_VERSION(VIU_MAJOR_VERSION, \
-                                               VIU_MINOR_VERSION, \
-                                               VIU_RELEASE)
+#define VIU_VERSION             "0.5.1"
 
 #define BUFFER_TIMEOUT          msecs_to_jiffies(500)  /* 0.5 seconds */
 
@@ -610,7 +604,6 @@ static int vidioc_querycap(struct file *file, void *priv,
 {
         strcpy(cap->driver, "viu");
         strcpy(cap->card, "viu");
-        cap->version = VIU_VERSION;
         cap->capabilities =     V4L2_CAP_VIDEO_CAPTURE |
                                 V4L2_CAP_STREAMING     |
                                 V4L2_CAP_VIDEO_OVERLAY |
@@ -1684,3 +1677,4 @@ module_exit(viu_exit);
 MODULE_DESCRIPTION("Freescale Video-In(VIU)");
 MODULE_AUTHOR("Hongjun Chen");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(VIU_VERSION);
diff --git a/drivers/media/video/gspca/Kconfig b/drivers/media/video/gspca/Kconfig
index 34ae2c29979..43d9a20caeb 100644
--- a/drivers/media/video/gspca/Kconfig
+++ b/drivers/media/video/gspca/Kconfig
@@ -179,6 +179,16 @@ config USB_GSPCA_PAC7311
           To compile this driver as a module, choose M here: the
           module will be called gspca_pac7311.
 
+config USB_GSPCA_SE401
+        tristate "SE401 USB Camera Driver"
+        depends on VIDEO_V4L2 && USB_GSPCA
+        help
+         Say Y here if you want support for cameras based on the
+         Endpoints (formerly known as AOX) se401 chip.
+
+         To compile this driver as a module, choose M here: the
+         module will be called gspca_se401.
+
 config USB_GSPCA_SN9C2028
         tristate "SONIX Dual-Mode USB Camera Driver"
         depends on VIDEO_V4L2 && USB_GSPCA
diff --git a/drivers/media/video/gspca/Makefile b/drivers/media/video/gspca/Makefile
index 802fbe1bff4..d6364a86333 100644
--- a/drivers/media/video/gspca/Makefile
+++ b/drivers/media/video/gspca/Makefile
@@ -16,6 +16,7 @@ obj-$(CONFIG_USB_GSPCA_OV534_9)  += gspca_ov534_9.o
 obj-$(CONFIG_USB_GSPCA_PAC207)   += gspca_pac207.o
 obj-$(CONFIG_USB_GSPCA_PAC7302)  += gspca_pac7302.o
 obj-$(CONFIG_USB_GSPCA_PAC7311)  += gspca_pac7311.o
+obj-$(CONFIG_USB_GSPCA_SE401)    += gspca_se401.o
 obj-$(CONFIG_USB_GSPCA_SN9C2028) += gspca_sn9c2028.o
 obj-$(CONFIG_USB_GSPCA_SN9C20X)  += gspca_sn9c20x.o
 obj-$(CONFIG_USB_GSPCA_SONIXB)   += gspca_sonixb.o
@@ -58,6 +59,7 @@ gspca_ov534_9-objs  := ov534_9.o
 gspca_pac207-objs   := pac207.o
 gspca_pac7302-objs  := pac7302.o
 gspca_pac7311-objs  := pac7311.o
+gspca_se401-objs    := se401.o
 gspca_sn9c2028-objs := sn9c2028.o
 gspca_sn9c20x-objs  := sn9c20x.o
 gspca_sonixb-objs   := sonixb.o
diff --git a/drivers/media/video/gspca/gl860/gl860.h b/drivers/media/video/gspca/gl860/gl860.h
index 49ad4acbf60..0330a0293b9 100644
--- a/drivers/media/video/gspca/gl860/gl860.h
+++ b/drivers/media/video/gspca/gl860/gl860.h
@@ -18,7 +18,6 @@
  */
 #ifndef GL860_DEV_H
 #define GL860_DEV_H
-#include <linux/version.h>
 
 #include "gspca.h"
 
diff --git a/drivers/media/video/gspca/gspca.c b/drivers/media/video/gspca/gspca.c
index 08ce9948d99..5da4879f47f 100644
--- a/drivers/media/video/gspca/gspca.c
+++ b/drivers/media/video/gspca/gspca.c
@@ -24,7 +24,6 @@
 #define MODULE_NAME "gspca"
 
 #include <linux/init.h>
-#include <linux/version.h>
 #include <linux/fs.h>
 #include <linux/vmalloc.h>
 #include <linux/sched.h>
@@ -51,11 +50,12 @@
 #error "DEF_NURBS too big"
 #endif
 
+#define DRIVER_VERSION_NUMBER   "2.13.0"
+
 MODULE_AUTHOR("Jean-François Moine <http://moinejf.free.fr>");
 MODULE_DESCRIPTION("GSPCA USB Camera Driver");
 MODULE_LICENSE("GPL");
-
-#define DRIVER_VERSION_NUMBER   KERNEL_VERSION(2, 13, 0)
+MODULE_VERSION(DRIVER_VERSION_NUMBER);
 
 #ifdef GSPCA_DEBUG
 int gspca_debug = D_ERR | D_PROBE;
@@ -443,8 +443,11 @@ void gspca_frame_add(struct gspca_dev *gspca_dev,
         } else {
                 switch (gspca_dev->last_packet_type) {
                 case DISCARD_PACKET:
-                        if (packet_type == LAST_PACKET)
+                        if (packet_type == LAST_PACKET) {
                                 gspca_dev->last_packet_type = packet_type;
+                                gspca_dev->image = NULL;
+                                gspca_dev->image_len = 0;
+                        }
                         return;
                 case LAST_PACKET:
                         return;
@@ -1278,10 +1281,10 @@ static int vidioc_querycap(struct file *file, void  *priv,
                 ret = -ENODEV;
                 goto out;
         }
-        strncpy((char *) cap->driver, gspca_dev->sd_desc->name,
+        strlcpy((char *) cap->driver, gspca_dev->sd_desc->name,
                         sizeof cap->driver);
         if (gspca_dev->dev->product != NULL) {
-                strncpy((char *) cap->card, gspca_dev->dev->product,
+                strlcpy((char *) cap->card, gspca_dev->dev->product,
                         sizeof cap->card);
         } else {
                 snprintf((char *) cap->card, sizeof cap->card,
@@ -1291,7 +1294,6 @@ static int vidioc_querycap(struct file *file, void  *priv,
         }
         usb_make_path(gspca_dev->dev, (char *) cap->bus_info,
                         sizeof(cap->bus_info));
-        cap->version = DRIVER_VERSION_NUMBER;
         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE
                           | V4L2_CAP_STREAMING
                           | V4L2_CAP_READWRITE;
@@ -1460,7 +1462,7 @@ static int vidioc_enum_input(struct file *file, void *priv,
                 return -EINVAL;
         input->type = V4L2_INPUT_TYPE_CAMERA;
         input->status = gspca_dev->cam.input_flags;
-        strncpy(input->name, gspca_dev->sd_desc->name,
+        strlcpy(input->name, gspca_dev->sd_desc->name,
                 sizeof input->name);
         return 0;
 }
@@ -2478,10 +2480,7 @@ EXPORT_SYMBOL(gspca_auto_gain_n_exposure);
 /* -- module insert / remove -- */
 static int __init gspca_init(void)
 {
-        info("v%d.%d.%d registered",
-                (DRIVER_VERSION_NUMBER >> 16) & 0xff,
-                (DRIVER_VERSION_NUMBER >> 8) & 0xff,
-                DRIVER_VERSION_NUMBER & 0xff);
+        info("v" DRIVER_VERSION_NUMBER " registered");
         return 0;
 }
 static void __exit gspca_exit(void)
diff --git a/drivers/media/video/gspca/ov519.c b/drivers/media/video/gspca/ov519.c
index 057e287b915..0800433b209 100644
--- a/drivers/media/video/gspca/ov519.c
+++ b/drivers/media/video/gspca/ov519.c
@@ -134,6 +134,7 @@ enum sensors {
         SEN_OV7670,
         SEN_OV76BE,
         SEN_OV8610,
+        SEN_OV9600,
 };
 
 /* Note this is a bit of a hack, but the w9968cf driver needs the code for all
@@ -340,6 +341,10 @@ static const unsigned ctrl_dis[] = {
                         (1 << EXPOSURE) |
                         (1 << AUTOGAIN) |
                         (1 << FREQ),
+[SEN_OV9600] =          ((1 << NCTRL) - 1)      /* no control */
+                        ^ ((1 << EXPOSURE)      /* but exposure */
+                         | (1 << AUTOGAIN)),    /* and autogain */
+
 };
 
 static const struct v4l2_pix_format ov519_vga_mode[] = {
@@ -525,6 +530,17 @@ static const struct v4l2_pix_format ovfx2_ov3610_mode[] = {
                 .colorspace = V4L2_COLORSPACE_SRGB,
                 .priv = 0},
 };
+static const struct v4l2_pix_format ovfx2_ov9600_mode[] = {
+        {640, 480, V4L2_PIX_FMT_SBGGR8, V4L2_FIELD_NONE,
+                .bytesperline = 640,
+                .sizeimage = 640 * 480,
+                .colorspace = V4L2_COLORSPACE_SRGB,
+                .priv = 1},
+        {1280, 1024, V4L2_PIX_FMT_SBGGR8, V4L2_FIELD_NONE,
+                .bytesperline = 1280,
+                .sizeimage = 1280 * 1024,
+                .colorspace = V4L2_COLORSPACE_SRGB},
+};
 
 /* Registers common to OV511 / OV518 */
 #define R51x_FIFO_PSIZE                 0x30    /* 2 bytes wide w/ OV518(+) */
@@ -1807,6 +1823,22 @@ static const struct ov_i2c_regvals norm_7660[] = {
                         | OV7670_COM8_AEC},
         {0xa1, 0xc8}
 };
+static const struct ov_i2c_regvals norm_9600[] = {
+        {0x12, 0x80},
+        {0x0c, 0x28},
+        {0x11, 0x80},
+        {0x13, 0xb5},
+        {0x14, 0x3e},
+        {0x1b, 0x04},
+        {0x24, 0xb0},
+        {0x25, 0x90},
+        {0x26, 0x94},
+        {0x35, 0x90},
+        {0x37, 0x07},
+        {0x38, 0x08},
+        {0x01, 0x8e},
+        {0x02, 0x85}
+};
 
 /* 7670. Defaults taken from OmniVision provided data,
 *  as provided by Jonathan Corbet of OLPC               */
@@ -2400,9 +2432,12 @@ static int ov518_i2c_r(struct sd *sd, u8 reg)
 
         /* Initiate 2-byte write cycle */
         reg_w(sd, R518_I2C_CTL, 0x03);
+        reg_r8(sd, R518_I2C_CTL);
 
         /* Initiate 2-byte read cycle */
         reg_w(sd, R518_I2C_CTL, 0x05);
+        reg_r8(sd, R518_I2C_CTL);
+
         value = reg_r(sd, R51x_I2C_DATA);
         PDEBUG(D_USBI, "ov518_i2c_r %02x %02x", reg, value);
         return value;
@@ -2686,7 +2721,7 @@ static void write_i2c_regvals(struct sd *sd,
  *
  ***************************************************************************/
 
-/* This initializes the OV2x10 / OV3610 / OV3620 */
+/* This initializes the OV2x10 / OV3610 / OV3620 / OV9600 */
 static void ov_hires_configure(struct sd *sd)
 {
         int high, low;
@@ -2702,19 +2737,32 @@ static void ov_hires_configure(struct sd *sd)
         high = i2c_r(sd, 0x0a);
         low = i2c_r(sd, 0x0b);
         /* info("%x, %x", high, low); */
-        if (high == 0x96 && low == 0x40) {
-                PDEBUG(D_PROBE, "Sensor is an OV2610");
-                sd->sensor = SEN_OV2610;
-        } else if (high == 0x96 && low == 0x41) {
-                PDEBUG(D_PROBE, "Sensor is an OV2610AE");
-                sd->sensor = SEN_OV2610AE;
-        } else if (high == 0x36 && (low & 0x0f) == 0x00) {
-                PDEBUG(D_PROBE, "Sensor is an OV3610");
-                sd->sensor = SEN_OV3610;
-        } else {
-                err("Error unknown sensor type: %02x%02x",
-                        high, low);
+        switch (high) {
+        case 0x96:
+                switch (low) {
+                case 0x40:
+                        PDEBUG(D_PROBE, "Sensor is a OV2610");
+                        sd->sensor = SEN_OV2610;
+                        return;
+                case 0x41:
+                        PDEBUG(D_PROBE, "Sensor is a OV2610AE");
+                        sd->sensor = SEN_OV2610AE;
+                        return;
+                case 0xb1:
+                        PDEBUG(D_PROBE, "Sensor is a OV9600");
+                        sd->sensor = SEN_OV9600;
+                        return;
+                }
+                break;
+        case 0x36:
+                if ((low & 0x0f) == 0x00) {
+                        PDEBUG(D_PROBE, "Sensor is a OV3610");
+                        sd->sensor = SEN_OV3610;
+                        return;
+                }
+                break;
         }
+        err("Error unknown sensor type: %02x%02x", high, low);
 }
 
 /* This initializes the OV8110, OV8610 sensor. The OV8110 uses
@@ -3400,6 +3448,10 @@ static int sd_init(struct gspca_dev *gspca_dev)
                         cam->cam_mode = ovfx2_ov3610_mode;
                         cam->nmodes = ARRAY_SIZE(ovfx2_ov3610_mode);
                         break;
+                case SEN_OV9600:
+                        cam->cam_mode = ovfx2_ov9600_mode;
+                        cam->nmodes = ARRAY_SIZE(ovfx2_ov9600_mode);
+                        break;
                 default:
                         if (sd->sif) {
                                 cam->cam_mode = ov519_sif_mode;
@@ -3497,6 +3549,12 @@ static int sd_init(struct gspca_dev *gspca_dev)
         case SEN_OV8610:
                 write_i2c_regvals(sd, norm_8610, ARRAY_SIZE(norm_8610));
                 break;
+        case SEN_OV9600:
+                write_i2c_regvals(sd, norm_9600, ARRAY_SIZE(norm_9600));
+
+                /* enable autoexpo */
+/*              i2c_w_mask(sd, 0x13, 0x05, 0x05); */
+                break;
         }
         return gspca_dev->usb_err;
 error:
@@ -4085,6 +4143,33 @@ static void mode_init_ov_sensor_regs(struct sd *sd)
                 i2c_w_mask(sd, 0x14, qvga ? 0x20 : 0x00, 0x20);
                 i2c_w_mask(sd, 0x12, 0x04, 0x06); /* AWB: 1 Test pattern: 0 */
                 break;
+        case SEN_OV9600: {
+                const struct ov_i2c_regvals *vals;
+                static const struct ov_i2c_regvals sxga_15[] = {
+                        {0x11, 0x80}, {0x14, 0x3e}, {0x24, 0x85}, {0x25, 0x75}
+                };
+                static const struct ov_i2c_regvals sxga_7_5[] = {
+                        {0x11, 0x81}, {0x14, 0x3e}, {0x24, 0x85}, {0x25, 0x75}
+                };
+                static const struct ov_i2c_regvals vga_30[] = {
+                        {0x11, 0x81}, {0x14, 0x7e}, {0x24, 0x70}, {0x25, 0x60}
+                };
+                static const struct ov_i2c_regvals vga_15[] = {
+                        {0x11, 0x83}, {0x14, 0x3e}, {0x24, 0x80}, {0x25, 0x70}
+                };
+
+                /* frame rates:
+                 *      15fps / 7.5 fps for 1280x1024
+                 *      30fps / 15fps for 640x480
+                 */
+                i2c_w_mask(sd, 0x12, qvga ? 0x40 : 0x00, 0x40);
+                if (qvga)
+                        vals = sd->frame_rate < 30 ? vga_15 : vga_30;
+                else
+                        vals = sd->frame_rate < 15 ? sxga_7_5 : sxga_15;
+                write_i2c_regvals(sd, vals, ARRAY_SIZE(sxga_15));
+                return;
+            }
         default:
                 return;
         }
@@ -4120,6 +4205,7 @@ static void set_ov_sensor_window(struct sd *sd)
         case SEN_OV2610AE:
         case SEN_OV3610:
         case SEN_OV7670:
+        case SEN_OV9600:
                 mode_init_ov_sensor_regs(sd);
                 return;
         case SEN_OV7660:
@@ -4920,7 +5006,8 @@ static const struct sd_desc sd_desc = {
 static const struct usb_device_id device_table[] = {
         {USB_DEVICE(0x041e, 0x4003), .driver_info = BRIDGE_W9968CF },
         {USB_DEVICE(0x041e, 0x4052), .driver_info = BRIDGE_OV519 },
-        {USB_DEVICE(0x041e, 0x405f), .driver_info = BRIDGE_OV519 },
+        {USB_DEVICE(0x041e, 0x405f),
+                .driver_info = BRIDGE_OV519 | BRIDGE_INVERT_LED },
         {USB_DEVICE(0x041e, 0x4060), .driver_info = BRIDGE_OV519 },
         {USB_DEVICE(0x041e, 0x4061), .driver_info = BRIDGE_OV519 },
         {USB_DEVICE(0x041e, 0x4064),
diff --git a/drivers/media/video/gspca/se401.c b/drivers/media/video/gspca/se401.c
new file mode 100644
index 00000000000..4c283c24c75
--- /dev/null
+++ b/drivers/media/video/gspca/se401.c
@@ -0,0 +1,774 @@
+/*
+ * GSPCA Endpoints (formerly known as AOX) se401 USB Camera sub Driver
+ *
+ * Copyright (C) 2011 Hans de Goede <hdegoede@redhat.com>
+ *
+ * Based on the v4l1 se401 driver which is:
+ *
+ * Copyright (c) 2000 Jeroen B. Vreeken (pe1rxq@amsat.org)
+ *
+ * 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 MODULE_NAME "se401"
+
+#define BULK_SIZE 4096
+#define PACKET_SIZE 1024
+#define READ_REQ_SIZE 64
+#define MAX_MODES ((READ_REQ_SIZE - 6) / 4)
+/* The se401 compression algorithm uses a fixed quant factor, which
+   can be configured by setting the high nibble of the SE401_OPERATINGMODE
+   feature. This needs to exactly match what is in libv4l! */
+#define SE401_QUANT_FACT 8
+
+#include <linux/input.h>
+#include <linux/slab.h>
+#include "gspca.h"
+#include "se401.h"
+
+MODULE_AUTHOR("Hans de Goede <hdegoede@redhat.com>");
+MODULE_DESCRIPTION("Endpoints se401");
+MODULE_LICENSE("GPL");
+
+/* controls */
+enum e_ctrl {
+        BRIGHTNESS,
+        GAIN,
+        EXPOSURE,
+        FREQ,
+        NCTRL   /* number of controls */
+};
+
+/* exposure change state machine states */
+enum {
+        EXPO_CHANGED,
+        EXPO_DROP_FRAME,
+        EXPO_NO_CHANGE,
+};
+
+/* specific webcam descriptor */
+struct sd {
+        struct gspca_dev gspca_dev;     /* !! must be the first item */
+        struct gspca_ctrl ctrls[NCTRL];
+        struct v4l2_pix_format fmts[MAX_MODES];
+        int pixels_read;
+        int packet_read;
+        u8 packet[PACKET_SIZE];
+        u8 restart_stream;
+        u8 button_state;
+        u8 resetlevel;
+        u8 resetlevel_frame_count;
+        int resetlevel_adjust_dir;
+        int expo_change_state;
+};
+
+static void setbrightness(struct gspca_dev *gspca_dev);
+static void setgain(struct gspca_dev *gspca_dev);
+static void setexposure(struct gspca_dev *gspca_dev);
+
+static const struct ctrl sd_ctrls[NCTRL] = {
+[BRIGHTNESS] = {
+                {
+                        .id      = V4L2_CID_BRIGHTNESS,
+                        .type    = V4L2_CTRL_TYPE_INTEGER,
+                        .name    = "Brightness",
+                        .minimum = 0,
+                        .maximum = 255,
+                        .step    = 1,
+                        .default_value = 15,
+                },
+                .set_control = setbrightness
+        },
+[GAIN] = {
+                {
+                        .id      = V4L2_CID_GAIN,
+                        .type    = V4L2_CTRL_TYPE_INTEGER,
+                        .name    = "Gain",
+                        .minimum = 0,
+                        .maximum = 50, /* Really 63 but > 50 is not pretty */
+                        .step    = 1,
+                        .default_value = 25,
+                },
+                .set_control = setgain
+        },
+[EXPOSURE] = {
+                {
+                        .id = V4L2_CID_EXPOSURE,
+                        .type = V4L2_CTRL_TYPE_INTEGER,
+                        .name = "Exposure",
+                        .minimum = 0,
+                        .maximum = 32767,
+                        .step = 1,
+                        .default_value = 15000,
+                },
+                .set_control = setexposure
+        },
+[FREQ] = {
+                {
+                        .id      = V4L2_CID_POWER_LINE_FREQUENCY,
+                        .type    = V4L2_CTRL_TYPE_MENU,
+                        .name    = "Light frequency filter",
+                        .minimum = 0,
+                        .maximum = 2,
+                        .step    = 1,
+                        .default_value = 0,
+                },
+                .set_control = setexposure
+        },
+};
+
+static void se401_write_req(struct gspca_dev *gspca_dev, u16 req, u16 value,
+                            int silent)
+{
+        int err;
+
+        if (gspca_dev->usb_err < 0)
+                return;
+
+        err = usb_control_msg(gspca_dev->dev,
+                              usb_sndctrlpipe(gspca_dev->dev, 0), req,
+                              USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+                              value, 0, NULL, 0, 1000);
+        if (err < 0) {
+                if (!silent)
+                        err("write req failed req %#04x val %#04x error %d",
+                            req, value, err);
+                gspca_dev->usb_err = err;
+        }
+}
+
+static void se401_read_req(struct gspca_dev *gspca_dev, u16 req, int silent)
+{
+        int err;
+
+        if (gspca_dev->usb_err < 0)
+                return;
+
+        if (USB_BUF_SZ < READ_REQ_SIZE) {
+                err("USB_BUF_SZ too small!!");
+                gspca_dev->usb_err = -ENOBUFS;
+                return;
+        }
+
+        err = usb_control_msg(gspca_dev->dev,
+                              usb_rcvctrlpipe(gspca_dev->dev, 0), req,
+                              USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+                              0, 0, gspca_dev->usb_buf, READ_REQ_SIZE, 1000);
+        if (err < 0) {
+                if (!silent)
+                        err("read req failed req %#04x error %d", req, err);
+                gspca_dev->usb_err = err;
+        }
+}
+
+static void se401_set_feature(struct gspca_dev *gspca_dev,
+                              u16 selector, u16 param)
+{
+        int err;
+
+        if (gspca_dev->usb_err < 0)
+                return;
+
+        err = usb_control_msg(gspca_dev->dev,
+                              usb_sndctrlpipe(gspca_dev->dev, 0),
+                              SE401_REQ_SET_EXT_FEATURE,
+                              USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+                              param, selector, NULL, 0, 1000);
+        if (err < 0) {
+                err("set feature failed sel %#04x param %#04x error %d",
+                    selector, param, err);
+                gspca_dev->usb_err = err;
+        }
+}
+
+static int se401_get_feature(struct gspca_dev *gspca_dev, u16 selector)
+{
+        int err;
+
+        if (gspca_dev->usb_err < 0)
+                return gspca_dev->usb_err;
+
+        if (USB_BUF_SZ < 2) {
+                err("USB_BUF_SZ too small!!");
+                gspca_dev->usb_err = -ENOBUFS;
+                return gspca_dev->usb_err;
+        }
+
+        err = usb_control_msg(gspca_dev->dev,
+                              usb_rcvctrlpipe(gspca_dev->dev, 0),
+                              SE401_REQ_GET_EXT_FEATURE,
+                              USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+                              0, selector, gspca_dev->usb_buf, 2, 1000);
+        if (err < 0) {
+                err("get feature failed sel %#04x error %d", selector, err);
+                gspca_dev->usb_err = err;
+                return err;
+        }
+        return gspca_dev->usb_buf[0] | (gspca_dev->usb_buf[1] << 8);
+}
+
+static void setbrightness(struct gspca_dev *gspca_dev)
+{
+        struct sd *sd = (struct sd *) gspca_dev;
+
+        if (gspca_dev->ctrl_dis & (1 << BRIGHTNESS))
+                return;
+
+        /* HDG: this does not seem to do anything on my cam */
+        se401_write_req(gspca_dev, SE401_REQ_SET_BRT,
+                        sd->ctrls[BRIGHTNESS].val, 0);
+}
+
+static void setgain(struct gspca_dev *gspca_dev)
+{
+        struct sd *sd = (struct sd *) gspca_dev;
+        u16 gain = 63 - sd->ctrls[GAIN].val;
+
+        /* red color gain */
+        se401_set_feature(gspca_dev, HV7131_REG_ARCG, gain);
+        /* green color gain */
+        se401_set_feature(gspca_dev, HV7131_REG_AGCG, gain);
+        /* blue color gain */
+        se401_set_feature(gspca_dev, HV7131_REG_ABCG, gain);
+}
+
+static void setexposure(struct gspca_dev *gspca_dev)
+{
+        struct sd *sd = (struct sd *) gspca_dev;
+        int integration = sd->ctrls[EXPOSURE].val << 6;
+        u8 expose_h, expose_m, expose_l;
+
+        /* Do this before the set_feature calls, for proper timing wrt
+           the interrupt driven pkt_scan. Note we may still race but that
+           is not a big issue, the expo change state machine is merely for
+           avoiding underexposed frames getting send out, if one sneaks
+           through so be it */
+        sd->expo_change_state = EXPO_CHANGED;
+
+        if (sd->ctrls[FREQ].val == V4L2_CID_POWER_LINE_FREQUENCY_50HZ)
+                integration = integration - integration % 106667;
+        if (sd->ctrls[FREQ].val == V4L2_CID_POWER_LINE_FREQUENCY_60HZ)
+                integration = integration - integration % 88889;
+
+        expose_h = (integration >> 16);
+        expose_m = (integration >> 8);
+        expose_l = integration;
+
+        /* integration time low */
+        se401_set_feature(gspca_dev, HV7131_REG_TITL, expose_l);
+        /* integration time mid */
+        se401_set_feature(gspca_dev, HV7131_REG_TITM, expose_m);
+        /* integration time high */
+        se401_set_feature(gspca_dev, HV7131_REG_TITU, expose_h);
+}
+
+static int sd_config(struct gspca_dev *gspca_dev,
+                        const struct usb_device_id *id)
+{
+        struct sd *sd = (struct sd *)gspca_dev;
+        struct cam *cam = &gspca_dev->cam;
+        u8 *cd = gspca_dev->usb_buf;
+        int i, j, n;
+        int widths[MAX_MODES], heights[MAX_MODES];
+
+        /* Read the camera descriptor */
+        se401_read_req(gspca_dev, SE401_REQ_GET_CAMERA_DESCRIPTOR, 1);
+        if (gspca_dev->usb_err) {
+                /* Sometimes after being idle for a while the se401 won't
+                   respond and needs a good kicking  */
+                usb_reset_device(gspca_dev->dev);
+                gspca_dev->usb_err = 0;
+                se401_read_req(gspca_dev, SE401_REQ_GET_CAMERA_DESCRIPTOR, 0);
+        }
+
+        /* Some cameras start with their LED on */
+        se401_write_req(gspca_dev, SE401_REQ_LED_CONTROL, 0, 0);
+        if (gspca_dev->usb_err)
+                return gspca_dev->usb_err;
+
+        if (cd[1] != 0x41) {
+                err("Wrong descriptor type");
+                return -ENODEV;
+        }
+
+        if (!(cd[2] & SE401_FORMAT_BAYER)) {
+                err("Bayer format not supported!");
+                return -ENODEV;
+        }
+
+        if (cd[3])
+                info("ExtraFeatures: %d", cd[3]);
+
+        n = cd[4] | (cd[5] << 8);
+        if (n > MAX_MODES) {
+                err("Too many frame sizes");
+                return -ENODEV;
+        }
+
+        for (i = 0; i < n ; i++) {
+                widths[i] = cd[6 + i * 4 + 0] | (cd[6 + i * 4 + 1] << 8);
+                heights[i] = cd[6 + i * 4 + 2] | (cd[6 + i * 4 + 3] << 8);
+        }
+
+        for (i = 0; i < n ; i++) {
+                sd->fmts[i].width = widths[i];
+                sd->fmts[i].height = heights[i];
+                sd->fmts[i].field = V4L2_FIELD_NONE;
+                sd->fmts[i].colorspace = V4L2_COLORSPACE_SRGB;
+                sd->fmts[i].priv = 1;
+
+                /* janggu compression only works for 1/4th or 1/16th res */
+                for (j = 0; j < n; j++) {
+                        if (widths[j] / 2 == widths[i] &&
+                            heights[j] / 2 == heights[i]) {
+                                sd->fmts[i].priv = 2;
+                                break;
+                        }
+                }
+                /* 1/16th if available too is better then 1/4th, because
+                   we then use a larger area of the sensor */
+                for (j = 0; j < n; j++) {
+                        if (widths[j] / 4 == widths[i] &&
+                            heights[j] / 4 == heights[i]) {
+                                sd->fmts[i].priv = 4;
+                                break;
+                        }
+                }
+
+                if (sd->fmts[i].priv == 1) {
+                        /* Not a 1/4th or 1/16th res, use bayer */
+                        sd->fmts[i].pixelformat = V4L2_PIX_FMT_SBGGR8;
+                        sd->fmts[i].bytesperline = widths[i];
+                        sd->fmts[i].sizeimage = widths[i] * heights[i];
+                        info("Frame size: %dx%d bayer", widths[i], heights[i]);
+                } else {
+                        /* Found a match use janggu compression */
+                        sd->fmts[i].pixelformat = V4L2_PIX_FMT_SE401;
+                        sd->fmts[i].bytesperline = 0;
+                        sd->fmts[i].sizeimage = widths[i] * heights[i] * 3;
+                        info("Frame size: %dx%d 1/%dth janggu",
+                             widths[i], heights[i],
+                             sd->fmts[i].priv * sd->fmts[i].priv);
+                }
+        }
+
+        cam->cam_mode = sd->fmts;
+        cam->nmodes = n;
+        cam->bulk = 1;
+        cam->bulk_size = BULK_SIZE;
+        cam->bulk_nurbs = 4;
+        cam->ctrls = sd->ctrls;
+        gspca_dev->nbalt = 1;  /* Ignore the bogus isoc alt settings */
+        sd->resetlevel = 0x2d; /* Set initial resetlevel */
+
+        /* See if the camera supports brightness */
+        se401_read_req(gspca_dev, SE401_REQ_GET_BRT, 1);
+        if (gspca_dev->usb_err) {
+                gspca_dev->ctrl_dis = (1 << BRIGHTNESS);
+                gspca_dev->usb_err = 0;
+        }
+
+        return 0;
+}
+
+/* this function is called at probe and resume time */
+static int sd_init(struct gspca_dev *gspca_dev)
+{
+        return 0;
+}
+
+/* -- start the camera -- */
+static int sd_start(struct gspca_dev *gspca_dev)
+{
+        struct sd *sd = (struct sd *)gspca_dev;
+        int mult = gspca_dev->cam.cam_mode[gspca_dev->curr_mode].priv;
+        int mode = 0;
+
+        se401_write_req(gspca_dev, SE401_REQ_CAMERA_POWER, 1, 1);
+        if (gspca_dev->usb_err) {
+                /* Sometimes after being idle for a while the se401 won't
+                   respond and needs a good kicking  */
+                usb_reset_device(gspca_dev->dev);
+                gspca_dev->usb_err = 0;
+                se401_write_req(gspca_dev, SE401_REQ_CAMERA_POWER, 1, 0);
+        }
+        se401_write_req(gspca_dev, SE401_REQ_LED_CONTROL, 1, 0);
+
+        se401_set_feature(gspca_dev, HV7131_REG_MODE_B, 0x05);
+
+        /* set size + mode */
+        se401_write_req(gspca_dev, SE401_REQ_SET_WIDTH,
+                        gspca_dev->width * mult, 0);
+        se401_write_req(gspca_dev, SE401_REQ_SET_HEIGHT,
+                        gspca_dev->height * mult, 0);
+        /*
+         * HDG: disabled this as it does not seem to do anything
+         * se401_write_req(gspca_dev, SE401_REQ_SET_OUTPUT_MODE,
+         *                 SE401_FORMAT_BAYER, 0);
+         */
+
+        switch (mult) {
+        case 1: /* Raw bayer */
+                mode = 0x03; break;
+        case 2: /* 1/4th janggu */
+                mode = SE401_QUANT_FACT << 4; break;
+        case 4: /* 1/16th janggu */
+                mode = (SE401_QUANT_FACT << 4) | 0x02; break;
+        }
+        se401_set_feature(gspca_dev, SE401_OPERATINGMODE, mode);
+
+        setbrightness(gspca_dev);
+        setgain(gspca_dev);
+        setexposure(gspca_dev);
+        se401_set_feature(gspca_dev, HV7131_REG_ARLV, sd->resetlevel);
+
+        sd->packet_read = 0;
+        sd->pixels_read = 0;
+        sd->restart_stream = 0;
+        sd->resetlevel_frame_count = 0;
+        sd->resetlevel_adjust_dir = 0;
+        sd->expo_change_state = EXPO_NO_CHANGE;
+
+        se401_write_req(gspca_dev, SE401_REQ_START_CONTINUOUS_CAPTURE, 0, 0);
+
+        return gspca_dev->usb_err;
+}
+
+static void sd_stopN(struct gspca_dev *gspca_dev)
+{
+        se401_write_req(gspca_dev, SE401_REQ_STOP_CONTINUOUS_CAPTURE, 0, 0);
+        se401_write_req(gspca_dev, SE401_REQ_LED_CONTROL, 0, 0);
+        se401_write_req(gspca_dev, SE401_REQ_CAMERA_POWER, 0, 0);
+}
+
+static void sd_dq_callback(struct gspca_dev *gspca_dev)
+{
+        struct sd *sd = (struct sd *)gspca_dev;
+        unsigned int ahrc, alrc;
+        int oldreset, adjust_dir;
+
+        /* Restart the stream if requested do so by pkt_scan */
+        if (sd->restart_stream) {
+                sd_stopN(gspca_dev);
+                sd_start(gspca_dev);
+                sd->restart_stream = 0;
+        }
+
+        /* Automatically adjust sensor reset level
+           Hyundai have some really nice docs about this and other sensor
+           related stuff on their homepage: www.hei.co.kr */
+        sd->resetlevel_frame_count++;
+        if (sd->resetlevel_frame_count < 20)
+                return;
+
+        /* For some reason this normally read-only register doesn't get reset
+           to zero after reading them just once... */
+        se401_get_feature(gspca_dev, HV7131_REG_HIREFNOH);
+        se401_get_feature(gspca_dev, HV7131_REG_HIREFNOL);
+        se401_get_feature(gspca_dev, HV7131_REG_LOREFNOH);
+        se401_get_feature(gspca_dev, HV7131_REG_LOREFNOL);
+        ahrc = 256*se401_get_feature(gspca_dev, HV7131_REG_HIREFNOH) +
+            se401_get_feature(gspca_dev, HV7131_REG_HIREFNOL);
+        alrc = 256*se401_get_feature(gspca_dev, HV7131_REG_LOREFNOH) +
+            se401_get_feature(gspca_dev, HV7131_REG_LOREFNOL);
+
+        /* Not an exact science, but it seems to work pretty well... */
+        oldreset = sd->resetlevel;
+        if (alrc > 10) {
+                while (alrc >= 10 && sd->resetlevel < 63) {
+                        sd->resetlevel++;
+                        alrc /= 2;
+                }
+        } else if (ahrc > 20) {
+                while (ahrc >= 20 && sd->resetlevel > 0) {
+                        sd->resetlevel--;
+                        ahrc /= 2;
+                }
+        }
+        /* Detect ping-pong-ing and halve adjustment to avoid overshoot */
+        if (sd->resetlevel > oldreset)
+                adjust_dir = 1;
+        else
+                adjust_dir = -1;
+        if (sd->resetlevel_adjust_dir &&
+            sd->resetlevel_adjust_dir != adjust_dir)
+                sd->resetlevel = oldreset + (sd->resetlevel - oldreset) / 2;
+
+        if (sd->resetlevel != oldreset) {
+                sd->resetlevel_adjust_dir = adjust_dir;
+                se401_set_feature(gspca_dev, HV7131_REG_ARLV, sd->resetlevel);
+        }
+
+        sd->resetlevel_frame_count = 0;
+}
+
+static void sd_complete_frame(struct gspca_dev *gspca_dev, u8 *data, int len)
+{
+        struct sd *sd = (struct sd *)gspca_dev;
+
+        switch (sd->expo_change_state) {
+        case EXPO_CHANGED:
+                /* The exposure was changed while this frame
+                   was being send, so this frame is ok */
+                sd->expo_change_state = EXPO_DROP_FRAME;
+                break;
+        case EXPO_DROP_FRAME:
+                /* The exposure was changed while this frame
+                   was being captured, drop it! */
+                gspca_dev->last_packet_type = DISCARD_PACKET;
+                sd->expo_change_state = EXPO_NO_CHANGE;
+                break;
+        case EXPO_NO_CHANGE:
+                break;
+        }
+        gspca_frame_add(gspca_dev, LAST_PACKET, data, len);
+}
+
+static void sd_pkt_scan_janggu(struct gspca_dev *gspca_dev, u8 *data, int len)
+{
+        struct sd *sd = (struct sd *)gspca_dev;
+        int imagesize = gspca_dev->width * gspca_dev->height;
+        int i, plen, bits, pixels, info, count;
+
+        if (sd->restart_stream)
+                return;
+
+        /* Sometimes a 1024 bytes garbage bulk packet is send between frames */
+        if (gspca_dev->last_packet_type == LAST_PACKET && len == 1024) {
+                gspca_dev->last_packet_type = DISCARD_PACKET;
+                return;
+        }
+
+        i = 0;
+        while (i < len) {
+                /* Read header if not already be present from prev bulk pkt */
+                if (sd->packet_read < 4) {
+                        count = 4 - sd->packet_read;
+                        if (count > len - i)
+                                count = len - i;
+                        memcpy(&sd->packet[sd->packet_read], &data[i], count);
+                        sd->packet_read += count;
+                        i += count;
+                        if (sd->packet_read < 4)
+                                break;
+                }
+                bits   = sd->packet[3] + (sd->packet[2] << 8);
+                pixels = sd->packet[1] + ((sd->packet[0] & 0x3f) << 8);
+                info   = (sd->packet[0] & 0xc0) >> 6;
+                plen   = ((bits + 47) >> 4) << 1;
+                /* Sanity checks */
+                if (plen > 1024) {
+                        err("invalid packet len %d restarting stream", plen);
+                        goto error;
+                }
+                if (info == 3) {
+                        err("unknown frame info value restarting stream");
+                        goto error;
+                }
+
+                /* Read (remainder of) packet contents */
+                count = plen - sd->packet_read;
+                if (count > len - i)
+                        count = len - i;
+                memcpy(&sd->packet[sd->packet_read], &data[i], count);
+                sd->packet_read += count;
+                i += count;
+                if (sd->packet_read < plen)
+                        break;
+
+                sd->pixels_read += pixels;
+                sd->packet_read = 0;
+
+                switch (info) {
+                case 0: /* Frame data */
+                        gspca_frame_add(gspca_dev, INTER_PACKET, sd->packet,
+                                        plen);
+                        break;
+                case 1: /* EOF */
+                        if (sd->pixels_read != imagesize) {
+                                err("frame size %d expected %d",
+                                    sd->pixels_read, imagesize);
+                                goto error;
+                        }
+                        sd_complete_frame(gspca_dev, sd->packet, plen);
+                        return; /* Discard the rest of the bulk packet !! */
+                case 2: /* SOF */
+                        gspca_frame_add(gspca_dev, FIRST_PACKET, sd->packet,
+                                        plen);
+                        sd->pixels_read = pixels;
+                        break;
+                }
+        }
+        return;
+
+error:
+        sd->restart_stream = 1;
+        /* Give userspace a 0 bytes frame, so our dq callback gets
+           called and it can restart the stream */
+        gspca_frame_add(gspca_dev, FIRST_PACKET, NULL, 0);
+        gspca_frame_add(gspca_dev, LAST_PACKET, NULL, 0);
+}
+
+static void sd_pkt_scan_bayer(struct gspca_dev *gspca_dev, u8 *data, int len)
+{
+        struct cam *cam = &gspca_dev->cam;
+        int imagesize = cam->cam_mode[gspca_dev->curr_mode].sizeimage;
+
+        if (gspca_dev->image_len == 0) {
+                gspca_frame_add(gspca_dev, FIRST_PACKET, data, len);
+                return;
+        }
+
+        if (gspca_dev->image_len + len >= imagesize) {
+                sd_complete_frame(gspca_dev, data, len);
+                return;
+        }
+
+        gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
+}
+
+static void sd_pkt_scan(struct gspca_dev *gspca_dev, u8 *data, int len)
+{
+        int mult = gspca_dev->cam.cam_mode[gspca_dev->curr_mode].priv;
+
+        if (len == 0)
+                return;
+
+        if (mult == 1) /* mult == 1 means raw bayer */
+                sd_pkt_scan_bayer(gspca_dev, data, len);
+        else
+                sd_pkt_scan_janggu(gspca_dev, data, len);
+}
+
+static int sd_querymenu(struct gspca_dev *gspca_dev,
+                        struct v4l2_querymenu *menu)
+{
+        switch (menu->id) {
+        case V4L2_CID_POWER_LINE_FREQUENCY:
+                switch (menu->index) {
+                case V4L2_CID_POWER_LINE_FREQUENCY_DISABLED:
+                        strcpy((char *) menu->name, "NoFliker");
+                        return 0;
+                case V4L2_CID_POWER_LINE_FREQUENCY_50HZ:
+                        strcpy((char *) menu->name, "50 Hz");
+                        return 0;
+                case V4L2_CID_POWER_LINE_FREQUENCY_60HZ:
+                        strcpy((char *) menu->name, "60 Hz");
+                        return 0;
+                }
+                break;
+        }
+        return -EINVAL;
+}
+
+#if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
+static int sd_int_pkt_scan(struct gspca_dev *gspca_dev, u8 *data, int len)
+{
+        struct sd *sd = (struct sd *)gspca_dev;
+        u8 state;
+
+        if (len != 2)
+                return -EINVAL;
+
+        switch (data[0]) {
+        case 0:
+        case 1:
+                state = data[0];
+                break;
+        default:
+                return -EINVAL;
+        }
+        if (sd->button_state != state) {
+                input_report_key(gspca_dev->input_dev, KEY_CAMERA, state);
+                input_sync(gspca_dev->input_dev);
+                sd->button_state = state;
+        }
+
+        return 0;
+}
+#endif
+
+/* sub-driver description */
+static const struct sd_desc sd_desc = {
+        .name = MODULE_NAME,
+        .ctrls = sd_ctrls,
+        .nctrls = ARRAY_SIZE(sd_ctrls),
+        .config = sd_config,
+        .init = sd_init,
+        .start = sd_start,
+        .stopN = sd_stopN,
+        .dq_callback = sd_dq_callback,
+        .pkt_scan = sd_pkt_scan,
+        .querymenu = sd_querymenu,
+#if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
+        .int_pkt_scan = sd_int_pkt_scan,
+#endif
+};
+
+/* -- module initialisation -- */
+static const struct usb_device_id device_table[] = {
+        {USB_DEVICE(0x03e8, 0x0004)}, /* Endpoints/Aox SE401 */
+        {USB_DEVICE(0x0471, 0x030b)}, /* Philips PCVC665K */
+        {USB_DEVICE(0x047d, 0x5001)}, /* Kensington 67014 */
+        {USB_DEVICE(0x047d, 0x5002)}, /* Kensington 6701(5/7) */
+        {USB_DEVICE(0x047d, 0x5003)}, /* Kensington 67016 */
+        {}
+};
+MODULE_DEVICE_TABLE(usb, device_table);
+
+/* -- device connect -- */
+static int sd_probe(struct usb_interface *intf,
+                        const struct usb_device_id *id)
+{
+        return gspca_dev_probe(intf, id, &sd_desc, sizeof(struct sd),
+                                THIS_MODULE);
+}
+
+static int sd_pre_reset(struct usb_interface *intf)
+{
+        return 0;
+}
+
+static int sd_post_reset(struct usb_interface *intf)
+{
+        return 0;
+}
+
+static struct usb_driver sd_driver = {
+        .name = MODULE_NAME,
+        .id_table = device_table,
+        .probe = sd_probe,
+        .disconnect = gspca_disconnect,
+#ifdef CONFIG_PM
+        .suspend = gspca_suspend,
+        .resume = gspca_resume,
+#endif
+        .pre_reset = sd_pre_reset,
+        .post_reset = sd_post_reset,
+};
+
+/* -- module insert / remove -- */
+static int __init sd_mod_init(void)
+{
+        return usb_register(&sd_driver);
+}
+static void __exit sd_mod_exit(void)
+{
+        usb_deregister(&sd_driver);
+}
+
+module_init(sd_mod_init);
+module_exit(sd_mod_exit);
diff --git a/drivers/media/video/gspca/se401.h b/drivers/media/video/gspca/se401.h
new file mode 100644
index 00000000000..96d8ebf3cf5
--- /dev/null
+++ b/drivers/media/video/gspca/se401.h
@@ -0,0 +1,90 @@
+/*
+ * GSPCA Endpoints (formerly known as AOX) se401 USB Camera sub Driver
+ *
+ * Copyright (C) 2011 Hans de Goede <hdegoede@redhat.com>
+ *
+ * Based on the v4l1 se401 driver which is:
+ *
+ * Copyright (c) 2000 Jeroen B. Vreeken (pe1rxq@amsat.org)
+ *
+ * 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 SE401_REQ_GET_CAMERA_DESCRIPTOR         0x06
+#define SE401_REQ_START_CONTINUOUS_CAPTURE      0x41
+#define SE401_REQ_STOP_CONTINUOUS_CAPTURE       0x42
+#define SE401_REQ_CAPTURE_FRAME                 0x43
+#define SE401_REQ_GET_BRT                       0x44
+#define SE401_REQ_SET_BRT                       0x45
+#define SE401_REQ_GET_WIDTH                     0x4c
+#define SE401_REQ_SET_WIDTH                     0x4d
+#define SE401_REQ_GET_HEIGHT                    0x4e
+#define SE401_REQ_SET_HEIGHT                    0x4f
+#define SE401_REQ_GET_OUTPUT_MODE               0x50
+#define SE401_REQ_SET_OUTPUT_MODE               0x51
+#define SE401_REQ_GET_EXT_FEATURE               0x52
+#define SE401_REQ_SET_EXT_FEATURE               0x53
+#define SE401_REQ_CAMERA_POWER                  0x56
+#define SE401_REQ_LED_CONTROL                   0x57
+#define SE401_REQ_BIOS                          0xff
+
+#define SE401_BIOS_READ                         0x07
+
+#define SE401_FORMAT_BAYER      0x40
+
+/* Hyundai hv7131b registers
+   7121 and 7141 should be the same (haven't really checked...) */
+/* Mode registers: */
+#define HV7131_REG_MODE_A               0x00
+#define HV7131_REG_MODE_B               0x01
+#define HV7131_REG_MODE_C               0x02
+/* Frame registers: */
+#define HV7131_REG_FRSU         0x10
+#define HV7131_REG_FRSL         0x11
+#define HV7131_REG_FCSU         0x12
+#define HV7131_REG_FCSL         0x13
+#define HV7131_REG_FWHU         0x14
+#define HV7131_REG_FWHL         0x15
+#define HV7131_REG_FWWU         0x16
+#define HV7131_REG_FWWL         0x17
+/* Timing registers: */
+#define HV7131_REG_THBU         0x20
+#define HV7131_REG_THBL         0x21
+#define HV7131_REG_TVBU         0x22
+#define HV7131_REG_TVBL         0x23
+#define HV7131_REG_TITU         0x25
+#define HV7131_REG_TITM         0x26
+#define HV7131_REG_TITL         0x27
+#define HV7131_REG_TMCD         0x28
+/* Adjust Registers: */
+#define HV7131_REG_ARLV         0x30
+#define HV7131_REG_ARCG         0x31
+#define HV7131_REG_AGCG         0x32
+#define HV7131_REG_ABCG         0x33
+#define HV7131_REG_APBV         0x34
+#define HV7131_REG_ASLP         0x54
+/* Offset Registers: */
+#define HV7131_REG_OFSR         0x50
+#define HV7131_REG_OFSG         0x51
+#define HV7131_REG_OFSB         0x52
+/* REset level statistics registers: */
+#define HV7131_REG_LOREFNOH     0x57
+#define HV7131_REG_LOREFNOL     0x58
+#define HV7131_REG_HIREFNOH     0x59
+#define HV7131_REG_HIREFNOL     0x5a
+
+/* se401 registers */
+#define SE401_OPERATINGMODE     0x2000
diff --git a/drivers/media/video/gspca/sunplus.c b/drivers/media/video/gspca/sunplus.c
index b089c0d3ee9..6ec23290218 100644
--- a/drivers/media/video/gspca/sunplus.c
+++ b/drivers/media/video/gspca/sunplus.c
@@ -247,7 +247,6 @@ static const struct cmd spca504A_clicksmart420_init_data[] = {
         {0x30, 0x0004, 0x000a},
         {0xb0, 0x0001, 0x0000},
 
-
         {0xa1, 0x0080, 0x0001},
         {0x30, 0x0049, 0x0000},
         {0x30, 0x0060, 0x0005},
@@ -256,8 +255,6 @@ static const struct cmd spca504A_clicksmart420_init_data[] = {
         {0x00, 0x0000, 0x2000},
         {0x00, 0x0013, 0x2301},
         {0x00, 0x0003, 0x2000},
-        {0x00, 0x0000, 0x2000},
-
 };
 
 /* clicksmart 420 open data ? */
diff --git a/drivers/media/video/gspca/t613.c b/drivers/media/video/gspca/t613.c
index 7e762d55109..d1d733b9359 100644
--- a/drivers/media/video/gspca/t613.c
+++ b/drivers/media/video/gspca/t613.c
@@ -1387,7 +1387,7 @@ static int sd_querymenu(struct gspca_dev *gspca_dev,
                 return 0;
         case V4L2_CID_EFFECTS:
                 if ((unsigned) menu->index < ARRAY_SIZE(effects_control)) {
-                        strncpy((char *) menu->name,
+                        strlcpy((char *) menu->name,
                                 effects_control[menu->index],
                                 sizeof menu->name);
                         return 0;
diff --git a/drivers/media/video/hdpvr/hdpvr-core.c b/drivers/media/video/hdpvr/hdpvr-core.c
index 5f1db46beb4..441dacf642b 100644
--- a/drivers/media/video/hdpvr/hdpvr-core.c
+++ b/drivers/media/video/hdpvr/hdpvr-core.c
@@ -474,5 +474,6 @@ module_init(hdpvr_init);
 module_exit(hdpvr_exit);
 
 MODULE_LICENSE("GPL");
+MODULE_VERSION("0.2.1");
 MODULE_AUTHOR("Janne Grunau");
 MODULE_DESCRIPTION("Hauppauge HD PVR driver");
diff --git a/drivers/media/video/hdpvr/hdpvr-video.c b/drivers/media/video/hdpvr/hdpvr-video.c
index 514aea76eaa..087f7c08cb8 100644
--- a/drivers/media/video/hdpvr/hdpvr-video.c
+++ b/drivers/media/video/hdpvr/hdpvr-video.c
@@ -17,7 +17,6 @@
 #include <linux/uaccess.h>
 #include <linux/usb.h>
 #include <linux/mutex.h>
-#include <linux/version.h>
 #include <linux/workqueue.h>
 
 #include <linux/videodev2.h>
@@ -574,7 +573,6 @@ static int vidioc_querycap(struct file *file, void  *priv,
         strcpy(cap->driver, "hdpvr");
         strcpy(cap->card, "Hauppauge HD PVR");
         usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
-        cap->version = HDPVR_VERSION;
         cap->capabilities =     V4L2_CAP_VIDEO_CAPTURE |
                                 V4L2_CAP_AUDIO         |
                                 V4L2_CAP_READWRITE;
diff --git a/drivers/media/video/hdpvr/hdpvr.h b/drivers/media/video/hdpvr/hdpvr.h
index 072f23c570f..d6439db1d18 100644
--- a/drivers/media/video/hdpvr/hdpvr.h
+++ b/drivers/media/video/hdpvr/hdpvr.h
@@ -18,12 +18,6 @@
 #include <media/v4l2-device.h>
 #include <media/ir-kbd-i2c.h>
 
-#define HDPVR_MAJOR_VERSION 0
-#define HDPVR_MINOR_VERSION 2
-#define HDPVR_RELEASE 0
-#define HDPVR_VERSION \
-        KERNEL_VERSION(HDPVR_MAJOR_VERSION, HDPVR_MINOR_VERSION, HDPVR_RELEASE)
-
 #define HDPVR_MAX 8
 #define HDPVR_I2C_MAX_SIZE 128
 
diff --git a/drivers/media/video/ivtv/ivtv-driver.h b/drivers/media/video/ivtv/ivtv-driver.h
index 84bdf0f42a8..8f9cc17b518 100644
--- a/drivers/media/video/ivtv/ivtv-driver.h
+++ b/drivers/media/video/ivtv/ivtv-driver.h
@@ -36,7 +36,6 @@
  *                using information provided by Jiun-Kuei Jung @ AVerMedia.
  */
 
-#include <linux/version.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/delay.h>
diff --git a/drivers/media/video/ivtv/ivtv-fileops.c b/drivers/media/video/ivtv/ivtv-fileops.c
index a7f54b010a5..38f052257f4 100644
--- a/drivers/media/video/ivtv/ivtv-fileops.c
+++ b/drivers/media/video/ivtv/ivtv-fileops.c
@@ -722,8 +722,8 @@ unsigned int ivtv_v4l2_dec_poll(struct file *filp, poll_table *wait)
 
         /* If there are subscribed events, then only use the new event
            API instead of the old video.h based API. */
-        if (!list_empty(&id->fh.events->subscribed)) {
-                poll_wait(filp, &id->fh.events->wait, wait);
+        if (!list_empty(&id->fh.subscribed)) {
+                poll_wait(filp, &id->fh.wait, wait);
                 /* Turn off the old-style vsync events */
                 clear_bit(IVTV_F_I_EV_VSYNC_ENABLED, &itv->i_flags);
                 if (v4l2_event_pending(&id->fh))
@@ -750,6 +750,7 @@ unsigned int ivtv_v4l2_enc_poll(struct file *filp, poll_table * wait)
         struct ivtv *itv = id->itv;
         struct ivtv_stream *s = &itv->streams[id->type];
         int eof = test_bit(IVTV_F_S_STREAMOFF, &s->s_flags);
+        unsigned res = 0;
 
         /* Start a capture if there is none */
         if (!eof && !test_bit(IVTV_F_S_STREAMING, &s->s_flags)) {
@@ -769,12 +770,16 @@ unsigned int ivtv_v4l2_enc_poll(struct file *filp, poll_table * wait)
         /* add stream's waitq to the poll list */
         IVTV_DEBUG_HI_FILE("Encoder poll\n");
         poll_wait(filp, &s->waitq, wait);
+        if (v4l2_event_pending(&id->fh))
+                res |= POLLPRI;
+        else
+                poll_wait(filp, &id->fh.wait, wait);
 
         if (s->q_full.length || s->q_io.length)
-                return POLLIN | POLLRDNORM;
+                return res | POLLIN | POLLRDNORM;
         if (eof)
-                return POLLHUP;
-        return 0;
+                return res | POLLHUP;
+        return res;
 }
 
 void ivtv_stop_capture(struct ivtv_open_id *id, int gop_end)
@@ -961,10 +966,6 @@ static int ivtv_serialized_open(struct ivtv_stream *s, struct file *filp)
                 return -ENOMEM;
         }
         v4l2_fh_init(&item->fh, s->vdev);
-        if (s->type == IVTV_DEC_STREAM_TYPE_YUV ||
-            s->type == IVTV_DEC_STREAM_TYPE_MPG) {
-                res = v4l2_event_alloc(&item->fh, 60);
-        }
         if (res < 0) {
                 v4l2_fh_exit(&item->fh);
                 kfree(item);
diff --git a/drivers/media/video/ivtv/ivtv-ioctl.c b/drivers/media/video/ivtv/ivtv-ioctl.c
index 120c7d8e089..3e5c090af11 100644
--- a/drivers/media/video/ivtv/ivtv-ioctl.c
+++ b/drivers/media/video/ivtv/ivtv-ioctl.c
@@ -757,7 +757,6 @@ static int ivtv_querycap(struct file *file, void *fh, struct v4l2_capability *vc
         strlcpy(vcap->driver, IVTV_DRIVER_NAME, sizeof(vcap->driver));
         strlcpy(vcap->card, itv->card_name, sizeof(vcap->card));
         snprintf(vcap->bus_info, sizeof(vcap->bus_info), "PCI:%s", pci_name(itv->pdev));
-        vcap->version = IVTV_DRIVER_VERSION;        /* version */
         vcap->capabilities = itv->v4l2_cap;         /* capabilities */
         return 0;
 }
@@ -1451,11 +1450,11 @@ static int ivtv_subscribe_event(struct v4l2_fh *fh, struct v4l2_event_subscripti
         switch (sub->type) {
         case V4L2_EVENT_VSYNC:
         case V4L2_EVENT_EOS:
-                break;
+        case V4L2_EVENT_CTRL:
+                return v4l2_event_subscribe(fh, sub, 0);
         default:
                 return -EINVAL;
         }
-        return v4l2_event_subscribe(fh, sub);
 }
 
 static int ivtv_log_status(struct file *file, void *fh)
diff --git a/drivers/media/video/ivtv/ivtv-version.h b/drivers/media/video/ivtv/ivtv-version.h
index b67a4048f5a..a20f346fcad 100644
--- a/drivers/media/video/ivtv/ivtv-version.h
+++ b/drivers/media/video/ivtv/ivtv-version.h
@@ -21,11 +21,6 @@
 #define IVTV_VERSION_H
 
 #define IVTV_DRIVER_NAME "ivtv"
-#define IVTV_DRIVER_VERSION_MAJOR 1
-#define IVTV_DRIVER_VERSION_MINOR 4
-#define IVTV_DRIVER_VERSION_PATCHLEVEL 2
-
-#define IVTV_VERSION __stringify(IVTV_DRIVER_VERSION_MAJOR) "." __stringify(IVTV_DRIVER_VERSION_MINOR) "." __stringify(IVTV_DRIVER_VERSION_PATCHLEVEL)
-#define IVTV_DRIVER_VERSION KERNEL_VERSION(IVTV_DRIVER_VERSION_MAJOR,IVTV_DRIVER_VERSION_MINOR,IVTV_DRIVER_VERSION_PATCHLEVEL)
+#define IVTV_VERSION "1.4.3"
 
 #endif
diff --git a/drivers/media/video/m5mols/m5mols_capture.c b/drivers/media/video/m5mols/m5mols_capture.c
index a45d8f098e0..3248ac80571 100644
--- a/drivers/media/video/m5mols/m5mols_capture.c
+++ b/drivers/media/video/m5mols/m5mols_capture.c
@@ -18,7 +18,6 @@
 #include <linux/irq.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
-#include <linux/version.h>
 #include <linux/gpio.h>
 #include <linux/regulator/consumer.h>
 #include <linux/videodev2.h>
diff --git a/drivers/media/video/m5mols/m5mols_core.c b/drivers/media/video/m5mols/m5mols_core.c
index 43c68f51c5c..fb8e4a7a9dd 100644
--- a/drivers/media/video/m5mols/m5mols_core.c
+++ b/drivers/media/video/m5mols/m5mols_core.c
@@ -18,7 +18,6 @@
 #include <linux/irq.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
-#include <linux/version.h>
 #include <linux/gpio.h>
 #include <linux/regulator/consumer.h>
 #include <linux/videodev2.h>
diff --git a/drivers/media/video/marvell-ccic/Kconfig b/drivers/media/video/marvell-ccic/Kconfig
new file mode 100644
index 00000000000..bf739e3b339
--- /dev/null
+++ b/drivers/media/video/marvell-ccic/Kconfig
@@ -0,0 +1,23 @@
+config VIDEO_CAFE_CCIC
+        tristate "Marvell 88ALP01 (Cafe) CMOS Camera Controller support"
+        depends on PCI && I2C && VIDEO_V4L2
+        select VIDEO_OV7670
+        select VIDEOBUF2_VMALLOC
+        select VIDEOBUF2_DMA_CONTIG
+        ---help---
+          This is a video4linux2 driver for the Marvell 88ALP01 integrated
+          CMOS camera controller.  This is the controller found on first-
+          generation OLPC systems.
+
+config VIDEO_MMP_CAMERA
+        tristate "Marvell Armada 610 integrated camera controller support"
+        depends on ARCH_MMP && I2C && VIDEO_V4L2
+        select VIDEO_OV7670
+        select I2C_GPIO
+        select VIDEOBUF2_DMA_SG
+        ---help---
+          This is a Video4Linux2 driver for the integrated camera
+          controller found on Marvell Armada 610 application
+          processors (and likely beyond).  This is the controller found
+          in OLPC XO 1.75 systems.
+
diff --git a/drivers/media/video/marvell-ccic/Makefile b/drivers/media/video/marvell-ccic/Makefile
new file mode 100644
index 00000000000..05a792c579a
--- /dev/null
+++ b/drivers/media/video/marvell-ccic/Makefile
@@ -0,0 +1,6 @@
+obj-$(CONFIG_VIDEO_CAFE_CCIC) += cafe_ccic.o
+cafe_ccic-y := cafe-driver.o mcam-core.o
+
+obj-$(CONFIG_VIDEO_MMP_CAMERA) += mmp_camera.o
+mmp_camera-y := mmp-driver.o mcam-core.o
+
diff --git a/drivers/media/video/marvell-ccic/cafe-driver.c b/drivers/media/video/marvell-ccic/cafe-driver.c
new file mode 100644
index 00000000000..d030f9beae8
--- /dev/null
+++ b/drivers/media/video/marvell-ccic/cafe-driver.c
@@ -0,0 +1,654 @@
+/*
+ * A driver for the CMOS camera controller in the Marvell 88ALP01 "cafe"
+ * multifunction chip.  Currently works with the Omnivision OV7670
+ * sensor.
+ *
+ * The data sheet for this device can be found at:
+ *    http://www.marvell.com/products/pc_connectivity/88alp01/
+ *
+ * Copyright 2006-11 One Laptop Per Child Association, Inc.
+ * Copyright 2006-11 Jonathan Corbet <corbet@lwn.net>
+ *
+ * Written by Jonathan Corbet, corbet@lwn.net.
+ *
+ * v4l2_device/v4l2_subdev conversion by:
+ * Copyright (C) 2009 Hans Verkuil <hverkuil@xs4all.nl>
+ *
+ * This file may be distributed under the terms of the GNU General
+ * Public License, version 2.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <linux/device.h>
+#include <linux/wait.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+
+#include "mcam-core.h"
+
+#define CAFE_VERSION 0x000002
+
+
+/*
+ * Parameters.
+ */
+MODULE_AUTHOR("Jonathan Corbet <corbet@lwn.net>");
+MODULE_DESCRIPTION("Marvell 88ALP01 CMOS Camera Controller driver");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("Video");
+
+
+
+
+struct cafe_camera {
+        int registered;                 /* Fully initialized? */
+        struct mcam_camera mcam;
+        struct pci_dev *pdev;
+        wait_queue_head_t smbus_wait;   /* Waiting on i2c events */
+};
+
+/*
+ * Most of the camera controller registers are defined in mcam-core.h,
+ * but the Cafe platform has some additional registers of its own;
+ * they are described here.
+ */
+
+/*
+ * "General purpose register" has a couple of GPIOs used for sensor
+ * power and reset on OLPC XO 1.0 systems.
+ */
+#define REG_GPR         0xb4
+#define   GPR_C1EN        0x00000020    /* Pad 1 (power down) enable */
+#define   GPR_C0EN        0x00000010    /* Pad 0 (reset) enable */
+#define   GPR_C1          0x00000002    /* Control 1 value */
+/*
+ * Control 0 is wired to reset on OLPC machines.  For ov7x sensors,
+ * it is active low.
+ */
+#define   GPR_C0          0x00000001    /* Control 0 value */
+
+/*
+ * These registers control the SMBUS module for communicating
+ * with the sensor.
+ */
+#define REG_TWSIC0      0xb8    /* TWSI (smbus) control 0 */
+#define   TWSIC0_EN       0x00000001    /* TWSI enable */
+#define   TWSIC0_MODE     0x00000002    /* 1 = 16-bit, 0 = 8-bit */
+#define   TWSIC0_SID      0x000003fc    /* Slave ID */
+/*
+ * Subtle trickery: the slave ID field starts with bit 2.  But the
+ * Linux i2c stack wants to treat the bottommost bit as a separate
+ * read/write bit, which is why slave ID's are usually presented
+ * >>1.  For consistency with that behavior, we shift over three
+ * bits instead of two.
+ */
+#define   TWSIC0_SID_SHIFT 3
+#define   TWSIC0_CLKDIV   0x0007fc00    /* Clock divider */
+#define   TWSIC0_MASKACK  0x00400000    /* Mask ack from sensor */
+#define   TWSIC0_OVMAGIC  0x00800000    /* Make it work on OV sensors */
+
+#define REG_TWSIC1      0xbc    /* TWSI control 1 */
+#define   TWSIC1_DATA     0x0000ffff    /* Data to/from camchip */
+#define   TWSIC1_ADDR     0x00ff0000    /* Address (register) */
+#define   TWSIC1_ADDR_SHIFT 16
+#define   TWSIC1_READ     0x01000000    /* Set for read op */
+#define   TWSIC1_WSTAT    0x02000000    /* Write status */
+#define   TWSIC1_RVALID   0x04000000    /* Read data valid */
+#define   TWSIC1_ERROR    0x08000000    /* Something screwed up */
+
+/*
+ * Here's the weird global control registers
+ */
+#define REG_GL_CSR     0x3004  /* Control/status register */
+#define   GCSR_SRS       0x00000001     /* SW Reset set */
+#define   GCSR_SRC       0x00000002     /* SW Reset clear */
+#define   GCSR_MRS       0x00000004     /* Master reset set */
+#define   GCSR_MRC       0x00000008     /* HW Reset clear */
+#define   GCSR_CCIC_EN   0x00004000    /* CCIC Clock enable */
+#define REG_GL_IMASK   0x300c  /* Interrupt mask register */
+#define   GIMSK_CCIC_EN          0x00000004    /* CCIC Interrupt enable */
+
+#define REG_GL_FCR      0x3038  /* GPIO functional control register */
+#define   GFCR_GPIO_ON    0x08          /* Camera GPIO enabled */
+#define REG_GL_GPIOR    0x315c  /* GPIO register */
+#define   GGPIO_OUT             0x80000 /* GPIO output */
+#define   GGPIO_VAL             0x00008 /* Output pin value */
+
+#define REG_LEN                (REG_GL_IMASK + 4)
+
+
+/*
+ * Debugging and related.
+ */
+#define cam_err(cam, fmt, arg...) \
+        dev_err(&(cam)->pdev->dev, fmt, ##arg);
+#define cam_warn(cam, fmt, arg...) \
+        dev_warn(&(cam)->pdev->dev, fmt, ##arg);
+
+/* -------------------------------------------------------------------- */
+/*
+ * The I2C/SMBUS interface to the camera itself starts here.  The
+ * controller handles SMBUS itself, presenting a relatively simple register
+ * interface; all we have to do is to tell it where to route the data.
+ */
+#define CAFE_SMBUS_TIMEOUT (HZ)  /* generous */
+
+static inline struct cafe_camera *to_cam(struct v4l2_device *dev)
+{
+        struct mcam_camera *m = container_of(dev, struct mcam_camera, v4l2_dev);
+        return container_of(m, struct cafe_camera, mcam);
+}
+
+
+static int cafe_smbus_write_done(struct mcam_camera *mcam)
+{
+        unsigned long flags;
+        int c1;
+
+        /*
+         * We must delay after the interrupt, or the controller gets confused
+         * and never does give us good status.  Fortunately, we don't do this
+         * often.
+         */
+        udelay(20);
+        spin_lock_irqsave(&mcam->dev_lock, flags);
+        c1 = mcam_reg_read(mcam, REG_TWSIC1);
+        spin_unlock_irqrestore(&mcam->dev_lock, flags);
+        return (c1 & (TWSIC1_WSTAT|TWSIC1_ERROR)) != TWSIC1_WSTAT;
+}
+
+static int cafe_smbus_write_data(struct cafe_camera *cam,
+                u16 addr, u8 command, u8 value)
+{
+        unsigned int rval;
+        unsigned long flags;
+        struct mcam_camera *mcam = &cam->mcam;
+
+        spin_lock_irqsave(&mcam->dev_lock, flags);
+        rval = TWSIC0_EN | ((addr << TWSIC0_SID_SHIFT) & TWSIC0_SID);
+        rval |= TWSIC0_OVMAGIC;  /* Make OV sensors work */
+        /*
+         * Marvell sez set clkdiv to all 1's for now.
+         */
+        rval |= TWSIC0_CLKDIV;
+        mcam_reg_write(mcam, REG_TWSIC0, rval);
+        (void) mcam_reg_read(mcam, REG_TWSIC1); /* force write */
+        rval = value | ((command << TWSIC1_ADDR_SHIFT) & TWSIC1_ADDR);
+        mcam_reg_write(mcam, REG_TWSIC1, rval);
+        spin_unlock_irqrestore(&mcam->dev_lock, flags);
+
+        /* Unfortunately, reading TWSIC1 too soon after sending a command
+         * causes the device to die.
+         * Use a busy-wait because we often send a large quantity of small
+         * commands at-once; using msleep() would cause a lot of context
+         * switches which take longer than 2ms, resulting in a noticeable
+         * boot-time and capture-start delays.
+         */
+        mdelay(2);
+
+        /*
+         * Another sad fact is that sometimes, commands silently complete but
+         * cafe_smbus_write_done() never becomes aware of this.
+         * This happens at random and appears to possible occur with any
+         * command.
+         * We don't understand why this is. We work around this issue
+         * with the timeout in the wait below, assuming that all commands
+         * complete within the timeout.
+         */
+        wait_event_timeout(cam->smbus_wait, cafe_smbus_write_done(mcam),
+                        CAFE_SMBUS_TIMEOUT);
+
+        spin_lock_irqsave(&mcam->dev_lock, flags);
+        rval = mcam_reg_read(mcam, REG_TWSIC1);
+        spin_unlock_irqrestore(&mcam->dev_lock, flags);
+
+        if (rval & TWSIC1_WSTAT) {
+                cam_err(cam, "SMBUS write (%02x/%02x/%02x) timed out\n", addr,
+                                command, value);
+                return -EIO;
+        }
+        if (rval & TWSIC1_ERROR) {
+                cam_err(cam, "SMBUS write (%02x/%02x/%02x) error\n", addr,
+                                command, value);
+                return -EIO;
+        }
+        return 0;
+}
+
+
+
+static int cafe_smbus_read_done(struct mcam_camera *mcam)
+{
+        unsigned long flags;
+        int c1;
+
+        /*
+         * We must delay after the interrupt, or the controller gets confused
+         * and never does give us good status.  Fortunately, we don't do this
+         * often.
+         */
+        udelay(20);
+        spin_lock_irqsave(&mcam->dev_lock, flags);
+        c1 = mcam_reg_read(mcam, REG_TWSIC1);
+        spin_unlock_irqrestore(&mcam->dev_lock, flags);
+        return c1 & (TWSIC1_RVALID|TWSIC1_ERROR);
+}
+
+
+
+static int cafe_smbus_read_data(struct cafe_camera *cam,
+                u16 addr, u8 command, u8 *value)
+{
+        unsigned int rval;
+        unsigned long flags;
+        struct mcam_camera *mcam = &cam->mcam;
+
+        spin_lock_irqsave(&mcam->dev_lock, flags);
+        rval = TWSIC0_EN | ((addr << TWSIC0_SID_SHIFT) & TWSIC0_SID);
+        rval |= TWSIC0_OVMAGIC; /* Make OV sensors work */
+        /*
+         * Marvel sez set clkdiv to all 1's for now.
+         */
+        rval |= TWSIC0_CLKDIV;
+        mcam_reg_write(mcam, REG_TWSIC0, rval);
+        (void) mcam_reg_read(mcam, REG_TWSIC1); /* force write */
+        rval = TWSIC1_READ | ((command << TWSIC1_ADDR_SHIFT) & TWSIC1_ADDR);
+        mcam_reg_write(mcam, REG_TWSIC1, rval);
+        spin_unlock_irqrestore(&mcam->dev_lock, flags);
+
+        wait_event_timeout(cam->smbus_wait,
+                        cafe_smbus_read_done(mcam), CAFE_SMBUS_TIMEOUT);
+        spin_lock_irqsave(&mcam->dev_lock, flags);
+        rval = mcam_reg_read(mcam, REG_TWSIC1);
+        spin_unlock_irqrestore(&mcam->dev_lock, flags);
+
+        if (rval & TWSIC1_ERROR) {
+                cam_err(cam, "SMBUS read (%02x/%02x) error\n", addr, command);
+                return -EIO;
+        }
+        if (!(rval & TWSIC1_RVALID)) {
+                cam_err(cam, "SMBUS read (%02x/%02x) timed out\n", addr,
+                                command);
+                return -EIO;
+        }
+        *value = rval & 0xff;
+        return 0;
+}
+
+/*
+ * Perform a transfer over SMBUS.  This thing is called under
+ * the i2c bus lock, so we shouldn't race with ourselves...
+ */
+static int cafe_smbus_xfer(struct i2c_adapter *adapter, u16 addr,
+                unsigned short flags, char rw, u8 command,
+                int size, union i2c_smbus_data *data)
+{
+        struct cafe_camera *cam = i2c_get_adapdata(adapter);
+        int ret = -EINVAL;
+
+        /*
+         * This interface would appear to only do byte data ops.  OK
+         * it can do word too, but the cam chip has no use for that.
+         */
+        if (size != I2C_SMBUS_BYTE_DATA) {
+                cam_err(cam, "funky xfer size %d\n", size);
+                return -EINVAL;
+        }
+
+        if (rw == I2C_SMBUS_WRITE)
+                ret = cafe_smbus_write_data(cam, addr, command, data->byte);
+        else if (rw == I2C_SMBUS_READ)
+                ret = cafe_smbus_read_data(cam, addr, command, &data->byte);
+        return ret;
+}
+
+
+static void cafe_smbus_enable_irq(struct cafe_camera *cam)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&cam->mcam.dev_lock, flags);
+        mcam_reg_set_bit(&cam->mcam, REG_IRQMASK, TWSIIRQS);
+        spin_unlock_irqrestore(&cam->mcam.dev_lock, flags);
+}
+
+static u32 cafe_smbus_func(struct i2c_adapter *adapter)
+{
+        return I2C_FUNC_SMBUS_READ_BYTE_DATA  |
+               I2C_FUNC_SMBUS_WRITE_BYTE_DATA;
+}
+
+static struct i2c_algorithm cafe_smbus_algo = {
+        .smbus_xfer = cafe_smbus_xfer,
+        .functionality = cafe_smbus_func
+};
+
+static int cafe_smbus_setup(struct cafe_camera *cam)
+{
+        struct i2c_adapter *adap;
+        int ret;
+
+        adap = kzalloc(sizeof(*adap), GFP_KERNEL);
+        if (adap == NULL)
+                return -ENOMEM;
+        cam->mcam.i2c_adapter = adap;
+        cafe_smbus_enable_irq(cam);
+        adap->owner = THIS_MODULE;
+        adap->algo = &cafe_smbus_algo;
+        strcpy(adap->name, "cafe_ccic");
+        adap->dev.parent = &cam->pdev->dev;
+        i2c_set_adapdata(adap, cam);
+        ret = i2c_add_adapter(adap);
+        if (ret)
+                printk(KERN_ERR "Unable to register cafe i2c adapter\n");
+        return ret;
+}
+
+static void cafe_smbus_shutdown(struct cafe_camera *cam)
+{
+        i2c_del_adapter(cam->mcam.i2c_adapter);
+        kfree(cam->mcam.i2c_adapter);
+}
+
+
+/*
+ * Controller-level stuff
+ */
+
+static void cafe_ctlr_init(struct mcam_camera *mcam)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&mcam->dev_lock, flags);
+        /*
+         * Added magic to bring up the hardware on the B-Test board
+         */
+        mcam_reg_write(mcam, 0x3038, 0x8);
+        mcam_reg_write(mcam, 0x315c, 0x80008);
+        /*
+         * Go through the dance needed to wake the device up.
+         * Note that these registers are global and shared
+         * with the NAND and SD devices.  Interaction between the
+         * three still needs to be examined.
+         */
+        mcam_reg_write(mcam, REG_GL_CSR, GCSR_SRS|GCSR_MRS); /* Needed? */
+        mcam_reg_write(mcam, REG_GL_CSR, GCSR_SRC|GCSR_MRC);
+        mcam_reg_write(mcam, REG_GL_CSR, GCSR_SRC|GCSR_MRS);
+        /*
+         * Here we must wait a bit for the controller to come around.
+         */
+        spin_unlock_irqrestore(&mcam->dev_lock, flags);
+        msleep(5);
+        spin_lock_irqsave(&mcam->dev_lock, flags);
+
+        mcam_reg_write(mcam, REG_GL_CSR, GCSR_CCIC_EN|GCSR_SRC|GCSR_MRC);
+        mcam_reg_set_bit(mcam, REG_GL_IMASK, GIMSK_CCIC_EN);
+        /*
+         * Mask all interrupts.
+         */
+        mcam_reg_write(mcam, REG_IRQMASK, 0);
+        spin_unlock_irqrestore(&mcam->dev_lock, flags);
+}
+
+
+static void cafe_ctlr_power_up(struct mcam_camera *mcam)
+{
+        /*
+         * Part one of the sensor dance: turn the global
+         * GPIO signal on.
+         */
+        mcam_reg_write(mcam, REG_GL_FCR, GFCR_GPIO_ON);
+        mcam_reg_write(mcam, REG_GL_GPIOR, GGPIO_OUT|GGPIO_VAL);
+        /*
+         * Put the sensor into operational mode (assumes OLPC-style
+         * wiring).  Control 0 is reset - set to 1 to operate.
+         * Control 1 is power down, set to 0 to operate.
+         */
+        mcam_reg_write(mcam, REG_GPR, GPR_C1EN|GPR_C0EN); /* pwr up, reset */
+        mcam_reg_write(mcam, REG_GPR, GPR_C1EN|GPR_C0EN|GPR_C0);
+}
+
+static void cafe_ctlr_power_down(struct mcam_camera *mcam)
+{
+        mcam_reg_write(mcam, REG_GPR, GPR_C1EN|GPR_C0EN|GPR_C1);
+        mcam_reg_write(mcam, REG_GL_FCR, GFCR_GPIO_ON);
+        mcam_reg_write(mcam, REG_GL_GPIOR, GGPIO_OUT);
+}
+
+
+
+/*
+ * The platform interrupt handler.
+ */
+static irqreturn_t cafe_irq(int irq, void *data)
+{
+        struct cafe_camera *cam = data;
+        struct mcam_camera *mcam = &cam->mcam;
+        unsigned int irqs, handled;
+
+        spin_lock(&mcam->dev_lock);
+        irqs = mcam_reg_read(mcam, REG_IRQSTAT);
+        handled = cam->registered && mccic_irq(mcam, irqs);
+        if (irqs & TWSIIRQS) {
+                mcam_reg_write(mcam, REG_IRQSTAT, TWSIIRQS);
+                wake_up(&cam->smbus_wait);
+                handled = 1;
+        }
+        spin_unlock(&mcam->dev_lock);
+        return IRQ_RETVAL(handled);
+}
+
+
+/* -------------------------------------------------------------------------- */
+/*
+ * PCI interface stuff.
+ */
+
+static int cafe_pci_probe(struct pci_dev *pdev,
+                const struct pci_device_id *id)
+{
+        int ret;
+        struct cafe_camera *cam;
+        struct mcam_camera *mcam;
+
+        /*
+         * Start putting together one of our big camera structures.
+         */
+        ret = -ENOMEM;
+        cam = kzalloc(sizeof(struct cafe_camera), GFP_KERNEL);
+        if (cam == NULL)
+                goto out;
+        cam->pdev = pdev;
+        mcam = &cam->mcam;
+        mcam->chip_id = V4L2_IDENT_CAFE;
+        spin_lock_init(&mcam->dev_lock);
+        init_waitqueue_head(&cam->smbus_wait);
+        mcam->plat_power_up = cafe_ctlr_power_up;
+        mcam->plat_power_down = cafe_ctlr_power_down;
+        mcam->dev = &pdev->dev;
+        /*
+         * Set the clock speed for the XO 1; I don't believe this
+         * driver has ever run anywhere else.
+         */
+        mcam->clock_speed = 45;
+        mcam->use_smbus = 1;
+        /*
+         * Vmalloc mode for buffers is traditional with this driver.
+         * We *might* be able to run DMA_contig, especially on a system
+         * with CMA in it.
+         */
+        mcam->buffer_mode = B_vmalloc;
+        /*
+         * Get set up on the PCI bus.
+         */
+        ret = pci_enable_device(pdev);
+        if (ret)
+                goto out_free;
+        pci_set_master(pdev);
+
+        ret = -EIO;
+        mcam->regs = pci_iomap(pdev, 0, 0);
+        if (!mcam->regs) {
+                printk(KERN_ERR "Unable to ioremap cafe-ccic regs\n");
+                goto out_disable;
+        }
+        ret = request_irq(pdev->irq, cafe_irq, IRQF_SHARED, "cafe-ccic", cam);
+        if (ret)
+                goto out_iounmap;
+
+        /*
+         * Initialize the controller and leave it powered up.  It will
+         * stay that way until the sensor driver shows up.
+         */
+        cafe_ctlr_init(mcam);
+        cafe_ctlr_power_up(mcam);
+        /*
+         * Set up I2C/SMBUS communications.  We have to drop the mutex here
+         * because the sensor could attach in this call chain, leading to
+         * unsightly deadlocks.
+         */
+        ret = cafe_smbus_setup(cam);
+        if (ret)
+                goto out_pdown;
+
+        ret = mccic_register(mcam);
+        if (ret == 0) {
+                cam->registered = 1;
+                return 0;
+        }
+
+        cafe_smbus_shutdown(cam);
+out_pdown:
+        cafe_ctlr_power_down(mcam);
+        free_irq(pdev->irq, cam);
+out_iounmap:
+        pci_iounmap(pdev, mcam->regs);
+out_disable:
+        pci_disable_device(pdev);
+out_free:
+        kfree(cam);
+out:
+        return ret;
+}
+
+
+/*
+ * Shut down an initialized device
+ */
+static void cafe_shutdown(struct cafe_camera *cam)
+{
+        mccic_shutdown(&cam->mcam);
+        cafe_smbus_shutdown(cam);
+        free_irq(cam->pdev->irq, cam);
+        pci_iounmap(cam->pdev, cam->mcam.regs);
+}
+
+
+static void cafe_pci_remove(struct pci_dev *pdev)
+{
+        struct v4l2_device *v4l2_dev = dev_get_drvdata(&pdev->dev);
+        struct cafe_camera *cam = to_cam(v4l2_dev);
+
+        if (cam == NULL) {
+                printk(KERN_WARNING "pci_remove on unknown pdev %p\n", pdev);
+                return;
+        }
+        cafe_shutdown(cam);
+        kfree(cam);
+}
+
+
+#ifdef CONFIG_PM
+/*
+ * Basic power management.
+ */
+static int cafe_pci_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+        struct v4l2_device *v4l2_dev = dev_get_drvdata(&pdev->dev);
+        struct cafe_camera *cam = to_cam(v4l2_dev);
+        int ret;
+
+        ret = pci_save_state(pdev);
+        if (ret)
+                return ret;
+        mccic_suspend(&cam->mcam);
+        pci_disable_device(pdev);
+        return 0;
+}
+
+
+static int cafe_pci_resume(struct pci_dev *pdev)
+{
+        struct v4l2_device *v4l2_dev = dev_get_drvdata(&pdev->dev);
+        struct cafe_camera *cam = to_cam(v4l2_dev);
+        int ret = 0;
+
+        pci_restore_state(pdev);
+        ret = pci_enable_device(pdev);
+
+        if (ret) {
+                cam_warn(cam, "Unable to re-enable device on resume!\n");
+                return ret;
+        }
+        cafe_ctlr_init(&cam->mcam);
+        return mccic_resume(&cam->mcam);
+}
+
+#endif  /* CONFIG_PM */
+
+static struct pci_device_id cafe_ids[] = {
+        { PCI_DEVICE(PCI_VENDOR_ID_MARVELL,
+                     PCI_DEVICE_ID_MARVELL_88ALP01_CCIC) },
+        { 0, }
+};
+
+MODULE_DEVICE_TABLE(pci, cafe_ids);
+
+static struct pci_driver cafe_pci_driver = {
+        .name = "cafe1000-ccic",
+        .id_table = cafe_ids,
+        .probe = cafe_pci_probe,
+        .remove = cafe_pci_remove,
+#ifdef CONFIG_PM
+        .suspend = cafe_pci_suspend,
+        .resume = cafe_pci_resume,
+#endif
+};
+
+
+
+
+static int __init cafe_init(void)
+{
+        int ret;
+
+        printk(KERN_NOTICE "Marvell M88ALP01 'CAFE' Camera Controller version %d\n",
+                        CAFE_VERSION);
+        ret = pci_register_driver(&cafe_pci_driver);
+        if (ret) {
+                printk(KERN_ERR "Unable to register cafe_ccic driver\n");
+                goto out;
+        }
+        ret = 0;
+
+out:
+        return ret;
+}
+
+
+static void __exit cafe_exit(void)
+{
+        pci_unregister_driver(&cafe_pci_driver);
+}
+
+module_init(cafe_init);
+module_exit(cafe_exit);
diff --git a/drivers/media/video/marvell-ccic/mcam-core.c b/drivers/media/video/marvell-ccic/mcam-core.c
new file mode 100644
index 00000000000..83c14514cd5
--- /dev/null
+++ b/drivers/media/video/marvell-ccic/mcam-core.c
@@ -0,0 +1,1843 @@
+/*
+ * The Marvell camera core.  This device appears in a number of settings,
+ * so it needs platform-specific support outside of the core.
+ *
+ * Copyright 2011 Jonathan Corbet corbet@lwn.net
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/wait.h>
+#include <linux/list.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/vmalloc.h>
+#include <linux/io.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/ov7670.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/videobuf2-dma-sg.h>
+
+#include "mcam-core.h"
+
+/*
+ * Basic frame stats - to be deleted shortly
+ */
+static int frames;
+static int singles;
+static int delivered;
+
+#ifdef MCAM_MODE_VMALLOC
+/*
+ * Internal DMA buffer management.  Since the controller cannot do S/G I/O,
+ * we must have physically contiguous buffers to bring frames into.
+ * These parameters control how many buffers we use, whether we
+ * allocate them at load time (better chance of success, but nails down
+ * memory) or when somebody tries to use the camera (riskier), and,
+ * for load-time allocation, how big they should be.
+ *
+ * The controller can cycle through three buffers.  We could use
+ * more by flipping pointers around, but it probably makes little
+ * sense.
+ */
+
+static int alloc_bufs_at_read;
+module_param(alloc_bufs_at_read, bool, 0444);
+MODULE_PARM_DESC(alloc_bufs_at_read,
+                "Non-zero value causes DMA buffers to be allocated when the "
+                "video capture device is read, rather than at module load "
+                "time.  This saves memory, but decreases the chances of "
+                "successfully getting those buffers.  This parameter is "
+                "only used in the vmalloc buffer mode");
+
+static int n_dma_bufs = 3;
+module_param(n_dma_bufs, uint, 0644);
+MODULE_PARM_DESC(n_dma_bufs,
+                "The number of DMA buffers to allocate.  Can be either two "
+                "(saves memory, makes timing tighter) or three.");
+
+static int dma_buf_size = VGA_WIDTH * VGA_HEIGHT * 2;  /* Worst case */
+module_param(dma_buf_size, uint, 0444);
+MODULE_PARM_DESC(dma_buf_size,
+                "The size of the allocated DMA buffers.  If actual operating "
+                "parameters require larger buffers, an attempt to reallocate "
+                "will be made.");
+#else /* MCAM_MODE_VMALLOC */
+static const int alloc_bufs_at_read = 0;
+static const int n_dma_bufs = 3;  /* Used by S/G_PARM */
+#endif /* MCAM_MODE_VMALLOC */
+
+static int flip;
+module_param(flip, bool, 0444);
+MODULE_PARM_DESC(flip,
+                "If set, the sensor will be instructed to flip the image "
+                "vertically.");
+
+static int buffer_mode = -1;
+module_param(buffer_mode, int, 0444);
+MODULE_PARM_DESC(buffer_mode,
+                "Set the buffer mode to be used; default is to go with what "
+                "the platform driver asks for.  Set to 0 for vmalloc, 1 for "
+                "DMA contiguous.");
+
+/*
+ * Status flags.  Always manipulated with bit operations.
+ */
+#define CF_BUF0_VALID    0      /* Buffers valid - first three */
+#define CF_BUF1_VALID    1
+#define CF_BUF2_VALID    2
+#define CF_DMA_ACTIVE    3      /* A frame is incoming */
+#define CF_CONFIG_NEEDED 4      /* Must configure hardware */
+#define CF_SINGLE_BUFFER 5      /* Running with a single buffer */
+#define CF_SG_RESTART    6      /* SG restart needed */
+
+#define sensor_call(cam, o, f, args...) \
+        v4l2_subdev_call(cam->sensor, o, f, ##args)
+
+static struct mcam_format_struct {
+        __u8 *desc;
+        __u32 pixelformat;
+        int bpp;   /* Bytes per pixel */
+        enum v4l2_mbus_pixelcode mbus_code;
+} mcam_formats[] = {
+        {
+                .desc           = "YUYV 4:2:2",
+                .pixelformat    = V4L2_PIX_FMT_YUYV,
+                .mbus_code      = V4L2_MBUS_FMT_YUYV8_2X8,
+                .bpp            = 2,
+        },
+        {
+                .desc           = "RGB 444",
+                .pixelformat    = V4L2_PIX_FMT_RGB444,
+                .mbus_code      = V4L2_MBUS_FMT_RGB444_2X8_PADHI_LE,
+                .bpp            = 2,
+        },
+        {
+                .desc           = "RGB 565",
+                .pixelformat    = V4L2_PIX_FMT_RGB565,
+                .mbus_code      = V4L2_MBUS_FMT_RGB565_2X8_LE,
+                .bpp            = 2,
+        },
+        {
+                .desc           = "Raw RGB Bayer",
+                .pixelformat    = V4L2_PIX_FMT_SBGGR8,
+                .mbus_code      = V4L2_MBUS_FMT_SBGGR8_1X8,
+                .bpp            = 1
+        },
+};
+#define N_MCAM_FMTS ARRAY_SIZE(mcam_formats)
+
+static struct mcam_format_struct *mcam_find_format(u32 pixelformat)
+{
+        unsigned i;
+
+        for (i = 0; i < N_MCAM_FMTS; i++)
+                if (mcam_formats[i].pixelformat == pixelformat)
+                        return mcam_formats + i;
+        /* Not found? Then return the first format. */
+        return mcam_formats;
+}
+
+/*
+ * The default format we use until somebody says otherwise.
+ */
+static const struct v4l2_pix_format mcam_def_pix_format = {
+        .width          = VGA_WIDTH,
+        .height         = VGA_HEIGHT,
+        .pixelformat    = V4L2_PIX_FMT_YUYV,
+        .field          = V4L2_FIELD_NONE,
+        .bytesperline   = VGA_WIDTH*2,
+        .sizeimage      = VGA_WIDTH*VGA_HEIGHT*2,
+};
+
+static const enum v4l2_mbus_pixelcode mcam_def_mbus_code =
+                                        V4L2_MBUS_FMT_YUYV8_2X8;
+
+
+/*
+ * The two-word DMA descriptor format used by the Armada 610 and like.  There
+ * Is a three-word format as well (set C1_DESC_3WORD) where the third
+ * word is a pointer to the next descriptor, but we don't use it.  Two-word
+ * descriptors have to be contiguous in memory.
+ */
+struct mcam_dma_desc {
+        u32 dma_addr;
+        u32 segment_len;
+};
+
+/*
+ * Our buffer type for working with videobuf2.  Note that the vb2
+ * developers have decreed that struct vb2_buffer must be at the
+ * beginning of this structure.
+ */
+struct mcam_vb_buffer {
+        struct vb2_buffer vb_buf;
+        struct list_head queue;
+        struct mcam_dma_desc *dma_desc; /* Descriptor virtual address */
+        dma_addr_t dma_desc_pa;         /* Descriptor physical address */
+        int dma_desc_nent;              /* Number of mapped descriptors */
+};
+
+static inline struct mcam_vb_buffer *vb_to_mvb(struct vb2_buffer *vb)
+{
+        return container_of(vb, struct mcam_vb_buffer, vb_buf);
+}
+
+/*
+ * Hand a completed buffer back to user space.
+ */
+static void mcam_buffer_done(struct mcam_camera *cam, int frame,
+                struct vb2_buffer *vbuf)
+{
+        vbuf->v4l2_buf.bytesused = cam->pix_format.sizeimage;
+        vbuf->v4l2_buf.sequence = cam->buf_seq[frame];
+        vb2_set_plane_payload(vbuf, 0, cam->pix_format.sizeimage);
+        vb2_buffer_done(vbuf, VB2_BUF_STATE_DONE);
+}
+
+
+
+/*
+ * Debugging and related.
+ */
+#define cam_err(cam, fmt, arg...) \
+        dev_err((cam)->dev, fmt, ##arg);
+#define cam_warn(cam, fmt, arg...) \
+        dev_warn((cam)->dev, fmt, ##arg);
+#define cam_dbg(cam, fmt, arg...) \
+        dev_dbg((cam)->dev, fmt, ##arg);
+
+
+/*
+ * Flag manipulation helpers
+ */
+static void mcam_reset_buffers(struct mcam_camera *cam)
+{
+        int i;
+
+        cam->next_buf = -1;
+        for (i = 0; i < cam->nbufs; i++)
+                clear_bit(i, &cam->flags);
+}
+
+static inline int mcam_needs_config(struct mcam_camera *cam)
+{
+        return test_bit(CF_CONFIG_NEEDED, &cam->flags);
+}
+
+static void mcam_set_config_needed(struct mcam_camera *cam, int needed)
+{
+        if (needed)
+                set_bit(CF_CONFIG_NEEDED, &cam->flags);
+        else
+                clear_bit(CF_CONFIG_NEEDED, &cam->flags);
+}
+
+/* ------------------------------------------------------------------- */
+/*
+ * Make the controller start grabbing images.  Everything must
+ * be set up before doing this.
+ */
+static void mcam_ctlr_start(struct mcam_camera *cam)
+{
+        /* set_bit performs a read, so no other barrier should be
+           needed here */
+        mcam_reg_set_bit(cam, REG_CTRL0, C0_ENABLE);
+}
+
+static void mcam_ctlr_stop(struct mcam_camera *cam)
+{
+        mcam_reg_clear_bit(cam, REG_CTRL0, C0_ENABLE);
+}
+
+/* ------------------------------------------------------------------- */
+
+#ifdef MCAM_MODE_VMALLOC
+/*
+ * Code specific to the vmalloc buffer mode.
+ */
+
+/*
+ * Allocate in-kernel DMA buffers for vmalloc mode.
+ */
+static int mcam_alloc_dma_bufs(struct mcam_camera *cam, int loadtime)
+{
+        int i;
+
+        mcam_set_config_needed(cam, 1);
+        if (loadtime)
+                cam->dma_buf_size = dma_buf_size;
+        else
+                cam->dma_buf_size = cam->pix_format.sizeimage;
+        if (n_dma_bufs > 3)
+                n_dma_bufs = 3;
+
+        cam->nbufs = 0;
+        for (i = 0; i < n_dma_bufs; i++) {
+                cam->dma_bufs[i] = dma_alloc_coherent(cam->dev,
+                                cam->dma_buf_size, cam->dma_handles + i,
+                                GFP_KERNEL);
+                if (cam->dma_bufs[i] == NULL) {
+                        cam_warn(cam, "Failed to allocate DMA buffer\n");
+                        break;
+                }
+                (cam->nbufs)++;
+        }
+
+        switch (cam->nbufs) {
+        case 1:
+                dma_free_coherent(cam->dev, cam->dma_buf_size,
+                                cam->dma_bufs[0], cam->dma_handles[0]);
+                cam->nbufs = 0;
+        case 0:
+                cam_err(cam, "Insufficient DMA buffers, cannot operate\n");
+                return -ENOMEM;
+
+        case 2:
+                if (n_dma_bufs > 2)
+                        cam_warn(cam, "Will limp along with only 2 buffers\n");
+                break;
+        }
+        return 0;
+}
+
+static void mcam_free_dma_bufs(struct mcam_camera *cam)
+{
+        int i;
+
+        for (i = 0; i < cam->nbufs; i++) {
+                dma_free_coherent(cam->dev, cam->dma_buf_size,
+                                cam->dma_bufs[i], cam->dma_handles[i]);
+                cam->dma_bufs[i] = NULL;
+        }
+        cam->nbufs = 0;
+}
+
+
+/*
+ * Set up DMA buffers when operating in vmalloc mode
+ */
+static void mcam_ctlr_dma_vmalloc(struct mcam_camera *cam)
+{
+        /*
+         * Store the first two Y buffers (we aren't supporting
+         * planar formats for now, so no UV bufs).  Then either
+         * set the third if it exists, or tell the controller
+         * to just use two.
+         */
+        mcam_reg_write(cam, REG_Y0BAR, cam->dma_handles[0]);
+        mcam_reg_write(cam, REG_Y1BAR, cam->dma_handles[1]);
+        if (cam->nbufs > 2) {
+                mcam_reg_write(cam, REG_Y2BAR, cam->dma_handles[2]);
+                mcam_reg_clear_bit(cam, REG_CTRL1, C1_TWOBUFS);
+        } else
+                mcam_reg_set_bit(cam, REG_CTRL1, C1_TWOBUFS);
+        if (cam->chip_id == V4L2_IDENT_CAFE)
+                mcam_reg_write(cam, REG_UBAR, 0); /* 32 bits only */
+}
+
+/*
+ * Copy data out to user space in the vmalloc case
+ */
+static void mcam_frame_tasklet(unsigned long data)
+{
+        struct mcam_camera *cam = (struct mcam_camera *) data;
+        int i;
+        unsigned long flags;
+        struct mcam_vb_buffer *buf;
+
+        spin_lock_irqsave(&cam->dev_lock, flags);
+        for (i = 0; i < cam->nbufs; i++) {
+                int bufno = cam->next_buf;
+
+                if (cam->state != S_STREAMING || bufno < 0)
+                        break;  /* I/O got stopped */
+                if (++(cam->next_buf) >= cam->nbufs)
+                        cam->next_buf = 0;
+                if (!test_bit(bufno, &cam->flags))
+                        continue;
+                if (list_empty(&cam->buffers)) {
+                        singles++;
+                        break;  /* Leave it valid, hope for better later */
+                }
+                delivered++;
+                clear_bit(bufno, &cam->flags);
+                buf = list_first_entry(&cam->buffers, struct mcam_vb_buffer,
+                                queue);
+                list_del_init(&buf->queue);
+                /*
+                 * Drop the lock during the big copy.  This *should* be safe...
+                 */
+                spin_unlock_irqrestore(&cam->dev_lock, flags);
+                memcpy(vb2_plane_vaddr(&buf->vb_buf, 0), cam->dma_bufs[bufno],
+                                cam->pix_format.sizeimage);
+                mcam_buffer_done(cam, bufno, &buf->vb_buf);
+                spin_lock_irqsave(&cam->dev_lock, flags);
+        }
+        spin_unlock_irqrestore(&cam->dev_lock, flags);
+}
+
+
+/*
+ * Make sure our allocated buffers are up to the task.
+ */
+static int mcam_check_dma_buffers(struct mcam_camera *cam)
+{
+        if (cam->nbufs > 0 && cam->dma_buf_size < cam->pix_format.sizeimage)
+                        mcam_free_dma_bufs(cam);
+        if (cam->nbufs == 0)
+                return mcam_alloc_dma_bufs(cam, 0);
+        return 0;
+}
+
+static void mcam_vmalloc_done(struct mcam_camera *cam, int frame)
+{
+        tasklet_schedule(&cam->s_tasklet);
+}
+
+#else /* MCAM_MODE_VMALLOC */
+
+static inline int mcam_alloc_dma_bufs(struct mcam_camera *cam, int loadtime)
+{
+        return 0;
+}
+
+static inline void mcam_free_dma_bufs(struct mcam_camera *cam)
+{
+        return;
+}
+
+static inline int mcam_check_dma_buffers(struct mcam_camera *cam)
+{
+        return 0;
+}
+
+
+
+#endif /* MCAM_MODE_VMALLOC */
+
+
+#ifdef MCAM_MODE_DMA_CONTIG
+/* ---------------------------------------------------------------------- */
+/*
+ * DMA-contiguous code.
+ */
+/*
+ * Set up a contiguous buffer for the given frame.  Here also is where
+ * the underrun strategy is set: if there is no buffer available, reuse
+ * the buffer from the other BAR and set the CF_SINGLE_BUFFER flag to
+ * keep the interrupt handler from giving that buffer back to user
+ * space.  In this way, we always have a buffer to DMA to and don't
+ * have to try to play games stopping and restarting the controller.
+ */
+static void mcam_set_contig_buffer(struct mcam_camera *cam, int frame)
+{
+        struct mcam_vb_buffer *buf;
+        /*
+         * If there are no available buffers, go into single mode
+         */
+        if (list_empty(&cam->buffers)) {
+                buf = cam->vb_bufs[frame ^ 0x1];
+                cam->vb_bufs[frame] = buf;
+                mcam_reg_write(cam, frame == 0 ? REG_Y0BAR : REG_Y1BAR,
+                                vb2_dma_contig_plane_paddr(&buf->vb_buf, 0));
+                set_bit(CF_SINGLE_BUFFER, &cam->flags);
+                singles++;
+                return;
+        }
+        /*
+         * OK, we have a buffer we can use.
+         */
+        buf = list_first_entry(&cam->buffers, struct mcam_vb_buffer, queue);
+        list_del_init(&buf->queue);
+        mcam_reg_write(cam, frame == 0 ? REG_Y0BAR : REG_Y1BAR,
+                        vb2_dma_contig_plane_paddr(&buf->vb_buf, 0));
+        cam->vb_bufs[frame] = buf;
+        clear_bit(CF_SINGLE_BUFFER, &cam->flags);
+}
+
+/*
+ * Initial B_DMA_contig setup.
+ */
+static void mcam_ctlr_dma_contig(struct mcam_camera *cam)
+{
+        mcam_reg_set_bit(cam, REG_CTRL1, C1_TWOBUFS);
+        cam->nbufs = 2;
+        mcam_set_contig_buffer(cam, 0);
+        mcam_set_contig_buffer(cam, 1);
+}
+
+/*
+ * Frame completion handling.
+ */
+static void mcam_dma_contig_done(struct mcam_camera *cam, int frame)
+{
+        struct mcam_vb_buffer *buf = cam->vb_bufs[frame];
+
+        if (!test_bit(CF_SINGLE_BUFFER, &cam->flags)) {
+                delivered++;
+                mcam_buffer_done(cam, frame, &buf->vb_buf);
+        }
+        mcam_set_contig_buffer(cam, frame);
+}
+
+#endif /* MCAM_MODE_DMA_CONTIG */
+
+#ifdef MCAM_MODE_DMA_SG
+/* ---------------------------------------------------------------------- */
+/*
+ * Scatter/gather-specific code.
+ */
+
+/*
+ * Set up the next buffer for S/G I/O; caller should be sure that
+ * the controller is stopped and a buffer is available.
+ */
+static void mcam_sg_next_buffer(struct mcam_camera *cam)
+{
+        struct mcam_vb_buffer *buf;
+
+        buf = list_first_entry(&cam->buffers, struct mcam_vb_buffer, queue);
+        list_del_init(&buf->queue);
+        mcam_reg_write(cam, REG_DMA_DESC_Y, buf->dma_desc_pa);
+        mcam_reg_write(cam, REG_DESC_LEN_Y,
+                        buf->dma_desc_nent*sizeof(struct mcam_dma_desc));
+        mcam_reg_write(cam, REG_DESC_LEN_U, 0);
+        mcam_reg_write(cam, REG_DESC_LEN_V, 0);
+        cam->vb_bufs[0] = buf;
+}
+
+/*
+ * Initial B_DMA_sg setup
+ */
+static void mcam_ctlr_dma_sg(struct mcam_camera *cam)
+{
+        mcam_reg_clear_bit(cam, REG_CTRL1, C1_DESC_3WORD);
+        mcam_sg_next_buffer(cam);
+        mcam_reg_set_bit(cam, REG_CTRL1, C1_DESC_ENA);
+        cam->nbufs = 3;
+}
+
+
+/*
+ * Frame completion with S/G is trickier.  We can't muck with
+ * a descriptor chain on the fly, since the controller buffers it
+ * internally.  So we have to actually stop and restart; Marvell
+ * says this is the way to do it.
+ *
+ * Of course, stopping is easier said than done; experience shows
+ * that the controller can start a frame *after* C0_ENABLE has been
+ * cleared.  So when running in S/G mode, the controller is "stopped"
+ * on receipt of the start-of-frame interrupt.  That means we can
+ * safely change the DMA descriptor array here and restart things
+ * (assuming there's another buffer waiting to go).
+ */
+static void mcam_dma_sg_done(struct mcam_camera *cam, int frame)
+{
+        struct mcam_vb_buffer *buf = cam->vb_bufs[0];
+
+        /*
+         * Very Bad Not Good Things happen if you don't clear
+         * C1_DESC_ENA before making any descriptor changes.
+         */
+        mcam_reg_clear_bit(cam, REG_CTRL1, C1_DESC_ENA);
+        /*
+         * If we have another buffer available, put it in and
+         * restart the engine.
+         */
+        if (!list_empty(&cam->buffers)) {
+                mcam_sg_next_buffer(cam);
+                mcam_reg_set_bit(cam, REG_CTRL1, C1_DESC_ENA);
+                mcam_ctlr_start(cam);
+        /*
+         * Otherwise set CF_SG_RESTART and the controller will
+         * be restarted once another buffer shows up.
+         */
+        } else {
+                set_bit(CF_SG_RESTART, &cam->flags);
+                singles++;
+        }
+        /*
+         * Now we can give the completed frame back to user space.
+         */
+        delivered++;
+        mcam_buffer_done(cam, frame, &buf->vb_buf);
+}
+
+
+/*
+ * Scatter/gather mode requires stopping the controller between
+ * frames so we can put in a new DMA descriptor array.  If no new
+ * buffer exists at frame completion, the controller is left stopped;
+ * this function is charged with gettig things going again.
+ */
+static void mcam_sg_restart(struct mcam_camera *cam)
+{
+        mcam_ctlr_dma_sg(cam);
+        mcam_ctlr_start(cam);
+        clear_bit(CF_SG_RESTART, &cam->flags);
+}
+
+#else /* MCAM_MODE_DMA_SG */
+
+static inline void mcam_sg_restart(struct mcam_camera *cam)
+{
+        return;
+}
+
+#endif /* MCAM_MODE_DMA_SG */
+
+/* ---------------------------------------------------------------------- */
+/*
+ * Buffer-mode-independent controller code.
+ */
+
+/*
+ * Image format setup
+ */
+static void mcam_ctlr_image(struct mcam_camera *cam)
+{
+        int imgsz;
+        struct v4l2_pix_format *fmt = &cam->pix_format;
+
+        imgsz = ((fmt->height << IMGSZ_V_SHIFT) & IMGSZ_V_MASK) |
+                (fmt->bytesperline & IMGSZ_H_MASK);
+        mcam_reg_write(cam, REG_IMGSIZE, imgsz);
+        mcam_reg_write(cam, REG_IMGOFFSET, 0);
+        /* YPITCH just drops the last two bits */
+        mcam_reg_write_mask(cam, REG_IMGPITCH, fmt->bytesperline,
+                        IMGP_YP_MASK);
+        /*
+         * Tell the controller about the image format we are using.
+         */
+        switch (cam->pix_format.pixelformat) {
+        case V4L2_PIX_FMT_YUYV:
+            mcam_reg_write_mask(cam, REG_CTRL0,
+                            C0_DF_YUV|C0_YUV_PACKED|C0_YUVE_YUYV,
+                            C0_DF_MASK);
+            break;
+
+        case V4L2_PIX_FMT_RGB444:
+            mcam_reg_write_mask(cam, REG_CTRL0,
+                            C0_DF_RGB|C0_RGBF_444|C0_RGB4_XRGB,
+                            C0_DF_MASK);
+                /* Alpha value? */
+            break;
+
+        case V4L2_PIX_FMT_RGB565:
+            mcam_reg_write_mask(cam, REG_CTRL0,
+                            C0_DF_RGB|C0_RGBF_565|C0_RGB5_BGGR,
+                            C0_DF_MASK);
+            break;
+
+        default:
+            cam_err(cam, "Unknown format %x\n", cam->pix_format.pixelformat);
+            break;
+        }
+        /*
+         * Make sure it knows we want to use hsync/vsync.
+         */
+        mcam_reg_write_mask(cam, REG_CTRL0, C0_SIF_HVSYNC,
+                        C0_SIFM_MASK);
+}
+
+
+/*
+ * Configure the controller for operation; caller holds the
+ * device mutex.
+ */
+static int mcam_ctlr_configure(struct mcam_camera *cam)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&cam->dev_lock, flags);
+        cam->dma_setup(cam);
+        mcam_ctlr_image(cam);
+        mcam_set_config_needed(cam, 0);
+        clear_bit(CF_SG_RESTART, &cam->flags);
+        spin_unlock_irqrestore(&cam->dev_lock, flags);
+        return 0;
+}
+
+static void mcam_ctlr_irq_enable(struct mcam_camera *cam)
+{
+        /*
+         * Clear any pending interrupts, since we do not
+         * expect to have I/O active prior to enabling.
+         */
+        mcam_reg_write(cam, REG_IRQSTAT, FRAMEIRQS);
+        mcam_reg_set_bit(cam, REG_IRQMASK, FRAMEIRQS);
+}
+
+static void mcam_ctlr_irq_disable(struct mcam_camera *cam)
+{
+        mcam_reg_clear_bit(cam, REG_IRQMASK, FRAMEIRQS);
+}
+
+
+
+static void mcam_ctlr_init(struct mcam_camera *cam)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&cam->dev_lock, flags);
+        /*
+         * Make sure it's not powered down.
+         */
+        mcam_reg_clear_bit(cam, REG_CTRL1, C1_PWRDWN);
+        /*
+         * Turn off the enable bit.  It sure should be off anyway,
+         * but it's good to be sure.
+         */
+        mcam_reg_clear_bit(cam, REG_CTRL0, C0_ENABLE);
+        /*
+         * Clock the sensor appropriately.  Controller clock should
+         * be 48MHz, sensor "typical" value is half that.
+         */
+        mcam_reg_write_mask(cam, REG_CLKCTRL, 2, CLK_DIV_MASK);
+        spin_unlock_irqrestore(&cam->dev_lock, flags);
+}
+
+
+/*
+ * Stop the controller, and don't return until we're really sure that no
+ * further DMA is going on.
+ */
+static void mcam_ctlr_stop_dma(struct mcam_camera *cam)
+{
+        unsigned long flags;
+
+        /*
+         * Theory: stop the camera controller (whether it is operating
+         * or not).  Delay briefly just in case we race with the SOF
+         * interrupt, then wait until no DMA is active.
+         */
+        spin_lock_irqsave(&cam->dev_lock, flags);
+        clear_bit(CF_SG_RESTART, &cam->flags);
+        mcam_ctlr_stop(cam);
+        cam->state = S_IDLE;
+        spin_unlock_irqrestore(&cam->dev_lock, flags);
+        msleep(40);
+        if (test_bit(CF_DMA_ACTIVE, &cam->flags))
+                cam_err(cam, "Timeout waiting for DMA to end\n");
+                /* This would be bad news - what now? */
+        spin_lock_irqsave(&cam->dev_lock, flags);
+        mcam_ctlr_irq_disable(cam);
+        spin_unlock_irqrestore(&cam->dev_lock, flags);
+}
+
+/*
+ * Power up and down.
+ */
+static void mcam_ctlr_power_up(struct mcam_camera *cam)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&cam->dev_lock, flags);
+        cam->plat_power_up(cam);
+        mcam_reg_clear_bit(cam, REG_CTRL1, C1_PWRDWN);
+        spin_unlock_irqrestore(&cam->dev_lock, flags);
+        msleep(5); /* Just to be sure */
+}
+
+static void mcam_ctlr_power_down(struct mcam_camera *cam)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&cam->dev_lock, flags);
+        /*
+         * School of hard knocks department: be sure we do any register
+         * twiddling on the controller *before* calling the platform
+         * power down routine.
+         */
+        mcam_reg_set_bit(cam, REG_CTRL1, C1_PWRDWN);
+        cam->plat_power_down(cam);
+        spin_unlock_irqrestore(&cam->dev_lock, flags);
+}
+
+/* -------------------------------------------------------------------- */
+/*
+ * Communications with the sensor.
+ */
+
+static int __mcam_cam_reset(struct mcam_camera *cam)
+{
+        return sensor_call(cam, core, reset, 0);
+}
+
+/*
+ * We have found the sensor on the i2c.  Let's try to have a
+ * conversation.
+ */
+static int mcam_cam_init(struct mcam_camera *cam)
+{
+        struct v4l2_dbg_chip_ident chip;
+        int ret;
+
+        mutex_lock(&cam->s_mutex);
+        if (cam->state != S_NOTREADY)
+                cam_warn(cam, "Cam init with device in funky state %d",
+                                cam->state);
+        ret = __mcam_cam_reset(cam);
+        if (ret)
+                goto out;
+        chip.ident = V4L2_IDENT_NONE;
+        chip.match.type = V4L2_CHIP_MATCH_I2C_ADDR;
+        chip.match.addr = cam->sensor_addr;
+        ret = sensor_call(cam, core, g_chip_ident, &chip);
+        if (ret)
+                goto out;
+        cam->sensor_type = chip.ident;
+        if (cam->sensor_type != V4L2_IDENT_OV7670) {
+                cam_err(cam, "Unsupported sensor type 0x%x", cam->sensor_type);
+                ret = -EINVAL;
+                goto out;
+        }
+/* Get/set parameters? */
+        ret = 0;
+        cam->state = S_IDLE;
+out:
+        mcam_ctlr_power_down(cam);
+        mutex_unlock(&cam->s_mutex);
+        return ret;
+}
+
+/*
+ * Configure the sensor to match the parameters we have.  Caller should
+ * hold s_mutex
+ */
+static int mcam_cam_set_flip(struct mcam_camera *cam)
+{
+        struct v4l2_control ctrl;
+
+        memset(&ctrl, 0, sizeof(ctrl));
+        ctrl.id = V4L2_CID_VFLIP;
+        ctrl.value = flip;
+        return sensor_call(cam, core, s_ctrl, &ctrl);
+}
+
+
+static int mcam_cam_configure(struct mcam_camera *cam)
+{
+        struct v4l2_mbus_framefmt mbus_fmt;
+        int ret;
+
+        v4l2_fill_mbus_format(&mbus_fmt, &cam->pix_format, cam->mbus_code);
+        ret = sensor_call(cam, core, init, 0);
+        if (ret == 0)
+                ret = sensor_call(cam, video, s_mbus_fmt, &mbus_fmt);
+        /*
+         * OV7670 does weird things if flip is set *before* format...
+         */
+        ret += mcam_cam_set_flip(cam);
+        return ret;
+}
+
+/*
+ * Get everything ready, and start grabbing frames.
+ */
+static int mcam_read_setup(struct mcam_camera *cam)
+{
+        int ret;
+        unsigned long flags;
+
+        /*
+         * Configuration.  If we still don't have DMA buffers,
+         * make one last, desperate attempt.
+         */
+        if (cam->buffer_mode == B_vmalloc && cam->nbufs == 0 &&
+                        mcam_alloc_dma_bufs(cam, 0))
+                return -ENOMEM;
+
+        if (mcam_needs_config(cam)) {
+                mcam_cam_configure(cam);
+                ret = mcam_ctlr_configure(cam);
+                if (ret)
+                        return ret;
+        }
+
+        /*
+         * Turn it loose.
+         */
+        spin_lock_irqsave(&cam->dev_lock, flags);
+        mcam_reset_buffers(cam);
+        mcam_ctlr_irq_enable(cam);
+        cam->state = S_STREAMING;
+        mcam_ctlr_start(cam);
+        spin_unlock_irqrestore(&cam->dev_lock, flags);
+        return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+/*
+ * Videobuf2 interface code.
+ */
+
+static int mcam_vb_queue_setup(struct vb2_queue *vq, unsigned int *nbufs,
+                unsigned int *num_planes, unsigned long sizes[],
+                void *alloc_ctxs[])
+{
+        struct mcam_camera *cam = vb2_get_drv_priv(vq);
+        int minbufs = (cam->buffer_mode == B_DMA_contig) ? 3 : 2;
+
+        sizes[0] = cam->pix_format.sizeimage;
+        *num_planes = 1; /* Someday we have to support planar formats... */
+        if (*nbufs < minbufs)
+                *nbufs = minbufs;
+        if (cam->buffer_mode == B_DMA_contig)
+                alloc_ctxs[0] = cam->vb_alloc_ctx;
+        return 0;
+}
+
+
+static void mcam_vb_buf_queue(struct vb2_buffer *vb)
+{
+        struct mcam_vb_buffer *mvb = vb_to_mvb(vb);
+        struct mcam_camera *cam = vb2_get_drv_priv(vb->vb2_queue);
+        unsigned long flags;
+        int start;
+
+        spin_lock_irqsave(&cam->dev_lock, flags);
+        start = (cam->state == S_BUFWAIT) && !list_empty(&cam->buffers);
+        list_add(&mvb->queue, &cam->buffers);
+        if (test_bit(CF_SG_RESTART, &cam->flags))
+                mcam_sg_restart(cam);
+        spin_unlock_irqrestore(&cam->dev_lock, flags);
+        if (start)
+                mcam_read_setup(cam);
+}
+
+
+/*
+ * vb2 uses these to release the mutex when waiting in dqbuf.  I'm
+ * not actually sure we need to do this (I'm not sure that vb2_dqbuf() needs
+ * to be called with the mutex held), but better safe than sorry.
+ */
+static void mcam_vb_wait_prepare(struct vb2_queue *vq)
+{
+        struct mcam_camera *cam = vb2_get_drv_priv(vq);
+
+        mutex_unlock(&cam->s_mutex);
+}
+
+static void mcam_vb_wait_finish(struct vb2_queue *vq)
+{
+        struct mcam_camera *cam = vb2_get_drv_priv(vq);
+
+        mutex_lock(&cam->s_mutex);
+}
+
+/*
+ * These need to be called with the mutex held from vb2
+ */
+static int mcam_vb_start_streaming(struct vb2_queue *vq)
+{
+        struct mcam_camera *cam = vb2_get_drv_priv(vq);
+
+        if (cam->state != S_IDLE)
+                return -EINVAL;
+        cam->sequence = 0;
+        /*
+         * Videobuf2 sneakily hoards all the buffers and won't
+         * give them to us until *after* streaming starts.  But
+         * we can't actually start streaming until we have a
+         * destination.  So go into a wait state and hope they
+         * give us buffers soon.
+         */
+        if (cam->buffer_mode != B_vmalloc && list_empty(&cam->buffers)) {
+                cam->state = S_BUFWAIT;
+                return 0;
+        }
+        return mcam_read_setup(cam);
+}
+
+static int mcam_vb_stop_streaming(struct vb2_queue *vq)
+{
+        struct mcam_camera *cam = vb2_get_drv_priv(vq);
+        unsigned long flags;
+
+        if (cam->state == S_BUFWAIT) {
+                /* They never gave us buffers */
+                cam->state = S_IDLE;
+                return 0;
+        }
+        if (cam->state != S_STREAMING)
+                return -EINVAL;
+        mcam_ctlr_stop_dma(cam);
+        /*
+         * VB2 reclaims the buffers, so we need to forget
+         * about them.
+         */
+        spin_lock_irqsave(&cam->dev_lock, flags);
+        INIT_LIST_HEAD(&cam->buffers);
+        spin_unlock_irqrestore(&cam->dev_lock, flags);
+        return 0;
+}
+
+
+static const struct vb2_ops mcam_vb2_ops = {
+        .queue_setup            = mcam_vb_queue_setup,
+        .buf_queue              = mcam_vb_buf_queue,
+        .start_streaming        = mcam_vb_start_streaming,
+        .stop_streaming         = mcam_vb_stop_streaming,
+        .wait_prepare           = mcam_vb_wait_prepare,
+        .wait_finish            = mcam_vb_wait_finish,
+};
+
+
+#ifdef MCAM_MODE_DMA_SG
+/*
+ * Scatter/gather mode uses all of the above functions plus a
+ * few extras to deal with DMA mapping.
+ */
+static int mcam_vb_sg_buf_init(struct vb2_buffer *vb)
+{
+        struct mcam_vb_buffer *mvb = vb_to_mvb(vb);
+        struct mcam_camera *cam = vb2_get_drv_priv(vb->vb2_queue);
+        int ndesc = cam->pix_format.sizeimage/PAGE_SIZE + 1;
+
+        mvb->dma_desc = dma_alloc_coherent(cam->dev,
+                        ndesc * sizeof(struct mcam_dma_desc),
+                        &mvb->dma_desc_pa, GFP_KERNEL);
+        if (mvb->dma_desc == NULL) {
+                cam_err(cam, "Unable to get DMA descriptor array\n");
+                return -ENOMEM;
+        }
+        return 0;
+}
+
+static int mcam_vb_sg_buf_prepare(struct vb2_buffer *vb)
+{
+        struct mcam_vb_buffer *mvb = vb_to_mvb(vb);
+        struct mcam_camera *cam = vb2_get_drv_priv(vb->vb2_queue);
+        struct vb2_dma_sg_desc *sgd = vb2_dma_sg_plane_desc(vb, 0);
+        struct mcam_dma_desc *desc = mvb->dma_desc;
+        struct scatterlist *sg;
+        int i;
+
+        mvb->dma_desc_nent = dma_map_sg(cam->dev, sgd->sglist, sgd->num_pages,
+                        DMA_FROM_DEVICE);
+        if (mvb->dma_desc_nent <= 0)
+                return -EIO;  /* Not sure what's right here */
+        for_each_sg(sgd->sglist, sg, mvb->dma_desc_nent, i) {
+                desc->dma_addr = sg_dma_address(sg);
+                desc->segment_len = sg_dma_len(sg);
+                desc++;
+        }
+        return 0;
+}
+
+static int mcam_vb_sg_buf_finish(struct vb2_buffer *vb)
+{
+        struct mcam_camera *cam = vb2_get_drv_priv(vb->vb2_queue);
+        struct vb2_dma_sg_desc *sgd = vb2_dma_sg_plane_desc(vb, 0);
+
+        dma_unmap_sg(cam->dev, sgd->sglist, sgd->num_pages, DMA_FROM_DEVICE);
+        return 0;
+}
+
+static void mcam_vb_sg_buf_cleanup(struct vb2_buffer *vb)
+{
+        struct mcam_camera *cam = vb2_get_drv_priv(vb->vb2_queue);
+        struct mcam_vb_buffer *mvb = vb_to_mvb(vb);
+        int ndesc = cam->pix_format.sizeimage/PAGE_SIZE + 1;
+
+        dma_free_coherent(cam->dev, ndesc * sizeof(struct mcam_dma_desc),
+                        mvb->dma_desc, mvb->dma_desc_pa);
+}
+
+
+static const struct vb2_ops mcam_vb2_sg_ops = {
+        .queue_setup            = mcam_vb_queue_setup,
+        .buf_init               = mcam_vb_sg_buf_init,
+        .buf_prepare            = mcam_vb_sg_buf_prepare,
+        .buf_queue              = mcam_vb_buf_queue,
+        .buf_finish             = mcam_vb_sg_buf_finish,
+        .buf_cleanup            = mcam_vb_sg_buf_cleanup,
+        .start_streaming        = mcam_vb_start_streaming,
+        .stop_streaming         = mcam_vb_stop_streaming,
+        .wait_prepare           = mcam_vb_wait_prepare,
+        .wait_finish            = mcam_vb_wait_finish,
+};
+
+#endif /* MCAM_MODE_DMA_SG */
+
+static int mcam_setup_vb2(struct mcam_camera *cam)
+{
+        struct vb2_queue *vq = &cam->vb_queue;
+
+        memset(vq, 0, sizeof(*vq));
+        vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        vq->drv_priv = cam;
+        INIT_LIST_HEAD(&cam->buffers);
+        switch (cam->buffer_mode) {
+        case B_DMA_contig:
+#ifdef MCAM_MODE_DMA_CONTIG
+                vq->ops = &mcam_vb2_ops;
+                vq->mem_ops = &vb2_dma_contig_memops;
+                cam->vb_alloc_ctx = vb2_dma_contig_init_ctx(cam->dev);
+                vq->io_modes = VB2_MMAP | VB2_USERPTR;
+                cam->dma_setup = mcam_ctlr_dma_contig;
+                cam->frame_complete = mcam_dma_contig_done;
+#endif
+                break;
+        case B_DMA_sg:
+#ifdef MCAM_MODE_DMA_SG
+                vq->ops = &mcam_vb2_sg_ops;
+                vq->mem_ops = &vb2_dma_sg_memops;
+                vq->io_modes = VB2_MMAP | VB2_USERPTR;
+                cam->dma_setup = mcam_ctlr_dma_sg;
+                cam->frame_complete = mcam_dma_sg_done;
+#endif
+                break;
+        case B_vmalloc:
+#ifdef MCAM_MODE_VMALLOC
+                tasklet_init(&cam->s_tasklet, mcam_frame_tasklet,
+                                (unsigned long) cam);
+                vq->ops = &mcam_vb2_ops;
+                vq->mem_ops = &vb2_vmalloc_memops;
+                vq->buf_struct_size = sizeof(struct mcam_vb_buffer);
+                vq->io_modes = VB2_MMAP;
+                cam->dma_setup = mcam_ctlr_dma_vmalloc;
+                cam->frame_complete = mcam_vmalloc_done;
+#endif
+                break;
+        }
+        return vb2_queue_init(vq);
+}
+
+static void mcam_cleanup_vb2(struct mcam_camera *cam)
+{
+        vb2_queue_release(&cam->vb_queue);
+#ifdef MCAM_MODE_DMA_CONTIG
+        if (cam->buffer_mode == B_DMA_contig)
+                vb2_dma_contig_cleanup_ctx(cam->vb_alloc_ctx);
+#endif
+}
+
+
+/* ---------------------------------------------------------------------- */
+/*
+ * The long list of V4L2 ioctl() operations.
+ */
+
+static int mcam_vidioc_streamon(struct file *filp, void *priv,
+                enum v4l2_buf_type type)
+{
+        struct mcam_camera *cam = filp->private_data;
+        int ret;
+
+        mutex_lock(&cam->s_mutex);
+        ret = vb2_streamon(&cam->vb_queue, type);
+        mutex_unlock(&cam->s_mutex);
+        return ret;
+}
+
+
+static int mcam_vidioc_streamoff(struct file *filp, void *priv,
+                enum v4l2_buf_type type)
+{
+        struct mcam_camera *cam = filp->private_data;
+        int ret;
+
+        mutex_lock(&cam->s_mutex);
+        ret = vb2_streamoff(&cam->vb_queue, type);
+        mutex_unlock(&cam->s_mutex);
+        return ret;
+}
+
+
+static int mcam_vidioc_reqbufs(struct file *filp, void *priv,
+                struct v4l2_requestbuffers *req)
+{
+        struct mcam_camera *cam = filp->private_data;
+        int ret;
+
+        mutex_lock(&cam->s_mutex);
+        ret = vb2_reqbufs(&cam->vb_queue, req);
+        mutex_unlock(&cam->s_mutex);
+        return ret;
+}
+
+
+static int mcam_vidioc_querybuf(struct file *filp, void *priv,
+                struct v4l2_buffer *buf)
+{
+        struct mcam_camera *cam = filp->private_data;
+        int ret;
+
+        mutex_lock(&cam->s_mutex);
+        ret = vb2_querybuf(&cam->vb_queue, buf);
+        mutex_unlock(&cam->s_mutex);
+        return ret;
+}
+
+static int mcam_vidioc_qbuf(struct file *filp, void *priv,
+                struct v4l2_buffer *buf)
+{
+        struct mcam_camera *cam = filp->private_data;
+        int ret;
+
+        mutex_lock(&cam->s_mutex);
+        ret = vb2_qbuf(&cam->vb_queue, buf);
+        mutex_unlock(&cam->s_mutex);
+        return ret;
+}
+
+static int mcam_vidioc_dqbuf(struct file *filp, void *priv,
+                struct v4l2_buffer *buf)
+{
+        struct mcam_camera *cam = filp->private_data;
+        int ret;
+
+        mutex_lock(&cam->s_mutex);
+        ret = vb2_dqbuf(&cam->vb_queue, buf, filp->f_flags & O_NONBLOCK);
+        mutex_unlock(&cam->s_mutex);
+        return ret;
+}
+
+
+
+static int mcam_vidioc_queryctrl(struct file *filp, void *priv,
+                struct v4l2_queryctrl *qc)
+{
+        struct mcam_camera *cam = priv;
+        int ret;
+
+        mutex_lock(&cam->s_mutex);
+        ret = sensor_call(cam, core, queryctrl, qc);
+        mutex_unlock(&cam->s_mutex);
+        return ret;
+}
+
+
+static int mcam_vidioc_g_ctrl(struct file *filp, void *priv,
+                struct v4l2_control *ctrl)
+{
+        struct mcam_camera *cam = priv;
+        int ret;
+
+        mutex_lock(&cam->s_mutex);
+        ret = sensor_call(cam, core, g_ctrl, ctrl);
+        mutex_unlock(&cam->s_mutex);
+        return ret;
+}
+
+
+static int mcam_vidioc_s_ctrl(struct file *filp, void *priv,
+                struct v4l2_control *ctrl)
+{
+        struct mcam_camera *cam = priv;
+        int ret;
+
+        mutex_lock(&cam->s_mutex);
+        ret = sensor_call(cam, core, s_ctrl, ctrl);
+        mutex_unlock(&cam->s_mutex);
+        return ret;
+}
+
+
+static int mcam_vidioc_querycap(struct file *file, void *priv,
+                struct v4l2_capability *cap)
+{
+        strcpy(cap->driver, "marvell_ccic");
+        strcpy(cap->card, "marvell_ccic");
+        cap->version = 1;
+        cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
+                V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+        return 0;
+}
+
+
+static int mcam_vidioc_enum_fmt_vid_cap(struct file *filp,
+                void *priv, struct v4l2_fmtdesc *fmt)
+{
+        if (fmt->index >= N_MCAM_FMTS)
+                return -EINVAL;
+        strlcpy(fmt->description, mcam_formats[fmt->index].desc,
+                        sizeof(fmt->description));
+        fmt->pixelformat = mcam_formats[fmt->index].pixelformat;
+        return 0;
+}
+
+static int mcam_vidioc_try_fmt_vid_cap(struct file *filp, void *priv,
+                struct v4l2_format *fmt)
+{
+        struct mcam_camera *cam = priv;
+        struct mcam_format_struct *f;
+        struct v4l2_pix_format *pix = &fmt->fmt.pix;
+        struct v4l2_mbus_framefmt mbus_fmt;
+        int ret;
+
+        f = mcam_find_format(pix->pixelformat);
+        pix->pixelformat = f->pixelformat;
+        v4l2_fill_mbus_format(&mbus_fmt, pix, f->mbus_code);
+        mutex_lock(&cam->s_mutex);
+        ret = sensor_call(cam, video, try_mbus_fmt, &mbus_fmt);
+        mutex_unlock(&cam->s_mutex);
+        v4l2_fill_pix_format(pix, &mbus_fmt);
+        pix->bytesperline = pix->width * f->bpp;
+        pix->sizeimage = pix->height * pix->bytesperline;
+        return ret;
+}
+
+static int mcam_vidioc_s_fmt_vid_cap(struct file *filp, void *priv,
+                struct v4l2_format *fmt)
+{
+        struct mcam_camera *cam = priv;
+        struct mcam_format_struct *f;
+        int ret;
+
+        /*
+         * Can't do anything if the device is not idle
+         * Also can't if there are streaming buffers in place.
+         */
+        if (cam->state != S_IDLE || cam->vb_queue.num_buffers > 0)
+                return -EBUSY;
+
+        f = mcam_find_format(fmt->fmt.pix.pixelformat);
+
+        /*
+         * See if the formatting works in principle.
+         */
+        ret = mcam_vidioc_try_fmt_vid_cap(filp, priv, fmt);
+        if (ret)
+                return ret;
+        /*
+         * Now we start to change things for real, so let's do it
+         * under lock.
+         */
+        mutex_lock(&cam->s_mutex);
+        cam->pix_format = fmt->fmt.pix;
+        cam->mbus_code = f->mbus_code;
+
+        /*
+         * Make sure we have appropriate DMA buffers.
+         */
+        if (cam->buffer_mode == B_vmalloc) {
+                ret = mcam_check_dma_buffers(cam);
+                if (ret)
+                        goto out;
+        }
+        mcam_set_config_needed(cam, 1);
+        ret = 0;
+out:
+        mutex_unlock(&cam->s_mutex);
+        return ret;
+}
+
+/*
+ * Return our stored notion of how the camera is/should be configured.
+ * The V4l2 spec wants us to be smarter, and actually get this from
+ * the camera (and not mess with it at open time).  Someday.
+ */
+static int mcam_vidioc_g_fmt_vid_cap(struct file *filp, void *priv,
+                struct v4l2_format *f)
+{
+        struct mcam_camera *cam = priv;
+
+        f->fmt.pix = cam->pix_format;
+        return 0;
+}
+
+/*
+ * We only have one input - the sensor - so minimize the nonsense here.
+ */
+static int mcam_vidioc_enum_input(struct file *filp, void *priv,
+                struct v4l2_input *input)
+{
+        if (input->index != 0)
+                return -EINVAL;
+
+        input->type = V4L2_INPUT_TYPE_CAMERA;
+        input->std = V4L2_STD_ALL; /* Not sure what should go here */
+        strcpy(input->name, "Camera");
+        return 0;
+}
+
+static int mcam_vidioc_g_input(struct file *filp, void *priv, unsigned int *i)
+{
+        *i = 0;
+        return 0;
+}
+
+static int mcam_vidioc_s_input(struct file *filp, void *priv, unsigned int i)
+{
+        if (i != 0)
+                return -EINVAL;
+        return 0;
+}
+
+/* from vivi.c */
+static int mcam_vidioc_s_std(struct file *filp, void *priv, v4l2_std_id *a)
+{
+        return 0;
+}
+
+/*
+ * G/S_PARM.  Most of this is done by the sensor, but we are
+ * the level which controls the number of read buffers.
+ */
+static int mcam_vidioc_g_parm(struct file *filp, void *priv,
+                struct v4l2_streamparm *parms)
+{
+        struct mcam_camera *cam = priv;
+        int ret;
+
+        mutex_lock(&cam->s_mutex);
+        ret = sensor_call(cam, video, g_parm, parms);
+        mutex_unlock(&cam->s_mutex);
+        parms->parm.capture.readbuffers = n_dma_bufs;
+        return ret;
+}
+
+static int mcam_vidioc_s_parm(struct file *filp, void *priv,
+                struct v4l2_streamparm *parms)
+{
+        struct mcam_camera *cam = priv;
+        int ret;
+
+        mutex_lock(&cam->s_mutex);
+        ret = sensor_call(cam, video, s_parm, parms);
+        mutex_unlock(&cam->s_mutex);
+        parms->parm.capture.readbuffers = n_dma_bufs;
+        return ret;
+}
+
+static int mcam_vidioc_g_chip_ident(struct file *file, void *priv,
+                struct v4l2_dbg_chip_ident *chip)
+{
+        struct mcam_camera *cam = priv;
+
+        chip->ident = V4L2_IDENT_NONE;
+        chip->revision = 0;
+        if (v4l2_chip_match_host(&chip->match)) {
+                chip->ident = cam->chip_id;
+                return 0;
+        }
+        return sensor_call(cam, core, g_chip_ident, chip);
+}
+
+static int mcam_vidioc_enum_framesizes(struct file *filp, void *priv,
+                struct v4l2_frmsizeenum *sizes)
+{
+        struct mcam_camera *cam = priv;
+        int ret;
+
+        mutex_lock(&cam->s_mutex);
+        ret = sensor_call(cam, video, enum_framesizes, sizes);
+        mutex_unlock(&cam->s_mutex);
+        return ret;
+}
+
+static int mcam_vidioc_enum_frameintervals(struct file *filp, void *priv,
+                struct v4l2_frmivalenum *interval)
+{
+        struct mcam_camera *cam = priv;
+        int ret;
+
+        mutex_lock(&cam->s_mutex);
+        ret = sensor_call(cam, video, enum_frameintervals, interval);
+        mutex_unlock(&cam->s_mutex);
+        return ret;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int mcam_vidioc_g_register(struct file *file, void *priv,
+                struct v4l2_dbg_register *reg)
+{
+        struct mcam_camera *cam = priv;
+
+        if (v4l2_chip_match_host(&reg->match)) {
+                reg->val = mcam_reg_read(cam, reg->reg);
+                reg->size = 4;
+                return 0;
+        }
+        return sensor_call(cam, core, g_register, reg);
+}
+
+static int mcam_vidioc_s_register(struct file *file, void *priv,
+                struct v4l2_dbg_register *reg)
+{
+        struct mcam_camera *cam = priv;
+
+        if (v4l2_chip_match_host(&reg->match)) {
+                mcam_reg_write(cam, reg->reg, reg->val);
+                return 0;
+        }
+        return sensor_call(cam, core, s_register, reg);
+}
+#endif
+
+static const struct v4l2_ioctl_ops mcam_v4l_ioctl_ops = {
+        .vidioc_querycap        = mcam_vidioc_querycap,
+        .vidioc_enum_fmt_vid_cap = mcam_vidioc_enum_fmt_vid_cap,
+        .vidioc_try_fmt_vid_cap = mcam_vidioc_try_fmt_vid_cap,
+        .vidioc_s_fmt_vid_cap   = mcam_vidioc_s_fmt_vid_cap,
+        .vidioc_g_fmt_vid_cap   = mcam_vidioc_g_fmt_vid_cap,
+        .vidioc_enum_input      = mcam_vidioc_enum_input,
+        .vidioc_g_input         = mcam_vidioc_g_input,
+        .vidioc_s_input         = mcam_vidioc_s_input,
+        .vidioc_s_std           = mcam_vidioc_s_std,
+        .vidioc_reqbufs         = mcam_vidioc_reqbufs,
+        .vidioc_querybuf        = mcam_vidioc_querybuf,
+        .vidioc_qbuf            = mcam_vidioc_qbuf,
+        .vidioc_dqbuf           = mcam_vidioc_dqbuf,
+        .vidioc_streamon        = mcam_vidioc_streamon,
+        .vidioc_streamoff       = mcam_vidioc_streamoff,
+        .vidioc_queryctrl       = mcam_vidioc_queryctrl,
+        .vidioc_g_ctrl          = mcam_vidioc_g_ctrl,
+        .vidioc_s_ctrl          = mcam_vidioc_s_ctrl,
+        .vidioc_g_parm          = mcam_vidioc_g_parm,
+        .vidioc_s_parm          = mcam_vidioc_s_parm,
+        .vidioc_enum_framesizes = mcam_vidioc_enum_framesizes,
+        .vidioc_enum_frameintervals = mcam_vidioc_enum_frameintervals,
+        .vidioc_g_chip_ident    = mcam_vidioc_g_chip_ident,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+        .vidioc_g_register      = mcam_vidioc_g_register,
+        .vidioc_s_register      = mcam_vidioc_s_register,
+#endif
+};
+
+/* ---------------------------------------------------------------------- */
+/*
+ * Our various file operations.
+ */
+static int mcam_v4l_open(struct file *filp)
+{
+        struct mcam_camera *cam = video_drvdata(filp);
+        int ret = 0;
+
+        filp->private_data = cam;
+
+        frames = singles = delivered = 0;
+        mutex_lock(&cam->s_mutex);
+        if (cam->users == 0) {
+                ret = mcam_setup_vb2(cam);
+                if (ret)
+                        goto out;
+                mcam_ctlr_power_up(cam);
+                __mcam_cam_reset(cam);
+                mcam_set_config_needed(cam, 1);
+        }
+        (cam->users)++;
+out:
+        mutex_unlock(&cam->s_mutex);
+        return ret;
+}
+
+
+static int mcam_v4l_release(struct file *filp)
+{
+        struct mcam_camera *cam = filp->private_data;
+
+        cam_err(cam, "Release, %d frames, %d singles, %d delivered\n", frames,
+                        singles, delivered);
+        mutex_lock(&cam->s_mutex);
+        (cam->users)--;
+        if (filp == cam->owner) {
+                mcam_ctlr_stop_dma(cam);
+                cam->owner = NULL;
+        }
+        if (cam->users == 0) {
+                mcam_cleanup_vb2(cam);
+                mcam_ctlr_power_down(cam);
+                if (cam->buffer_mode == B_vmalloc && alloc_bufs_at_read)
+                        mcam_free_dma_bufs(cam);
+        }
+        mutex_unlock(&cam->s_mutex);
+        return 0;
+}
+
+static ssize_t mcam_v4l_read(struct file *filp,
+                char __user *buffer, size_t len, loff_t *pos)
+{
+        struct mcam_camera *cam = filp->private_data;
+        int ret;
+
+        mutex_lock(&cam->s_mutex);
+        ret = vb2_read(&cam->vb_queue, buffer, len, pos,
+                        filp->f_flags & O_NONBLOCK);
+        mutex_unlock(&cam->s_mutex);
+        return ret;
+}
+
+
+
+static unsigned int mcam_v4l_poll(struct file *filp,
+                struct poll_table_struct *pt)
+{
+        struct mcam_camera *cam = filp->private_data;
+        int ret;
+
+        mutex_lock(&cam->s_mutex);
+        ret = vb2_poll(&cam->vb_queue, filp, pt);
+        mutex_unlock(&cam->s_mutex);
+        return ret;
+}
+
+
+static int mcam_v4l_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+        struct mcam_camera *cam = filp->private_data;
+        int ret;
+
+        mutex_lock(&cam->s_mutex);
+        ret = vb2_mmap(&cam->vb_queue, vma);
+        mutex_unlock(&cam->s_mutex);
+        return ret;
+}
+
+
+
+static const struct v4l2_file_operations mcam_v4l_fops = {
+        .owner = THIS_MODULE,
+        .open = mcam_v4l_open,
+        .release = mcam_v4l_release,
+        .read = mcam_v4l_read,
+        .poll = mcam_v4l_poll,
+        .mmap = mcam_v4l_mmap,
+        .unlocked_ioctl = video_ioctl2,
+};
+
+
+/*
+ * This template device holds all of those v4l2 methods; we
+ * clone it for specific real devices.
+ */
+static struct video_device mcam_v4l_template = {
+        .name = "mcam",
+        .tvnorms = V4L2_STD_NTSC_M,
+        .current_norm = V4L2_STD_NTSC_M,  /* make mplayer happy */
+
+        .fops = &mcam_v4l_fops,
+        .ioctl_ops = &mcam_v4l_ioctl_ops,
+        .release = video_device_release_empty,
+};
+
+/* ---------------------------------------------------------------------- */
+/*
+ * Interrupt handler stuff
+ */
+static void mcam_frame_complete(struct mcam_camera *cam, int frame)
+{
+        /*
+         * Basic frame housekeeping.
+         */
+        set_bit(frame, &cam->flags);
+        clear_bit(CF_DMA_ACTIVE, &cam->flags);
+        cam->next_buf = frame;
+        cam->buf_seq[frame] = ++(cam->sequence);
+        frames++;
+        /*
+         * "This should never happen"
+         */
+        if (cam->state != S_STREAMING)
+                return;
+        /*
+         * Process the frame and set up the next one.
+         */
+        cam->frame_complete(cam, frame);
+}
+
+
+/*
+ * The interrupt handler; this needs to be called from the
+ * platform irq handler with the lock held.
+ */
+int mccic_irq(struct mcam_camera *cam, unsigned int irqs)
+{
+        unsigned int frame, handled = 0;
+
+        mcam_reg_write(cam, REG_IRQSTAT, FRAMEIRQS); /* Clear'em all */
+        /*
+         * Handle any frame completions.  There really should
+         * not be more than one of these, or we have fallen
+         * far behind.
+         *
+         * When running in S/G mode, the frame number lacks any
+         * real meaning - there's only one descriptor array - but
+         * the controller still picks a different one to signal
+         * each time.
+         */
+        for (frame = 0; frame < cam->nbufs; frame++)
+                if (irqs & (IRQ_EOF0 << frame)) {
+                        mcam_frame_complete(cam, frame);
+                        handled = 1;
+                }
+        /*
+         * If a frame starts, note that we have DMA active.  This
+         * code assumes that we won't get multiple frame interrupts
+         * at once; may want to rethink that.
+         */
+        if (irqs & (IRQ_SOF0 | IRQ_SOF1 | IRQ_SOF2)) {
+                set_bit(CF_DMA_ACTIVE, &cam->flags);
+                handled = 1;
+                if (cam->buffer_mode == B_DMA_sg)
+                        mcam_ctlr_stop(cam);
+        }
+        return handled;
+}
+
+/* ---------------------------------------------------------------------- */
+/*
+ * Registration and such.
+ */
+static struct ov7670_config sensor_cfg = {
+        /*
+         * Exclude QCIF mode, because it only captures a tiny portion
+         * of the sensor FOV
+         */
+        .min_width = 320,
+        .min_height = 240,
+};
+
+
+int mccic_register(struct mcam_camera *cam)
+{
+        struct i2c_board_info ov7670_info = {
+                .type = "ov7670",
+                .addr = 0x42 >> 1,
+                .platform_data = &sensor_cfg,
+        };
+        int ret;
+
+        /*
+         * Validate the requested buffer mode.
+         */
+        if (buffer_mode >= 0)
+                cam->buffer_mode = buffer_mode;
+        if (cam->buffer_mode == B_DMA_sg &&
+                        cam->chip_id == V4L2_IDENT_CAFE) {
+                printk(KERN_ERR "marvell-cam: Cafe can't do S/G I/O, "
+                        "attempting vmalloc mode instead\n");
+                cam->buffer_mode = B_vmalloc;
+        }
+        if (!mcam_buffer_mode_supported(cam->buffer_mode)) {
+                printk(KERN_ERR "marvell-cam: buffer mode %d unsupported\n",
+                                cam->buffer_mode);
+                return -EINVAL;
+        }
+        /*
+         * Register with V4L
+         */
+        ret = v4l2_device_register(cam->dev, &cam->v4l2_dev);
+        if (ret)
+                return ret;
+
+        mutex_init(&cam->s_mutex);
+        cam->state = S_NOTREADY;
+        mcam_set_config_needed(cam, 1);
+        cam->pix_format = mcam_def_pix_format;
+        cam->mbus_code = mcam_def_mbus_code;
+        INIT_LIST_HEAD(&cam->buffers);
+        mcam_ctlr_init(cam);
+
+        /*
+         * Try to find the sensor.
+         */
+        sensor_cfg.clock_speed = cam->clock_speed;
+        sensor_cfg.use_smbus = cam->use_smbus;
+        cam->sensor_addr = ov7670_info.addr;
+        cam->sensor = v4l2_i2c_new_subdev_board(&cam->v4l2_dev,
+                        cam->i2c_adapter, &ov7670_info, NULL);
+        if (cam->sensor == NULL) {
+                ret = -ENODEV;
+                goto out_unregister;
+        }
+
+        ret = mcam_cam_init(cam);
+        if (ret)
+                goto out_unregister;
+        /*
+         * Get the v4l2 setup done.
+         */
+        mutex_lock(&cam->s_mutex);
+        cam->vdev = mcam_v4l_template;
+        cam->vdev.debug = 0;
+        cam->vdev.v4l2_dev = &cam->v4l2_dev;
+        ret = video_register_device(&cam->vdev, VFL_TYPE_GRABBER, -1);
+        if (ret)
+                goto out;
+        video_set_drvdata(&cam->vdev, cam);
+
+        /*
+         * If so requested, try to get our DMA buffers now.
+         */
+        if (cam->buffer_mode == B_vmalloc && !alloc_bufs_at_read) {
+                if (mcam_alloc_dma_bufs(cam, 1))
+                        cam_warn(cam, "Unable to alloc DMA buffers at load"
+                                        " will try again later.");
+        }
+
+out:
+        mutex_unlock(&cam->s_mutex);
+        return ret;
+out_unregister:
+        v4l2_device_unregister(&cam->v4l2_dev);
+        return ret;
+}
+
+
+void mccic_shutdown(struct mcam_camera *cam)
+{
+        /*
+         * If we have no users (and we really, really should have no
+         * users) the device will already be powered down.  Trying to
+         * take it down again will wedge the machine, which is frowned
+         * upon.
+         */
+        if (cam->users > 0) {
+                cam_warn(cam, "Removing a device with users!\n");
+                mcam_ctlr_power_down(cam);
+        }
+        vb2_queue_release(&cam->vb_queue);
+        if (cam->buffer_mode == B_vmalloc)
+                mcam_free_dma_bufs(cam);
+        video_unregister_device(&cam->vdev);
+        v4l2_device_unregister(&cam->v4l2_dev);
+}
+
+/*
+ * Power management
+ */
+#ifdef CONFIG_PM
+
+void mccic_suspend(struct mcam_camera *cam)
+{
+        enum mcam_state cstate = cam->state;
+
+        mcam_ctlr_stop_dma(cam);
+        mcam_ctlr_power_down(cam);
+        cam->state = cstate;
+}
+
+int mccic_resume(struct mcam_camera *cam)
+{
+        int ret = 0;
+
+        mutex_lock(&cam->s_mutex);
+        if (cam->users > 0) {
+                mcam_ctlr_power_up(cam);
+                __mcam_cam_reset(cam);
+        } else {
+                mcam_ctlr_power_down(cam);
+        }
+        mutex_unlock(&cam->s_mutex);
+
+        set_bit(CF_CONFIG_NEEDED, &cam->flags);
+        if (cam->state == S_STREAMING)
+                ret = mcam_read_setup(cam);
+        return ret;
+}
+#endif /* CONFIG_PM */
diff --git a/drivers/media/video/marvell-ccic/mcam-core.h b/drivers/media/video/marvell-ccic/mcam-core.h
new file mode 100644
index 00000000000..917200e6325
--- /dev/null
+++ b/drivers/media/video/marvell-ccic/mcam-core.h
@@ -0,0 +1,323 @@
+/*
+ * Marvell camera core structures.
+ *
+ * Copyright 2011 Jonathan Corbet corbet@lwn.net
+ */
+#ifndef _MCAM_CORE_H
+#define _MCAM_CORE_H
+
+#include <linux/list.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-dev.h>
+#include <media/videobuf2-core.h>
+
+/*
+ * Create our own symbols for the supported buffer modes, but, for now,
+ * base them entirely on which videobuf2 options have been selected.
+ */
+#if defined(CONFIG_VIDEOBUF2_VMALLOC) || defined(CONFIG_VIDEOBUF2_VMALLOC_MODULE)
+#define MCAM_MODE_VMALLOC 1
+#endif
+
+#if defined(CONFIG_VIDEOBUF2_DMA_CONTIG) || defined(CONFIG_VIDEOBUF2_DMA_CONTIG_MODULE)
+#define MCAM_MODE_DMA_CONTIG 1
+#endif
+
+#if defined(CONFIG_VIDEOBUF2_DMA_SG) || defined(CONFIG_VIDEOBUF2_DMA_SG_MODULE)
+#define MCAM_MODE_DMA_SG 1
+#endif
+
+#if !defined(MCAM_MODE_VMALLOC) && !defined(MCAM_MODE_DMA_CONTIG) && \
+        !defined(MCAM_MODE_DMA_SG)
+#error One of the videobuf buffer modes must be selected in the config
+#endif
+
+
+enum mcam_state {
+        S_NOTREADY,     /* Not yet initialized */
+        S_IDLE,         /* Just hanging around */
+        S_FLAKED,       /* Some sort of problem */
+        S_STREAMING,    /* Streaming data */
+        S_BUFWAIT       /* streaming requested but no buffers yet */
+};
+#define MAX_DMA_BUFS 3
+
+/*
+ * Different platforms work best with different buffer modes, so we
+ * let the platform pick.
+ */
+enum mcam_buffer_mode {
+        B_vmalloc = 0,
+        B_DMA_contig = 1,
+        B_DMA_sg = 2
+};
+
+/*
+ * Is a given buffer mode supported by the current kernel configuration?
+ */
+static inline int mcam_buffer_mode_supported(enum mcam_buffer_mode mode)
+{
+        switch (mode) {
+#ifdef MCAM_MODE_VMALLOC
+        case B_vmalloc:
+#endif
+#ifdef MCAM_MODE_DMA_CONTIG
+        case B_DMA_contig:
+#endif
+#ifdef MCAM_MODE_DMA_SG
+        case B_DMA_sg:
+#endif
+                return 1;
+        default:
+                return 0;
+        }
+}
+
+
+/*
+ * A description of one of our devices.
+ * Locking: controlled by s_mutex.  Certain fields, however, require
+ *          the dev_lock spinlock; they are marked as such by comments.
+ *          dev_lock is also required for access to device registers.
+ */
+struct mcam_camera {
+        /*
+         * These fields should be set by the platform code prior to
+         * calling mcam_register().
+         */
+        struct i2c_adapter *i2c_adapter;
+        unsigned char __iomem *regs;
+        spinlock_t dev_lock;
+        struct device *dev; /* For messages, dma alloc */
+        unsigned int chip_id;
+        short int clock_speed;  /* Sensor clock speed, default 30 */
+        short int use_smbus;    /* SMBUS or straight I2c? */
+        enum mcam_buffer_mode buffer_mode;
+        /*
+         * Callbacks from the core to the platform code.
+         */
+        void (*plat_power_up) (struct mcam_camera *cam);
+        void (*plat_power_down) (struct mcam_camera *cam);
+
+        /*
+         * Everything below here is private to the mcam core and
+         * should not be touched by the platform code.
+         */
+        struct v4l2_device v4l2_dev;
+        enum mcam_state state;
+        unsigned long flags;            /* Buffer status, mainly (dev_lock) */
+        int users;                      /* How many open FDs */
+        struct file *owner;             /* Who has data access (v4l2) */
+
+        /*
+         * Subsystem structures.
+         */
+        struct video_device vdev;
+        struct v4l2_subdev *sensor;
+        unsigned short sensor_addr;
+
+        /* Videobuf2 stuff */
+        struct vb2_queue vb_queue;
+        struct list_head buffers;       /* Available frames */
+
+        unsigned int nbufs;             /* How many are alloc'd */
+        int next_buf;                   /* Next to consume (dev_lock) */
+
+        /* DMA buffers - vmalloc mode */
+#ifdef MCAM_MODE_VMALLOC
+        unsigned int dma_buf_size;      /* allocated size */
+        void *dma_bufs[MAX_DMA_BUFS];   /* Internal buffer addresses */
+        dma_addr_t dma_handles[MAX_DMA_BUFS]; /* Buffer bus addresses */
+        struct tasklet_struct s_tasklet;
+#endif
+        unsigned int sequence;          /* Frame sequence number */
+        unsigned int buf_seq[MAX_DMA_BUFS]; /* Sequence for individual bufs */
+
+        /* DMA buffers - DMA modes */
+        struct mcam_vb_buffer *vb_bufs[MAX_DMA_BUFS];
+        struct vb2_alloc_ctx *vb_alloc_ctx;
+
+        /* Mode-specific ops, set at open time */
+        void (*dma_setup)(struct mcam_camera *cam);
+        void (*frame_complete)(struct mcam_camera *cam, int frame);
+
+        /* Current operating parameters */
+        u32 sensor_type;                /* Currently ov7670 only */
+        struct v4l2_pix_format pix_format;
+        enum v4l2_mbus_pixelcode mbus_code;
+
+        /* Locks */
+        struct mutex s_mutex; /* Access to this structure */
+};
+
+
+/*
+ * Register I/O functions.  These are here because the platform code
+ * may legitimately need to mess with the register space.
+ */
+/*
+ * Device register I/O
+ */
+static inline void mcam_reg_write(struct mcam_camera *cam, unsigned int reg,
+                unsigned int val)
+{
+        iowrite32(val, cam->regs + reg);
+}
+
+static inline unsigned int mcam_reg_read(struct mcam_camera *cam,
+                unsigned int reg)
+{
+        return ioread32(cam->regs + reg);
+}
+
+
+static inline void mcam_reg_write_mask(struct mcam_camera *cam, unsigned int reg,
+                unsigned int val, unsigned int mask)
+{
+        unsigned int v = mcam_reg_read(cam, reg);
+
+        v = (v & ~mask) | (val & mask);
+        mcam_reg_write(cam, reg, v);
+}
+
+static inline void mcam_reg_clear_bit(struct mcam_camera *cam,
+                unsigned int reg, unsigned int val)
+{
+        mcam_reg_write_mask(cam, reg, 0, val);
+}
+
+static inline void mcam_reg_set_bit(struct mcam_camera *cam,
+                unsigned int reg, unsigned int val)
+{
+        mcam_reg_write_mask(cam, reg, val, val);
+}
+
+/*
+ * Functions for use by platform code.
+ */
+int mccic_register(struct mcam_camera *cam);
+int mccic_irq(struct mcam_camera *cam, unsigned int irqs);
+void mccic_shutdown(struct mcam_camera *cam);
+#ifdef CONFIG_PM
+void mccic_suspend(struct mcam_camera *cam);
+int mccic_resume(struct mcam_camera *cam);
+#endif
+
+/*
+ * Register definitions for the m88alp01 camera interface.  Offsets in bytes
+ * as given in the spec.
+ */
+#define REG_Y0BAR       0x00
+#define REG_Y1BAR       0x04
+#define REG_Y2BAR       0x08
+/* ... */
+
+#define REG_IMGPITCH    0x24    /* Image pitch register */
+#define   IMGP_YP_SHFT    2             /* Y pitch params */
+#define   IMGP_YP_MASK    0x00003ffc    /* Y pitch field */
+#define   IMGP_UVP_SHFT   18            /* UV pitch (planar) */
+#define   IMGP_UVP_MASK   0x3ffc0000
+#define REG_IRQSTATRAW  0x28    /* RAW IRQ Status */
+#define   IRQ_EOF0        0x00000001    /* End of frame 0 */
+#define   IRQ_EOF1        0x00000002    /* End of frame 1 */
+#define   IRQ_EOF2        0x00000004    /* End of frame 2 */
+#define   IRQ_SOF0        0x00000008    /* Start of frame 0 */
+#define   IRQ_SOF1        0x00000010    /* Start of frame 1 */
+#define   IRQ_SOF2        0x00000020    /* Start of frame 2 */
+#define   IRQ_OVERFLOW    0x00000040    /* FIFO overflow */
+#define   IRQ_TWSIW       0x00010000    /* TWSI (smbus) write */
+#define   IRQ_TWSIR       0x00020000    /* TWSI read */
+#define   IRQ_TWSIE       0x00040000    /* TWSI error */
+#define   TWSIIRQS (IRQ_TWSIW|IRQ_TWSIR|IRQ_TWSIE)
+#define   FRAMEIRQS (IRQ_EOF0|IRQ_EOF1|IRQ_EOF2|IRQ_SOF0|IRQ_SOF1|IRQ_SOF2)
+#define   ALLIRQS (TWSIIRQS|FRAMEIRQS|IRQ_OVERFLOW)
+#define REG_IRQMASK     0x2c    /* IRQ mask - same bits as IRQSTAT */
+#define REG_IRQSTAT     0x30    /* IRQ status / clear */
+
+#define REG_IMGSIZE     0x34    /* Image size */
+#define  IMGSZ_V_MASK     0x1fff0000
+#define  IMGSZ_V_SHIFT    16
+#define  IMGSZ_H_MASK     0x00003fff
+#define REG_IMGOFFSET   0x38    /* IMage offset */
+
+#define REG_CTRL0       0x3c    /* Control 0 */
+#define   C0_ENABLE       0x00000001    /* Makes the whole thing go */
+
+/* Mask for all the format bits */
+#define   C0_DF_MASK      0x00fffffc    /* Bits 2-23 */
+
+/* RGB ordering */
+#define   C0_RGB4_RGBX    0x00000000
+#define   C0_RGB4_XRGB    0x00000004
+#define   C0_RGB4_BGRX    0x00000008
+#define   C0_RGB4_XBGR    0x0000000c
+#define   C0_RGB5_RGGB    0x00000000
+#define   C0_RGB5_GRBG    0x00000004
+#define   C0_RGB5_GBRG    0x00000008
+#define   C0_RGB5_BGGR    0x0000000c
+
+/* Spec has two fields for DIN and DOUT, but they must match, so
+   combine them here. */
+#define   C0_DF_YUV       0x00000000    /* Data is YUV      */
+#define   C0_DF_RGB       0x000000a0    /* ... RGB                  */
+#define   C0_DF_BAYER     0x00000140    /* ... Bayer                */
+/* 8-8-8 must be missing from the below - ask */
+#define   C0_RGBF_565     0x00000000
+#define   C0_RGBF_444     0x00000800
+#define   C0_RGB_BGR      0x00001000    /* Blue comes first */
+#define   C0_YUV_PLANAR   0x00000000    /* YUV 422 planar format */
+#define   C0_YUV_PACKED   0x00008000    /* YUV 422 packed       */
+#define   C0_YUV_420PL    0x0000a000    /* YUV 420 planar       */
+/* Think that 420 packed must be 111 - ask */
+#define   C0_YUVE_YUYV    0x00000000    /* Y1CbY0Cr             */
+#define   C0_YUVE_YVYU    0x00010000    /* Y1CrY0Cb             */
+#define   C0_YUVE_VYUY    0x00020000    /* CrY1CbY0             */
+#define   C0_YUVE_UYVY    0x00030000    /* CbY1CrY0             */
+#define   C0_YUVE_XYUV    0x00000000    /* 420: .YUV            */
+#define   C0_YUVE_XYVU    0x00010000    /* 420: .YVU            */
+#define   C0_YUVE_XUVY    0x00020000    /* 420: .UVY            */
+#define   C0_YUVE_XVUY    0x00030000    /* 420: .VUY            */
+/* Bayer bits 18,19 if needed */
+#define   C0_HPOL_LOW     0x01000000    /* HSYNC polarity active low */
+#define   C0_VPOL_LOW     0x02000000    /* VSYNC polarity active low */
+#define   C0_VCLK_LOW     0x04000000    /* VCLK on falling edge */
+#define   C0_DOWNSCALE    0x08000000    /* Enable downscaler */
+#define   C0_SIFM_MASK    0xc0000000    /* SIF mode bits */
+#define   C0_SIF_HVSYNC   0x00000000    /* Use H/VSYNC */
+#define   CO_SOF_NOSYNC   0x40000000    /* Use inband active signaling */
+
+/* Bits below C1_444ALPHA are not present in Cafe */
+#define REG_CTRL1       0x40    /* Control 1 */
+#define   C1_CLKGATE      0x00000001    /* Sensor clock gate */
+#define   C1_DESC_ENA     0x00000100    /* DMA descriptor enable */
+#define   C1_DESC_3WORD   0x00000200    /* Three-word descriptors used */
+#define   C1_444ALPHA     0x00f00000    /* Alpha field in RGB444 */
+#define   C1_ALPHA_SHFT   20
+#define   C1_DMAB32       0x00000000    /* 32-byte DMA burst */
+#define   C1_DMAB16       0x02000000    /* 16-byte DMA burst */
+#define   C1_DMAB64       0x04000000    /* 64-byte DMA burst */
+#define   C1_DMAB_MASK    0x06000000
+#define   C1_TWOBUFS      0x08000000    /* Use only two DMA buffers */
+#define   C1_PWRDWN       0x10000000    /* Power down */
+
+#define REG_CLKCTRL     0x88    /* Clock control */
+#define   CLK_DIV_MASK    0x0000ffff    /* Upper bits RW "reserved" */
+
+/* This appears to be a Cafe-only register */
+#define REG_UBAR        0xc4    /* Upper base address register */
+
+/* Armada 610 DMA descriptor registers */
+#define REG_DMA_DESC_Y  0x200
+#define REG_DMA_DESC_U  0x204
+#define REG_DMA_DESC_V  0x208
+#define REG_DESC_LEN_Y  0x20c   /* Lengths are in bytes */
+#define REG_DESC_LEN_U  0x210
+#define REG_DESC_LEN_V  0x214
+
+/*
+ * Useful stuff that probably belongs somewhere global.
+ */
+#define VGA_WIDTH       640
+#define VGA_HEIGHT      480
+
+#endif /* _MCAM_CORE_H */
diff --git a/drivers/media/video/marvell-ccic/mmp-driver.c b/drivers/media/video/marvell-ccic/mmp-driver.c
new file mode 100644
index 00000000000..d6b76454137
--- /dev/null
+++ b/drivers/media/video/marvell-ccic/mmp-driver.c
@@ -0,0 +1,340 @@
+/*
+ * Support for the camera device found on Marvell MMP processors; known
+ * to work with the Armada 610 as used in the OLPC 1.75 system.
+ *
+ * Copyright 2011 Jonathan Corbet <corbet@lwn.net>
+ *
+ * This file may be distributed under the terms of the GNU General
+ * Public License, version 2.
+ */
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/i2c-gpio.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/mmp-camera.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/gpio.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/list.h>
+
+#include "mcam-core.h"
+
+MODULE_AUTHOR("Jonathan Corbet <corbet@lwn.net>");
+MODULE_LICENSE("GPL");
+
+struct mmp_camera {
+        void *power_regs;
+        struct platform_device *pdev;
+        struct mcam_camera mcam;
+        struct list_head devlist;
+        int irq;
+};
+
+static inline struct mmp_camera *mcam_to_cam(struct mcam_camera *mcam)
+{
+        return container_of(mcam, struct mmp_camera, mcam);
+}
+
+/*
+ * A silly little infrastructure so we can keep track of our devices.
+ * Chances are that we will never have more than one of them, but
+ * the Armada 610 *does* have two controllers...
+ */
+
+static LIST_HEAD(mmpcam_devices);
+static struct mutex mmpcam_devices_lock;
+
+static void mmpcam_add_device(struct mmp_camera *cam)
+{
+        mutex_lock(&mmpcam_devices_lock);
+        list_add(&cam->devlist, &mmpcam_devices);
+        mutex_unlock(&mmpcam_devices_lock);
+}
+
+static void mmpcam_remove_device(struct mmp_camera *cam)
+{
+        mutex_lock(&mmpcam_devices_lock);
+        list_del(&cam->devlist);
+        mutex_unlock(&mmpcam_devices_lock);
+}
+
+/*
+ * Platform dev remove passes us a platform_device, and there's
+ * no handy unused drvdata to stash a backpointer in.  So just
+ * dig it out of our list.
+ */
+static struct mmp_camera *mmpcam_find_device(struct platform_device *pdev)
+{
+        struct mmp_camera *cam;
+
+        mutex_lock(&mmpcam_devices_lock);
+        list_for_each_entry(cam, &mmpcam_devices, devlist) {
+                if (cam->pdev == pdev) {
+                        mutex_unlock(&mmpcam_devices_lock);
+                        return cam;
+                }
+        }
+        mutex_unlock(&mmpcam_devices_lock);
+        return NULL;
+}
+
+
+
+
+/*
+ * Power-related registers; this almost certainly belongs
+ * somewhere else.
+ *
+ * ARMADA 610 register manual, sec 7.2.1, p1842.
+ */
+#define CPU_SUBSYS_PMU_BASE     0xd4282800
+#define REG_CCIC_DCGCR          0x28    /* CCIC dyn clock gate ctrl reg */
+#define REG_CCIC_CRCR           0x50    /* CCIC clk reset ctrl reg      */
+
+/*
+ * Power control.
+ */
+static void mmpcam_power_up(struct mcam_camera *mcam)
+{
+        struct mmp_camera *cam = mcam_to_cam(mcam);
+        struct mmp_camera_platform_data *pdata;
+/*
+ * Turn on power and clocks to the controller.
+ */
+        iowrite32(0x3f, cam->power_regs + REG_CCIC_DCGCR);
+        iowrite32(0x3805b, cam->power_regs + REG_CCIC_CRCR);
+        mdelay(1);
+/*
+ * Provide power to the sensor.
+ */
+        mcam_reg_write(mcam, REG_CLKCTRL, 0x60000002);
+        pdata = cam->pdev->dev.platform_data;
+        gpio_set_value(pdata->sensor_power_gpio, 1);
+        mdelay(5);
+        mcam_reg_clear_bit(mcam, REG_CTRL1, 0x10000000);
+        gpio_set_value(pdata->sensor_reset_gpio, 0); /* reset is active low */
+        mdelay(5);
+        gpio_set_value(pdata->sensor_reset_gpio, 1); /* reset is active low */
+        mdelay(5);
+}
+
+static void mmpcam_power_down(struct mcam_camera *mcam)
+{
+        struct mmp_camera *cam = mcam_to_cam(mcam);
+        struct mmp_camera_platform_data *pdata;
+/*
+ * Turn off clocks and set reset lines
+ */
+        iowrite32(0, cam->power_regs + REG_CCIC_DCGCR);
+        iowrite32(0, cam->power_regs + REG_CCIC_CRCR);
+/*
+ * Shut down the sensor.
+ */
+        pdata = cam->pdev->dev.platform_data;
+        gpio_set_value(pdata->sensor_power_gpio, 0);
+        gpio_set_value(pdata->sensor_reset_gpio, 0);
+}
+
+
+static irqreturn_t mmpcam_irq(int irq, void *data)
+{
+        struct mcam_camera *mcam = data;
+        unsigned int irqs, handled;
+
+        spin_lock(&mcam->dev_lock);
+        irqs = mcam_reg_read(mcam, REG_IRQSTAT);
+        handled = mccic_irq(mcam, irqs);
+        spin_unlock(&mcam->dev_lock);
+        return IRQ_RETVAL(handled);
+}
+
+
+static int mmpcam_probe(struct platform_device *pdev)
+{
+        struct mmp_camera *cam;
+        struct mcam_camera *mcam;
+        struct resource *res;
+        struct mmp_camera_platform_data *pdata;
+        int ret;
+
+        cam = kzalloc(sizeof(*cam), GFP_KERNEL);
+        if (cam == NULL)
+                return -ENOMEM;
+        cam->pdev = pdev;
+        INIT_LIST_HEAD(&cam->devlist);
+
+        mcam = &cam->mcam;
+        mcam->platform = MHP_Armada610;
+        mcam->plat_power_up = mmpcam_power_up;
+        mcam->plat_power_down = mmpcam_power_down;
+        mcam->dev = &pdev->dev;
+        mcam->use_smbus = 0;
+        mcam->chip_id = V4L2_IDENT_ARMADA610;
+        mcam->buffer_mode = B_DMA_sg;
+        spin_lock_init(&mcam->dev_lock);
+        /*
+         * Get our I/O memory.
+         */
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (res == NULL) {
+                dev_err(&pdev->dev, "no iomem resource!\n");
+                ret = -ENODEV;
+                goto out_free;
+        }
+        mcam->regs = ioremap(res->start, resource_size(res));
+        if (mcam->regs == NULL) {
+                dev_err(&pdev->dev, "MMIO ioremap fail\n");
+                ret = -ENODEV;
+                goto out_free;
+        }
+        /*
+         * Power/clock memory is elsewhere; get it too.  Perhaps this
+         * should really be managed outside of this driver?
+         */
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+        if (res == NULL) {
+                dev_err(&pdev->dev, "no power resource!\n");
+                ret = -ENODEV;
+                goto out_unmap1;
+        }
+        cam->power_regs = ioremap(res->start, resource_size(res));
+        if (cam->power_regs == NULL) {
+                dev_err(&pdev->dev, "power MMIO ioremap fail\n");
+                ret = -ENODEV;
+                goto out_unmap1;
+        }
+        /*
+         * Find the i2c adapter.  This assumes, of course, that the
+         * i2c bus is already up and functioning.
+         */
+        pdata = pdev->dev.platform_data;
+        mcam->i2c_adapter = platform_get_drvdata(pdata->i2c_device);
+        if (mcam->i2c_adapter == NULL) {
+                ret = -ENODEV;
+                dev_err(&pdev->dev, "No i2c adapter\n");
+                goto out_unmap2;
+        }
+        /*
+         * Sensor GPIO pins.
+         */
+        ret = gpio_request(pdata->sensor_power_gpio, "cam-power");
+        if (ret) {
+                dev_err(&pdev->dev, "Can't get sensor power gpio %d",
+                                pdata->sensor_power_gpio);
+                goto out_unmap2;
+        }
+        gpio_direction_output(pdata->sensor_power_gpio, 0);
+        ret = gpio_request(pdata->sensor_reset_gpio, "cam-reset");
+        if (ret) {
+                dev_err(&pdev->dev, "Can't get sensor reset gpio %d",
+                                pdata->sensor_reset_gpio);
+                goto out_gpio;
+        }
+        gpio_direction_output(pdata->sensor_reset_gpio, 0);
+        /*
+         * Power the device up and hand it off to the core.
+         */
+        mmpcam_power_up(mcam);
+        ret = mccic_register(mcam);
+        if (ret)
+                goto out_gpio2;
+        /*
+         * Finally, set up our IRQ now that the core is ready to
+         * deal with it.
+         */
+        res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+        if (res == NULL) {
+                ret = -ENODEV;
+                goto out_unregister;
+        }
+        cam->irq = res->start;
+        ret = request_irq(cam->irq, mmpcam_irq, IRQF_SHARED,
+                        "mmp-camera", mcam);
+        if (ret == 0) {
+                mmpcam_add_device(cam);
+                return 0;
+        }
+
+out_unregister:
+        mccic_shutdown(mcam);
+out_gpio2:
+        mmpcam_power_down(mcam);
+        gpio_free(pdata->sensor_reset_gpio);
+out_gpio:
+        gpio_free(pdata->sensor_power_gpio);
+out_unmap2:
+        iounmap(cam->power_regs);
+out_unmap1:
+        iounmap(mcam->regs);
+out_free:
+        kfree(cam);
+        return ret;
+}
+
+
+static int mmpcam_remove(struct mmp_camera *cam)
+{
+        struct mcam_camera *mcam = &cam->mcam;
+        struct mmp_camera_platform_data *pdata;
+
+        mmpcam_remove_device(cam);
+        free_irq(cam->irq, mcam);
+        mccic_shutdown(mcam);
+        mmpcam_power_down(mcam);
+        pdata = cam->pdev->dev.platform_data;
+        gpio_free(pdata->sensor_reset_gpio);
+        gpio_free(pdata->sensor_power_gpio);
+        iounmap(cam->power_regs);
+        iounmap(mcam->regs);
+        kfree(cam);
+        return 0;
+}
+
+static int mmpcam_platform_remove(struct platform_device *pdev)
+{
+        struct mmp_camera *cam = mmpcam_find_device(pdev);
+
+        if (cam == NULL)
+                return -ENODEV;
+        return mmpcam_remove(cam);
+}
+
+
+static struct platform_driver mmpcam_driver = {
+        .probe          = mmpcam_probe,
+        .remove         = mmpcam_platform_remove,
+        .driver = {
+                .name   = "mmp-camera",
+                .owner  = THIS_MODULE
+        }
+};
+
+
+static int __init mmpcam_init_module(void)
+{
+        mutex_init(&mmpcam_devices_lock);
+        return platform_driver_register(&mmpcam_driver);
+}
+
+static void __exit mmpcam_exit_module(void)
+{
+        platform_driver_unregister(&mmpcam_driver);
+        /*
+         * platform_driver_unregister() should have emptied the list
+         */
+        if (!list_empty(&mmpcam_devices))
+                printk(KERN_ERR "mmp_camera leaving devices behind\n");
+}
+
+module_init(mmpcam_init_module);
+module_exit(mmpcam_exit_module);
diff --git a/drivers/media/video/mem2mem_testdev.c b/drivers/media/video/mem2mem_testdev.c
index b03d74e09a3..166bf9349c1 100644
--- a/drivers/media/video/mem2mem_testdev.c
+++ b/drivers/media/video/mem2mem_testdev.c
@@ -19,7 +19,6 @@
 #include <linux/module.h>
 #include <linux/delay.h>
 #include <linux/fs.h>
-#include <linux/version.h>
 #include <linux/timer.h>
 #include <linux/sched.h>
 #include <linux/slab.h>
@@ -35,7 +34,7 @@
 MODULE_DESCRIPTION("Virtual device for mem2mem framework testing");
 MODULE_AUTHOR("Pawel Osciak, <pawel@osciak.com>");
 MODULE_LICENSE("GPL");
-
+MODULE_VERSION("0.1.1");
 
 #define MIN_W 32
 #define MIN_H 32
@@ -380,7 +379,6 @@ static int vidioc_querycap(struct file *file, void *priv,
         strncpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver) - 1);
         strncpy(cap->card, MEM2MEM_NAME, sizeof(cap->card) - 1);
         cap->bus_info[0] = 0;
-        cap->version = KERNEL_VERSION(0, 1, 0);
         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT
                           | V4L2_CAP_STREAMING;
 
diff --git a/drivers/media/video/mt9m001.c b/drivers/media/video/mt9m001.c
index e2bbd8c35c9..4da9cca939c 100644
--- a/drivers/media/video/mt9m001.c
+++ b/drivers/media/video/mt9m001.c
@@ -603,13 +603,9 @@ static int mt9m001_video_probe(struct soc_camera_device *icd,
         unsigned long flags;
         int ret;
 
-        /*
-         * We must have a parent by now. And it cannot be a wrong one.
-         * So this entire test is completely redundant.
-         */
-        if (!icd->dev.parent ||
-            to_soc_camera_host(icd->dev.parent)->nr != icd->iface)
-                return -ENODEV;
+        /* We must have a parent by now. And it cannot be a wrong one. */
+        BUG_ON(!icd->parent ||
+               to_soc_camera_host(icd->parent)->nr != icd->iface);
 
         /* Enable the chip */
         data = reg_write(client, MT9M001_CHIP_ENABLE, 1);
@@ -675,8 +671,8 @@ static void mt9m001_video_remove(struct soc_camera_device *icd)
 {
         struct soc_camera_link *icl = to_soc_camera_link(icd);
 
-        dev_dbg(&icd->dev, "Video removed: %p, %p\n",
-                icd->dev.parent, icd->vdev);
+        dev_dbg(icd->pdev, "Video removed: %p, %p\n",
+                icd->parent, icd->vdev);
         if (icl->free_bus)
                 icl->free_bus(icl);
 }
diff --git a/drivers/media/video/mt9m111.c b/drivers/media/video/mt9m111.c
index ebebed92962..a357aa889fc 100644
--- a/drivers/media/video/mt9m111.c
+++ b/drivers/media/video/mt9m111.c
@@ -63,6 +63,12 @@
 #define MT9M111_RESET_RESTART_FRAME     (1 << 1)
 #define MT9M111_RESET_RESET_MODE        (1 << 0)
 
+#define MT9M111_RM_FULL_POWER_RD        (0 << 10)
+#define MT9M111_RM_LOW_POWER_RD         (1 << 10)
+#define MT9M111_RM_COL_SKIP_4X          (1 << 5)
+#define MT9M111_RM_ROW_SKIP_4X          (1 << 4)
+#define MT9M111_RM_COL_SKIP_2X          (1 << 3)
+#define MT9M111_RM_ROW_SKIP_2X          (1 << 2)
 #define MT9M111_RMB_MIRROR_COLS         (1 << 1)
 #define MT9M111_RMB_MIRROR_ROWS         (1 << 0)
 #define MT9M111_CTXT_CTRL_RESTART       (1 << 15)
@@ -95,7 +101,8 @@
 
 #define MT9M111_OPMODE_AUTOEXPO_EN      (1 << 14)
 #define MT9M111_OPMODE_AUTOWHITEBAL_EN  (1 << 1)
-
+#define MT9M111_OUTFMT_FLIP_BAYER_COL   (1 << 9)
+#define MT9M111_OUTFMT_FLIP_BAYER_ROW   (1 << 8)
 #define MT9M111_OUTFMT_PROCESSED_BAYER  (1 << 14)
 #define MT9M111_OUTFMT_BYPASS_IFP       (1 << 10)
 #define MT9M111_OUTFMT_INV_PIX_CLOCK    (1 << 9)
@@ -110,9 +117,8 @@
 #define MT9M111_OUTFMT_TST_RAMP_FRAME   (3 << 4)
 #define MT9M111_OUTFMT_SHIFT_3_UP       (1 << 3)
 #define MT9M111_OUTFMT_AVG_CHROMA       (1 << 2)
-#define MT9M111_OUTFMT_SWAP_YCbCr_C_Y   (1 << 1)
-#define MT9M111_OUTFMT_SWAP_RGB_EVEN    (1 << 1)
-#define MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr (1 << 0)
+#define MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN  (1 << 1)
+#define MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B (1 << 0)
 
 /*
  * Camera control register addresses (0x200..0x2ff not implemented)
@@ -122,6 +128,8 @@
 #define reg_write(reg, val) mt9m111_reg_write(client, MT9M111_##reg, (val))
 #define reg_set(reg, val) mt9m111_reg_set(client, MT9M111_##reg, (val))
 #define reg_clear(reg, val) mt9m111_reg_clear(client, MT9M111_##reg, (val))
+#define reg_mask(reg, val, mask) mt9m111_reg_mask(client, MT9M111_##reg, \
+                (val), (mask))
 
 #define MT9M111_MIN_DARK_ROWS   8
 #define MT9M111_MIN_DARK_COLS   26
@@ -153,7 +161,11 @@ static const struct mt9m111_datafmt mt9m111_colour_fmts[] = {
         {V4L2_MBUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_JPEG},
         {V4L2_MBUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_JPEG},
         {V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
+        {V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE, V4L2_COLORSPACE_SRGB},
         {V4L2_MBUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB},
+        {V4L2_MBUS_FMT_RGB565_2X8_BE, V4L2_COLORSPACE_SRGB},
+        {V4L2_MBUS_FMT_BGR565_2X8_LE, V4L2_COLORSPACE_SRGB},
+        {V4L2_MBUS_FMT_BGR565_2X8_BE, V4L2_COLORSPACE_SRGB},
         {V4L2_MBUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB},
         {V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
 };
@@ -169,6 +181,8 @@ struct mt9m111 {
                          * from v4l2-chip-ident.h */
         enum mt9m111_context context;
         struct v4l2_rect rect;
+        struct mutex power_lock; /* lock to protect power_count */
+        int power_count;
         const struct mt9m111_datafmt *fmt;
         unsigned int gain;
         unsigned char autoexposure;
@@ -176,10 +190,6 @@ struct mt9m111 {
         unsigned int powered:1;
         unsigned int hflip:1;
         unsigned int vflip:1;
-        unsigned int swap_rgb_even_odd:1;
-        unsigned int swap_rgb_red_blue:1;
-        unsigned int swap_yuv_y_chromas:1;
-        unsigned int swap_yuv_cb_cr:1;
         unsigned int autowhitebalance:1;
 };
 
@@ -248,12 +258,26 @@ static int mt9m111_reg_clear(struct i2c_client *client, const u16 reg,
         int ret;
 
         ret = mt9m111_reg_read(client, reg);
-        return mt9m111_reg_write(client, reg, ret & ~data);
+        if (ret >= 0)
+                ret = mt9m111_reg_write(client, reg, ret & ~data);
+        return ret;
 }
 
-static int mt9m111_set_context(struct i2c_client *client,
+static int mt9m111_reg_mask(struct i2c_client *client, const u16 reg,
+                            const u16 data, const u16 mask)
+{
+        int ret;
+
+        ret = mt9m111_reg_read(client, reg);
+        if (ret >= 0)
+                ret = mt9m111_reg_write(client, reg, (ret & ~mask) | data);
+        return ret;
+}
+
+static int mt9m111_set_context(struct mt9m111 *mt9m111,
                                enum mt9m111_context ctxt)
 {
+        struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
         int valB = MT9M111_CTXT_CTRL_RESTART | MT9M111_CTXT_CTRL_DEFECTCOR_B
                 | MT9M111_CTXT_CTRL_RESIZE_B | MT9M111_CTXT_CTRL_CTRL2_B
                 | MT9M111_CTXT_CTRL_GAMMA_B | MT9M111_CTXT_CTRL_READ_MODE_B
@@ -267,10 +291,10 @@ static int mt9m111_set_context(struct i2c_client *client,
                 return reg_write(CONTEXT_CONTROL, valA);
 }
 
-static int mt9m111_setup_rect(struct i2c_client *client,
+static int mt9m111_setup_rect(struct mt9m111 *mt9m111,
                               struct v4l2_rect *rect)
 {
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
+        struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
         int ret, is_raw_format;
         int width = rect->width;
         int height = rect->height;
@@ -312,81 +336,9 @@ static int mt9m111_setup_rect(struct i2c_client *client,
         return ret;
 }
 
-static int mt9m111_setup_pixfmt(struct i2c_client *client, u16 outfmt)
+static int mt9m111_enable(struct mt9m111 *mt9m111)
 {
-        int ret;
-        u16 mask = MT9M111_OUTFMT_PROCESSED_BAYER | MT9M111_OUTFMT_RGB |
-                MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_SWAP_RGB_EVEN |
-                MT9M111_OUTFMT_RGB565 | MT9M111_OUTFMT_RGB555 |
-                MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr |
-                MT9M111_OUTFMT_SWAP_YCbCr_C_Y;
-
-        ret = reg_read(OUTPUT_FORMAT_CTRL2_A);
-        if (ret >= 0)
-                ret = reg_write(OUTPUT_FORMAT_CTRL2_A, (ret & ~mask) | outfmt);
-        if (!ret)
-                ret = reg_read(OUTPUT_FORMAT_CTRL2_B);
-        if (ret >= 0)
-                ret = reg_write(OUTPUT_FORMAT_CTRL2_B, (ret & ~mask) | outfmt);
-
-        return ret;
-}
-
-static int mt9m111_setfmt_bayer8(struct i2c_client *client)
-{
-        return mt9m111_setup_pixfmt(client, MT9M111_OUTFMT_PROCESSED_BAYER |
-                                    MT9M111_OUTFMT_RGB);
-}
-
-static int mt9m111_setfmt_bayer10(struct i2c_client *client)
-{
-        return mt9m111_setup_pixfmt(client, MT9M111_OUTFMT_BYPASS_IFP);
-}
-
-static int mt9m111_setfmt_rgb565(struct i2c_client *client)
-{
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
-        int val = 0;
-
-        if (mt9m111->swap_rgb_red_blue)
-                val |= MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr;
-        if (mt9m111->swap_rgb_even_odd)
-                val |= MT9M111_OUTFMT_SWAP_RGB_EVEN;
-        val |= MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565;
-
-        return mt9m111_setup_pixfmt(client, val);
-}
-
-static int mt9m111_setfmt_rgb555(struct i2c_client *client)
-{
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
-        int val = 0;
-
-        if (mt9m111->swap_rgb_red_blue)
-                val |= MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr;
-        if (mt9m111->swap_rgb_even_odd)
-                val |= MT9M111_OUTFMT_SWAP_RGB_EVEN;
-        val |= MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555;
-
-        return mt9m111_setup_pixfmt(client, val);
-}
-
-static int mt9m111_setfmt_yuv(struct i2c_client *client)
-{
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
-        int val = 0;
-
-        if (mt9m111->swap_yuv_cb_cr)
-                val |= MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr;
-        if (mt9m111->swap_yuv_y_chromas)
-                val |= MT9M111_OUTFMT_SWAP_YCbCr_C_Y;
-
-        return mt9m111_setup_pixfmt(client, val);
-}
-
-static int mt9m111_enable(struct i2c_client *client)
-{
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
+        struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
         int ret;
 
         ret = reg_set(RESET, MT9M111_RESET_CHIP_ENABLE);
@@ -395,8 +347,9 @@ static int mt9m111_enable(struct i2c_client *client)
         return ret;
 }
 
-static int mt9m111_reset(struct i2c_client *client)
+static int mt9m111_reset(struct mt9m111 *mt9m111)
 {
+        struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
         int ret;
 
         ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
@@ -424,11 +377,9 @@ static int mt9m111_set_bus_param(struct soc_camera_device *icd, unsigned long f)
         return 0;
 }
 
-static int mt9m111_make_rect(struct i2c_client *client,
+static int mt9m111_make_rect(struct mt9m111 *mt9m111,
                              struct v4l2_rect *rect)
 {
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
-
         if (mt9m111->fmt->code == V4L2_MBUS_FMT_SBGGR8_1X8 ||
             mt9m111->fmt->code == V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE) {
                 /* Bayer format - even size lengths */
@@ -444,14 +395,14 @@ static int mt9m111_make_rect(struct i2c_client *client,
         soc_camera_limit_side(&rect->top, &rect->height,
                      MT9M111_MIN_DARK_ROWS, 2, MT9M111_MAX_HEIGHT);
 
-        return mt9m111_setup_rect(client, rect);
+        return mt9m111_setup_rect(mt9m111, rect);
 }
 
 static int mt9m111_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
 {
         struct v4l2_rect rect = a->c;
         struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
+        struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
         int ret;
 
         dev_dbg(&client->dev, "%s left=%d, top=%d, width=%d, height=%d\n",
@@ -460,7 +411,7 @@ static int mt9m111_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
         if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
                 return -EINVAL;
 
-        ret = mt9m111_make_rect(client, &rect);
+        ret = mt9m111_make_rect(mt9m111, &rect);
         if (!ret)
                 mt9m111->rect = rect;
         return ret;
@@ -468,8 +419,7 @@ static int mt9m111_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
 
 static int mt9m111_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
 {
-        struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
+        struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
 
         a->c    = mt9m111->rect;
         a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
@@ -496,8 +446,7 @@ static int mt9m111_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
 static int mt9m111_g_fmt(struct v4l2_subdev *sd,
                          struct v4l2_mbus_framefmt *mf)
 {
-        struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
+        struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
 
         mf->width       = mt9m111->rect.width;
         mf->height      = mt9m111->rect.height;
@@ -508,51 +457,73 @@ static int mt9m111_g_fmt(struct v4l2_subdev *sd,
         return 0;
 }
 
-static int mt9m111_set_pixfmt(struct i2c_client *client,
+static int mt9m111_set_pixfmt(struct mt9m111 *mt9m111,
                               enum v4l2_mbus_pixelcode code)
 {
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
+        struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+        u16 data_outfmt2, mask_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
+                MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB |
+                MT9M111_OUTFMT_RGB565 | MT9M111_OUTFMT_RGB555 |
+                MT9M111_OUTFMT_RGB444x | MT9M111_OUTFMT_RGBx444 |
+                MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
+                MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
         int ret;
 
         switch (code) {
         case V4L2_MBUS_FMT_SBGGR8_1X8:
-                ret = mt9m111_setfmt_bayer8(client);
+                data_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
+                        MT9M111_OUTFMT_RGB;
                 break;
         case V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE:
-                ret = mt9m111_setfmt_bayer10(client);
+                data_outfmt2 = MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB;
                 break;
         case V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE:
-                ret = mt9m111_setfmt_rgb555(client);
+                data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555 |
+                        MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
+                break;
+        case V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE:
+                data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555;
                 break;
         case V4L2_MBUS_FMT_RGB565_2X8_LE:
-                ret = mt9m111_setfmt_rgb565(client);
+                data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
+                        MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
+                break;
+        case V4L2_MBUS_FMT_RGB565_2X8_BE:
+                data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565;
+                break;
+        case V4L2_MBUS_FMT_BGR565_2X8_BE:
+                data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
+                        MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
+                break;
+        case V4L2_MBUS_FMT_BGR565_2X8_LE:
+                data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
+                        MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
+                        MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
                 break;
         case V4L2_MBUS_FMT_UYVY8_2X8:
-                mt9m111->swap_yuv_y_chromas = 0;
-                mt9m111->swap_yuv_cb_cr = 0;
-                ret = mt9m111_setfmt_yuv(client);
+                data_outfmt2 = 0;
                 break;
         case V4L2_MBUS_FMT_VYUY8_2X8:
-                mt9m111->swap_yuv_y_chromas = 0;
-                mt9m111->swap_yuv_cb_cr = 1;
-                ret = mt9m111_setfmt_yuv(client);
+                data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
                 break;
         case V4L2_MBUS_FMT_YUYV8_2X8:
-                mt9m111->swap_yuv_y_chromas = 1;
-                mt9m111->swap_yuv_cb_cr = 0;
-                ret = mt9m111_setfmt_yuv(client);
+                data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
                 break;
         case V4L2_MBUS_FMT_YVYU8_2X8:
-                mt9m111->swap_yuv_y_chromas = 1;
-                mt9m111->swap_yuv_cb_cr = 1;
-                ret = mt9m111_setfmt_yuv(client);
+                data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
+                        MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
                 break;
         default:
-                dev_err(&client->dev, "Pixel format not handled : %x\n",
-                        code);
-                ret = -EINVAL;
+                dev_err(&client->dev, "Pixel format not handled: %x\n", code);
+                return -EINVAL;
         }
 
+        ret = reg_mask(OUTPUT_FORMAT_CTRL2_A, data_outfmt2,
+                       mask_outfmt2);
+        if (!ret)
+                ret = reg_mask(OUTPUT_FORMAT_CTRL2_B, data_outfmt2,
+                               mask_outfmt2);
+
         return ret;
 }
 
@@ -561,7 +532,7 @@ static int mt9m111_s_fmt(struct v4l2_subdev *sd,
 {
         struct i2c_client *client = v4l2_get_subdevdata(sd);
         const struct mt9m111_datafmt *fmt;
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
+        struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
         struct v4l2_rect rect = {
                 .left   = mt9m111->rect.left,
                 .top    = mt9m111->rect.top,
@@ -579,9 +550,9 @@ static int mt9m111_s_fmt(struct v4l2_subdev *sd,
                 "%s code=%x left=%d, top=%d, width=%d, height=%d\n", __func__,
                 mf->code, rect.left, rect.top, rect.width, rect.height);
 
-        ret = mt9m111_make_rect(client, &rect);
+        ret = mt9m111_make_rect(mt9m111, &rect);
         if (!ret)
-                ret = mt9m111_set_pixfmt(client, mf->code);
+                ret = mt9m111_set_pixfmt(mt9m111, mf->code);
         if (!ret) {
                 mt9m111->rect   = rect;
                 mt9m111->fmt    = fmt;
@@ -594,8 +565,7 @@ static int mt9m111_s_fmt(struct v4l2_subdev *sd,
 static int mt9m111_try_fmt(struct v4l2_subdev *sd,
                            struct v4l2_mbus_framefmt *mf)
 {
-        struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
+        struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
         const struct mt9m111_datafmt *fmt;
         bool bayer = mf->code == V4L2_MBUS_FMT_SBGGR8_1X8 ||
                 mf->code == V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE;
@@ -635,7 +605,7 @@ static int mt9m111_g_chip_ident(struct v4l2_subdev *sd,
                                 struct v4l2_dbg_chip_ident *id)
 {
         struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
+        struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
 
         if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
                 return -EINVAL;
@@ -726,21 +696,16 @@ static const struct v4l2_queryctrl mt9m111_controls[] = {
         }
 };
 
-static int mt9m111_resume(struct soc_camera_device *icd);
-static int mt9m111_suspend(struct soc_camera_device *icd, pm_message_t state);
-
 static struct soc_camera_ops mt9m111_ops = {
-        .suspend                = mt9m111_suspend,
-        .resume                 = mt9m111_resume,
         .query_bus_param        = mt9m111_query_bus_param,
         .set_bus_param          = mt9m111_set_bus_param,
         .controls               = mt9m111_controls,
         .num_controls           = ARRAY_SIZE(mt9m111_controls),
 };
 
-static int mt9m111_set_flip(struct i2c_client *client, int flip, int mask)
+static int mt9m111_set_flip(struct mt9m111 *mt9m111, int flip, int mask)
 {
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
+        struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
         int ret;
 
         if (mt9m111->context == HIGHPOWER) {
@@ -758,8 +723,9 @@ static int mt9m111_set_flip(struct i2c_client *client, int flip, int mask)
         return ret;
 }
 
-static int mt9m111_get_global_gain(struct i2c_client *client)
+static int mt9m111_get_global_gain(struct mt9m111 *mt9m111)
 {
+        struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
         int data;
 
         data = reg_read(GLOBAL_GAIN);
@@ -769,9 +735,9 @@ static int mt9m111_get_global_gain(struct i2c_client *client)
         return data;
 }
 
-static int mt9m111_set_global_gain(struct i2c_client *client, int gain)
+static int mt9m111_set_global_gain(struct mt9m111 *mt9m111, int gain)
 {
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
+        struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
         u16 val;
 
         if (gain > 63 * 2 * 2)
@@ -788,9 +754,9 @@ static int mt9m111_set_global_gain(struct i2c_client *client, int gain)
         return reg_write(GLOBAL_GAIN, val);
 }
 
-static int mt9m111_set_autoexposure(struct i2c_client *client, int on)
+static int mt9m111_set_autoexposure(struct mt9m111 *mt9m111, int on)
 {
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
+        struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
         int ret;
 
         if (on)
@@ -804,9 +770,9 @@ static int mt9m111_set_autoexposure(struct i2c_client *client, int on)
         return ret;
 }
 
-static int mt9m111_set_autowhitebalance(struct i2c_client *client, int on)
+static int mt9m111_set_autowhitebalance(struct mt9m111 *mt9m111, int on)
 {
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
+        struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
         int ret;
 
         if (on)
@@ -823,7 +789,7 @@ static int mt9m111_set_autowhitebalance(struct i2c_client *client, int on)
 static int mt9m111_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
 {
         struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
+        struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
         int data;
 
         switch (ctrl->id) {
@@ -848,7 +814,7 @@ static int mt9m111_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
                 ctrl->value = !!(data & MT9M111_RMB_MIRROR_COLS);
                 break;
         case V4L2_CID_GAIN:
-                data = mt9m111_get_global_gain(client);
+                data = mt9m111_get_global_gain(mt9m111);
                 if (data < 0)
                         return data;
                 ctrl->value = data;
@@ -865,8 +831,7 @@ static int mt9m111_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
 
 static int mt9m111_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
 {
-        struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
+        struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
         const struct v4l2_queryctrl *qctrl;
         int ret;
 
@@ -877,22 +842,22 @@ static int mt9m111_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
         switch (ctrl->id) {
         case V4L2_CID_VFLIP:
                 mt9m111->vflip = ctrl->value;
-                ret = mt9m111_set_flip(client, ctrl->value,
+                ret = mt9m111_set_flip(mt9m111, ctrl->value,
                                         MT9M111_RMB_MIRROR_ROWS);
                 break;
         case V4L2_CID_HFLIP:
                 mt9m111->hflip = ctrl->value;
-                ret = mt9m111_set_flip(client, ctrl->value,
+                ret = mt9m111_set_flip(mt9m111, ctrl->value,
                                         MT9M111_RMB_MIRROR_COLS);
                 break;
         case V4L2_CID_GAIN:
-                ret = mt9m111_set_global_gain(client, ctrl->value);
+                ret = mt9m111_set_global_gain(mt9m111, ctrl->value);
                 break;
         case V4L2_CID_EXPOSURE_AUTO:
-                ret =  mt9m111_set_autoexposure(client, ctrl->value);
+                ret =  mt9m111_set_autoexposure(mt9m111, ctrl->value);
                 break;
         case V4L2_CID_AUTO_WHITE_BALANCE:
-                ret =  mt9m111_set_autowhitebalance(client, ctrl->value);
+                ret =  mt9m111_set_autowhitebalance(mt9m111, ctrl->value);
                 break;
         default:
                 ret = -EINVAL;
@@ -901,60 +866,52 @@ static int mt9m111_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
         return ret;
 }
 
-static int mt9m111_suspend(struct soc_camera_device *icd, pm_message_t state)
+static int mt9m111_suspend(struct mt9m111 *mt9m111)
 {
-        struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
-
-        mt9m111->gain = mt9m111_get_global_gain(client);
+        mt9m111->gain = mt9m111_get_global_gain(mt9m111);
 
         return 0;
 }
 
-static int mt9m111_restore_state(struct i2c_client *client)
+static void mt9m111_restore_state(struct mt9m111 *mt9m111)
 {
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
-
-        mt9m111_set_context(client, mt9m111->context);
-        mt9m111_set_pixfmt(client, mt9m111->fmt->code);
-        mt9m111_setup_rect(client, &mt9m111->rect);
-        mt9m111_set_flip(client, mt9m111->hflip, MT9M111_RMB_MIRROR_COLS);
-        mt9m111_set_flip(client, mt9m111->vflip, MT9M111_RMB_MIRROR_ROWS);
-        mt9m111_set_global_gain(client, mt9m111->gain);
-        mt9m111_set_autoexposure(client, mt9m111->autoexposure);
-        mt9m111_set_autowhitebalance(client, mt9m111->autowhitebalance);
-        return 0;
+        mt9m111_set_context(mt9m111, mt9m111->context);
+        mt9m111_set_pixfmt(mt9m111, mt9m111->fmt->code);
+        mt9m111_setup_rect(mt9m111, &mt9m111->rect);
+        mt9m111_set_flip(mt9m111, mt9m111->hflip, MT9M111_RMB_MIRROR_COLS);
+        mt9m111_set_flip(mt9m111, mt9m111->vflip, MT9M111_RMB_MIRROR_ROWS);
+        mt9m111_set_global_gain(mt9m111, mt9m111->gain);
+        mt9m111_set_autoexposure(mt9m111, mt9m111->autoexposure);
+        mt9m111_set_autowhitebalance(mt9m111, mt9m111->autowhitebalance);
 }
 
-static int mt9m111_resume(struct soc_camera_device *icd)
+static int mt9m111_resume(struct mt9m111 *mt9m111)
 {
-        struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
         int ret = 0;
 
         if (mt9m111->powered) {
-                ret = mt9m111_enable(client);
+                ret = mt9m111_enable(mt9m111);
                 if (!ret)
-                        ret = mt9m111_reset(client);
+                        ret = mt9m111_reset(mt9m111);
                 if (!ret)
-                        ret = mt9m111_restore_state(client);
+                        mt9m111_restore_state(mt9m111);
         }
         return ret;
 }
 
-static int mt9m111_init(struct i2c_client *client)
+static int mt9m111_init(struct mt9m111 *mt9m111)
 {
-        struct mt9m111 *mt9m111 = to_mt9m111(client);
+        struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
         int ret;
 
         mt9m111->context = HIGHPOWER;
-        ret = mt9m111_enable(client);
+        ret = mt9m111_enable(mt9m111);
         if (!ret)
-                ret = mt9m111_reset(client);
+                ret = mt9m111_reset(mt9m111);
         if (!ret)
-                ret = mt9m111_set_context(client, mt9m111->context);
+                ret = mt9m111_set_context(mt9m111, mt9m111->context);
         if (!ret)
-                ret = mt9m111_set_autoexposure(client, mt9m111->autoexposure);
+                ret = mt9m111_set_autoexposure(mt9m111, mt9m111->autoexposure);
         if (ret)
                 dev_err(&client->dev, "mt9m111 init failed: %d\n", ret);
         return ret;
@@ -971,20 +928,13 @@ static int mt9m111_video_probe(struct soc_camera_device *icd,
         s32 data;
         int ret;
 
-        /*
-         * We must have a parent by now. And it cannot be a wrong one.
-         * So this entire test is completely redundant.
-         */
-        if (!icd->dev.parent ||
-            to_soc_camera_host(icd->dev.parent)->nr != icd->iface)
-                return -ENODEV;
+        /* We must have a parent by now. And it cannot be a wrong one. */
+        BUG_ON(!icd->parent ||
+               to_soc_camera_host(icd->parent)->nr != icd->iface);
 
         mt9m111->autoexposure = 1;
         mt9m111->autowhitebalance = 1;
 
-        mt9m111->swap_rgb_even_odd = 1;
-        mt9m111->swap_rgb_red_blue = 1;
-
         data = reg_read(CHIP_VERSION);
 
         switch (data) {
@@ -1005,16 +955,51 @@ static int mt9m111_video_probe(struct soc_camera_device *icd,
                 goto ei2c;
         }
 
-        ret = mt9m111_init(client);
+        ret = mt9m111_init(mt9m111);
 
 ei2c:
         return ret;
 }
 
+static int mt9m111_s_power(struct v4l2_subdev *sd, int on)
+{
+        struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        int ret = 0;
+
+        mutex_lock(&mt9m111->power_lock);
+
+        /*
+         * If the power count is modified from 0 to != 0 or from != 0 to 0,
+         * update the power state.
+         */
+        if (mt9m111->power_count == !on) {
+                if (on) {
+                        ret = mt9m111_resume(mt9m111);
+                        if (ret) {
+                                dev_err(&client->dev,
+                                        "Failed to resume the sensor: %d\n", ret);
+                                goto out;
+                        }
+                } else {
+                        mt9m111_suspend(mt9m111);
+                }
+        }
+
+        /* Update the power count. */
+        mt9m111->power_count += on ? 1 : -1;
+        WARN_ON(mt9m111->power_count < 0);
+
+out:
+        mutex_unlock(&mt9m111->power_lock);
+        return ret;
+}
+
 static struct v4l2_subdev_core_ops mt9m111_subdev_core_ops = {
         .g_ctrl         = mt9m111_g_ctrl,
         .s_ctrl         = mt9m111_s_ctrl,
         .g_chip_ident   = mt9m111_g_chip_ident,
+        .s_power        = mt9m111_s_power,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register     = mt9m111_g_register,
         .s_register     = mt9m111_s_register,
diff --git a/drivers/media/video/mt9t031.c b/drivers/media/video/mt9t031.c
index 7ce279c3751..30547cc3f89 100644
--- a/drivers/media/video/mt9t031.c
+++ b/drivers/media/video/mt9t031.c
@@ -700,8 +700,7 @@ static int mt9t031_runtime_suspend(struct device *dev)
 static int mt9t031_runtime_resume(struct device *dev)
 {
         struct video_device *vdev = to_video_device(dev);
-        struct soc_camera_device *icd = container_of(vdev->parent,
-                struct soc_camera_device, dev);
+        struct soc_camera_device *icd = dev_get_drvdata(vdev->parent);
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
         struct i2c_client *client = v4l2_get_subdevdata(sd);
         struct mt9t031 *mt9t031 = to_mt9t031(client);
diff --git a/drivers/media/video/mt9t112.c b/drivers/media/video/mt9t112.c
index bffa9ee1096..d2e0a50063a 100644
--- a/drivers/media/video/mt9t112.c
+++ b/drivers/media/video/mt9t112.c
@@ -1057,13 +1057,9 @@ static int mt9t112_camera_probe(struct soc_camera_device *icd,
         const char          *devname;
         int                  chipid;
 
-        /*
-         * We must have a parent by now. And it cannot be a wrong one.
-         * So this entire test is completely redundant.
-         */
-        if (!icd->dev.parent ||
-            to_soc_camera_host(icd->dev.parent)->nr != icd->iface)
-                return -ENODEV;
+        /* We must have a parent by now. And it cannot be a wrong one. */
+        BUG_ON(!icd->parent ||
+               to_soc_camera_host(icd->parent)->nr != icd->iface);
 
         /*
          * check and show chip ID
diff --git a/drivers/media/video/mt9v011.c b/drivers/media/video/mt9v011.c
index 4904d25f689..893a8b8f514 100644
--- a/drivers/media/video/mt9v011.c
+++ b/drivers/media/video/mt9v011.c
@@ -54,11 +54,20 @@ static struct v4l2_queryctrl mt9v011_qctrl[] = {
                 .type = V4L2_CTRL_TYPE_INTEGER,
                 .name = "Gain",
                 .minimum = 0,
-                .maximum = (1 << 10) - 1,
+                .maximum = (1 << 12) - 1 - 0x0020,
                 .step = 1,
                 .default_value = 0x0020,
                 .flags = 0,
         }, {
+                .id = V4L2_CID_EXPOSURE,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .name = "Exposure",
+                .minimum = 0,
+                .maximum = 2047,
+                .step = 1,
+                .default_value = 0x01fc,
+                .flags = 0,
+        }, {
                 .id = V4L2_CID_RED_BALANCE,
                 .type = V4L2_CTRL_TYPE_INTEGER,
                 .name = "Red Balance",
@@ -105,7 +114,8 @@ struct mt9v011 {
         unsigned hflip:1;
         unsigned vflip:1;
 
-        u16 global_gain, red_bal, blue_bal;
+        u16 global_gain, exposure;
+        s16 red_bal, blue_bal;
 };
 
 static inline struct mt9v011 *to_mt9v011(struct v4l2_subdev *sd)
@@ -180,24 +190,68 @@ static const struct i2c_reg_value mt9v011_init_default[] = {
                 { R07_MT9V011_OUT_CTRL, 0x0002 },       /* chip enable */
 };
 
+
+static u16 calc_mt9v011_gain(s16 lineargain)
+{
+
+        u16 digitalgain = 0;
+        u16 analogmult = 0;
+        u16 analoginit = 0;
+
+        if (lineargain < 0)
+                lineargain = 0;
+
+        /* recommended minimum */
+        lineargain += 0x0020;
+
+        if (lineargain > 2047)
+                lineargain = 2047;
+
+        if (lineargain > 1023) {
+                digitalgain = 3;
+                analogmult = 3;
+                analoginit = lineargain / 16;
+        } else if (lineargain > 511) {
+                digitalgain = 1;
+                analogmult = 3;
+                analoginit = lineargain / 8;
+        } else if (lineargain > 255) {
+                analogmult = 3;
+                analoginit = lineargain / 4;
+        } else if (lineargain > 127) {
+                analogmult = 1;
+                analoginit = lineargain / 2;
+        } else
+                analoginit = lineargain;
+
+        return analoginit + (analogmult << 7) + (digitalgain << 9);
+
+}
+
 static void set_balance(struct v4l2_subdev *sd)
 {
         struct mt9v011 *core = to_mt9v011(sd);
-        u16 green1_gain, green2_gain, blue_gain, red_gain;
+        u16 green_gain, blue_gain, red_gain;
+        u16 exposure;
+        s16 bal;
 
-        green1_gain = core->global_gain;
-        green2_gain = core->global_gain;
+        exposure = core->exposure;
 
-        blue_gain = core->global_gain +
-                    core->global_gain * core->blue_bal / (1 << 9);
+        green_gain = calc_mt9v011_gain(core->global_gain);
 
-        red_gain = core->global_gain +
-                   core->global_gain * core->blue_bal / (1 << 9);
+        bal = core->global_gain;
+        bal += (core->blue_bal * core->global_gain / (1 << 7));
+        blue_gain = calc_mt9v011_gain(bal);
 
-        mt9v011_write(sd, R2B_MT9V011_GREEN_1_GAIN, green1_gain);
-        mt9v011_write(sd, R2E_MT9V011_GREEN_2_GAIN,  green1_gain);
+        bal = core->global_gain;
+        bal += (core->red_bal * core->global_gain / (1 << 7));
+        red_gain = calc_mt9v011_gain(bal);
+
+        mt9v011_write(sd, R2B_MT9V011_GREEN_1_GAIN, green_gain);
+        mt9v011_write(sd, R2E_MT9V011_GREEN_2_GAIN, green_gain);
         mt9v011_write(sd, R2C_MT9V011_BLUE_GAIN, blue_gain);
         mt9v011_write(sd, R2D_MT9V011_RED_GAIN, red_gain);
+        mt9v011_write(sd, R09_MT9V011_SHUTTER_WIDTH, exposure);
 }
 
 static void calc_fps(struct v4l2_subdev *sd, u32 *numerator, u32 *denominator)
@@ -286,7 +340,7 @@ static void set_res(struct v4l2_subdev *sd)
          * be missing.
          */
 
-        hstart = 14 + (640 - core->width) / 2;
+        hstart = 20 + (640 - core->width) / 2;
         mt9v011_write(sd, R02_MT9V011_COLSTART, hstart);
         mt9v011_write(sd, R04_MT9V011_WIDTH, core->width);
         mt9v011_write(sd, R05_MT9V011_HBLANK, 771 - core->width);
@@ -338,6 +392,9 @@ static int mt9v011_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
         case V4L2_CID_GAIN:
                 ctrl->value = core->global_gain;
                 return 0;
+        case V4L2_CID_EXPOSURE:
+                ctrl->value = core->exposure;
+                return 0;
         case V4L2_CID_RED_BALANCE:
                 ctrl->value = core->red_bal;
                 return 0;
@@ -392,6 +449,9 @@ static int mt9v011_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
         case V4L2_CID_GAIN:
                 core->global_gain = ctrl->value;
                 break;
+        case V4L2_CID_EXPOSURE:
+                core->exposure = ctrl->value;
+                break;
         case V4L2_CID_RED_BALANCE:
                 core->red_bal = ctrl->value;
                 break;
@@ -598,6 +658,7 @@ static int mt9v011_probe(struct i2c_client *c,
         }
 
         core->global_gain = 0x0024;
+        core->exposure = 0x01fc;
         core->width  = 640;
         core->height = 480;
         core->xtal = 27000000;  /* Hz */
diff --git a/drivers/media/video/mt9v022.c b/drivers/media/video/mt9v022.c
index fc76ed1c08e..51b0fccbfe7 100644
--- a/drivers/media/video/mt9v022.c
+++ b/drivers/media/video/mt9v022.c
@@ -728,9 +728,9 @@ static int mt9v022_video_probe(struct soc_camera_device *icd,
         int ret;
         unsigned long flags;
 
-        if (!icd->dev.parent ||
-            to_soc_camera_host(icd->dev.parent)->nr != icd->iface)
-                return -ENODEV;
+        /* We must have a parent by now. And it cannot be a wrong one. */
+        BUG_ON(!icd->parent ||
+               to_soc_camera_host(icd->parent)->nr != icd->iface);
 
         /* Read out the chip version register */
         data = reg_read(client, MT9V022_CHIP_VERSION);
@@ -809,8 +809,8 @@ static void mt9v022_video_remove(struct soc_camera_device *icd)
 {
         struct soc_camera_link *icl = to_soc_camera_link(icd);
 
-        dev_dbg(&icd->dev, "Video removed: %p, %p\n",
-                icd->dev.parent, icd->vdev);
+        dev_dbg(icd->pdev, "Video removed: %p, %p\n",
+                icd->parent, icd->vdev);
         if (icl->free_bus)
                 icl->free_bus(icl);
 }
diff --git a/drivers/media/video/mt9v032.c b/drivers/media/video/mt9v032.c
index 1319c2c48af..c64e1dc4cb4 100644
--- a/drivers/media/video/mt9v032.c
+++ b/drivers/media/video/mt9v032.c
@@ -31,14 +31,14 @@
 #define MT9V032_CHIP_VERSION                            0x00
 #define         MT9V032_CHIP_ID_REV1                    0x1311
 #define         MT9V032_CHIP_ID_REV3                    0x1313
-#define MT9V032_ROW_START                               0x01
-#define         MT9V032_ROW_START_MIN                   4
-#define         MT9V032_ROW_START_DEF                   10
-#define         MT9V032_ROW_START_MAX                   482
-#define MT9V032_COLUMN_START                            0x02
+#define MT9V032_COLUMN_START                            0x01
 #define         MT9V032_COLUMN_START_MIN                1
-#define         MT9V032_COLUMN_START_DEF                2
+#define         MT9V032_COLUMN_START_DEF                1
 #define         MT9V032_COLUMN_START_MAX                752
+#define MT9V032_ROW_START                               0x02
+#define         MT9V032_ROW_START_MIN                   4
+#define         MT9V032_ROW_START_DEF                   5
+#define         MT9V032_ROW_START_MAX                   482
 #define MT9V032_WINDOW_HEIGHT                           0x03
 #define         MT9V032_WINDOW_HEIGHT_MIN               1
 #define         MT9V032_WINDOW_HEIGHT_DEF               480
@@ -420,13 +420,13 @@ static int mt9v032_set_crop(struct v4l2_subdev *subdev,
         struct v4l2_rect *__crop;
         struct v4l2_rect rect;
 
-        /* Clamp the crop rectangle boundaries and align them to a multiple of 2
-         * pixels.
+        /* Clamp the crop rectangle boundaries and align them to a non multiple
+         * of 2 pixels to ensure a GRBG Bayer pattern.
          */
-        rect.left = clamp(ALIGN(crop->rect.left, 2),
+        rect.left = clamp(ALIGN(crop->rect.left + 1, 2) - 1,
                           MT9V032_COLUMN_START_MIN,
                           MT9V032_COLUMN_START_MAX);
-        rect.top = clamp(ALIGN(crop->rect.top, 2),
+        rect.top = clamp(ALIGN(crop->rect.top + 1, 2) - 1,
                          MT9V032_ROW_START_MIN,
                          MT9V032_ROW_START_MAX);
         rect.width = clamp(ALIGN(crop->rect.width, 2),
diff --git a/drivers/media/video/mx1_camera.c b/drivers/media/video/mx1_camera.c
index 63f8a0cc33d..087db12a3a6 100644
--- a/drivers/media/video/mx1_camera.c
+++ b/drivers/media/video/mx1_camera.c
@@ -31,7 +31,6 @@
 #include <linux/sched.h>
 #include <linux/slab.h>
 #include <linux/time.h>
-#include <linux/version.h>
 #include <linux/videodev2.h>
 
 #include <media/soc_camera.h>
@@ -73,7 +72,7 @@
 #define CSISR_SOF_INT           (1 << 16)
 #define CSISR_DRDY              (1 << 0)
 
-#define VERSION_CODE KERNEL_VERSION(0, 0, 1)
+#define DRIVER_VERSION "0.0.2"
 #define DRIVER_NAME "mx1-camera"
 
 #define CSI_IRQ_MASK    (CSISR_SFF_OR_INT | CSISR_RFF_OR_INT | \
@@ -142,7 +141,7 @@ static int mx1_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
         if (*size * *count > MAX_VIDEO_MEM * 1024 * 1024)
                 *count = (MAX_VIDEO_MEM * 1024 * 1024) / *size;
 
-        dev_dbg(icd->dev.parent, "count=%d, size=%d\n", *count, *size);
+        dev_dbg(icd->parent, "count=%d, size=%d\n", *count, *size);
 
         return 0;
 }
@@ -154,7 +153,7 @@ static void free_buffer(struct videobuf_queue *vq, struct mx1_buffer *buf)
 
         BUG_ON(in_interrupt());
 
-        dev_dbg(icd->dev.parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
+        dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
                 vb, vb->baddr, vb->bsize);
 
         /*
@@ -179,7 +178,7 @@ static int mx1_videobuf_prepare(struct videobuf_queue *vq,
         if (bytes_per_line < 0)
                 return bytes_per_line;
 
-        dev_dbg(icd->dev.parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
+        dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
                 vb, vb->baddr, vb->bsize);
 
         /* Added list head initialization on alloc */
@@ -232,7 +231,7 @@ out:
 static int mx1_camera_setup_dma(struct mx1_camera_dev *pcdev)
 {
         struct videobuf_buffer *vbuf = &pcdev->active->vb;
-        struct device *dev = pcdev->icd->dev.parent;
+        struct device *dev = pcdev->icd->parent;
         int ret;
 
         if (unlikely(!pcdev->active)) {
@@ -256,11 +255,11 @@ static void mx1_videobuf_queue(struct videobuf_queue *vq,
                                                 struct videobuf_buffer *vb)
 {
         struct soc_camera_device *icd = vq->priv_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx1_camera_dev *pcdev = ici->priv;
         struct mx1_buffer *buf = container_of(vb, struct mx1_buffer, vb);
 
-        dev_dbg(icd->dev.parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
+        dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
                 vb, vb->baddr, vb->bsize);
 
         list_add_tail(&vb->queue, &pcdev->capture);
@@ -287,7 +286,7 @@ static void mx1_videobuf_release(struct videobuf_queue *vq,
         struct mx1_buffer *buf = container_of(vb, struct mx1_buffer, vb);
 #ifdef DEBUG
         struct soc_camera_device *icd = vq->priv_data;
-        struct device *dev = icd->dev.parent;
+        struct device *dev = icd->parent;
 
         dev_dbg(dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
                 vb, vb->baddr, vb->bsize);
@@ -343,7 +342,7 @@ static void mx1_camera_wakeup(struct mx1_camera_dev *pcdev,
 static void mx1_camera_dma_irq(int channel, void *data)
 {
         struct mx1_camera_dev *pcdev = data;
-        struct device *dev = pcdev->icd->dev.parent;
+        struct device *dev = pcdev->icd->parent;
         struct mx1_buffer *buf;
         struct videobuf_buffer *vb;
         unsigned long flags;
@@ -378,10 +377,10 @@ static struct videobuf_queue_ops mx1_videobuf_ops = {
 static void mx1_camera_init_videobuf(struct videobuf_queue *q,
                                      struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx1_camera_dev *pcdev = ici->priv;
 
-        videobuf_queue_dma_contig_init(q, &mx1_videobuf_ops, icd->dev.parent,
+        videobuf_queue_dma_contig_init(q, &mx1_videobuf_ops, icd->parent,
                                 &pcdev->lock, V4L2_BUF_TYPE_VIDEO_CAPTURE,
                                 V4L2_FIELD_NONE,
                                 sizeof(struct mx1_buffer), icd, &icd->video_lock);
@@ -401,7 +400,7 @@ static int mclk_get_divisor(struct mx1_camera_dev *pcdev)
          */
         div = (lcdclk + 2 * mclk - 1) / (2 * mclk) - 1;
 
-        dev_dbg(pcdev->icd->dev.parent,
+        dev_dbg(pcdev->icd->parent,
                 "System clock %lukHz, target freq %dkHz, divisor %lu\n",
                 lcdclk / 1000, mclk / 1000, div);
 
@@ -412,7 +411,7 @@ static void mx1_camera_activate(struct mx1_camera_dev *pcdev)
 {
         unsigned int csicr1 = CSICR1_EN;
 
-        dev_dbg(pcdev->icd->dev.parent, "Activate device\n");
+        dev_dbg(pcdev->icd->parent, "Activate device\n");
 
         clk_enable(pcdev->clk);
 
@@ -428,7 +427,7 @@ static void mx1_camera_activate(struct mx1_camera_dev *pcdev)
 
 static void mx1_camera_deactivate(struct mx1_camera_dev *pcdev)
 {
-        dev_dbg(pcdev->icd->dev.parent, "Deactivate device\n");
+        dev_dbg(pcdev->icd->parent, "Deactivate device\n");
 
         /* Disable all CSI interface */
         __raw_writel(0x00, pcdev->base + CSICR1);
@@ -442,13 +441,13 @@ static void mx1_camera_deactivate(struct mx1_camera_dev *pcdev)
  */
 static int mx1_camera_add_device(struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx1_camera_dev *pcdev = ici->priv;
 
         if (pcdev->icd)
                 return -EBUSY;
 
-        dev_info(icd->dev.parent, "MX1 Camera driver attached to camera %d\n",
+        dev_info(icd->parent, "MX1 Camera driver attached to camera %d\n",
                  icd->devnum);
 
         mx1_camera_activate(pcdev);
@@ -460,7 +459,7 @@ static int mx1_camera_add_device(struct soc_camera_device *icd)
 
 static void mx1_camera_remove_device(struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx1_camera_dev *pcdev = ici->priv;
         unsigned int csicr1;
 
@@ -473,7 +472,7 @@ static void mx1_camera_remove_device(struct soc_camera_device *icd)
         /* Stop DMA engine */
         imx_dma_disable(pcdev->dma_chan);
 
-        dev_info(icd->dev.parent, "MX1 Camera driver detached from camera %d\n",
+        dev_info(icd->parent, "MX1 Camera driver detached from camera %d\n",
                  icd->devnum);
 
         mx1_camera_deactivate(pcdev);
@@ -491,7 +490,7 @@ static int mx1_camera_set_crop(struct soc_camera_device *icd,
 
 static int mx1_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx1_camera_dev *pcdev = ici->priv;
         unsigned long camera_flags, common_flags;
         unsigned int csicr1;
@@ -562,14 +561,14 @@ static int mx1_camera_set_fmt(struct soc_camera_device *icd,
 
         xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
         if (!xlate) {
-                dev_warn(icd->dev.parent, "Format %x not found\n",
+                dev_warn(icd->parent, "Format %x not found\n",
                          pix->pixelformat);
                 return -EINVAL;
         }
 
         buswidth = xlate->host_fmt->bits_per_sample;
         if (buswidth > 8) {
-                dev_warn(icd->dev.parent,
+                dev_warn(icd->parent,
                          "bits-per-sample %d for format %x unsupported\n",
                          buswidth, pix->pixelformat);
                 return -EINVAL;
@@ -609,7 +608,7 @@ static int mx1_camera_try_fmt(struct soc_camera_device *icd,
 
         xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
         if (!xlate) {
-                dev_warn(icd->dev.parent, "Format %x not found\n",
+                dev_warn(icd->parent, "Format %x not found\n",
                          pix->pixelformat);
                 return -EINVAL;
         }
@@ -676,7 +675,6 @@ static int mx1_camera_querycap(struct soc_camera_host *ici,
 {
         /* cap->name is set by the friendly caller:-> */
         strlcpy(cap->card, "i.MX1/i.MXL Camera", sizeof(cap->card));
-        cap->version = VERSION_CODE;
         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 
         return 0;
@@ -883,4 +881,5 @@ module_exit(mx1_camera_exit);
 MODULE_DESCRIPTION("i.MX1/i.MXL SoC Camera Host driver");
 MODULE_AUTHOR("Paulius Zaleckas <paulius.zaleckas@teltonika.lt>");
 MODULE_LICENSE("GPL v2");
+MODULE_VERSION(DRIVER_VERSION);
 MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/media/video/mx2_camera.c b/drivers/media/video/mx2_camera.c
index 4eab1c62031..ec2410c0c80 100644
--- a/drivers/media/video/mx2_camera.c
+++ b/drivers/media/video/mx2_camera.c
@@ -23,7 +23,6 @@
 #include <linux/mm.h>
 #include <linux/moduleparam.h>
 #include <linux/time.h>
-#include <linux/version.h>
 #include <linux/device.h>
 #include <linux/platform_device.h>
 #include <linux/mutex.h>
@@ -47,7 +46,7 @@
 #include <asm/dma.h>
 
 #define MX2_CAM_DRV_NAME "mx2-camera"
-#define MX2_CAM_VERSION_CODE KERNEL_VERSION(0, 0, 5)
+#define MX2_CAM_VERSION "0.0.6"
 #define MX2_CAM_DRIVER_DESCRIPTION "i.MX2x_Camera"
 
 /* reset values */
@@ -278,7 +277,7 @@ static void mx2_camera_deactivate(struct mx2_camera_dev *pcdev)
  */
 static int mx2_camera_add_device(struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx2_camera_dev *pcdev = ici->priv;
         int ret;
         u32 csicr1;
@@ -303,7 +302,7 @@ static int mx2_camera_add_device(struct soc_camera_device *icd)
 
         pcdev->icd = icd;
 
-        dev_info(icd->dev.parent, "Camera driver attached to camera %d\n",
+        dev_info(icd->parent, "Camera driver attached to camera %d\n",
                  icd->devnum);
 
         return 0;
@@ -311,12 +310,12 @@ static int mx2_camera_add_device(struct soc_camera_device *icd)
 
 static void mx2_camera_remove_device(struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx2_camera_dev *pcdev = ici->priv;
 
         BUG_ON(icd != pcdev->icd);
 
-        dev_info(icd->dev.parent, "Camera driver detached from camera %d\n",
+        dev_info(icd->parent, "Camera driver detached from camera %d\n",
                  icd->devnum);
 
         mx2_camera_deactivate(pcdev);
@@ -437,7 +436,7 @@ static int mx2_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
                         icd->current_fmt->host_fmt);
 
-        dev_dbg(&icd->dev, "count=%d, size=%d\n", *count, *size);
+        dev_dbg(icd->parent, "count=%d, size=%d\n", *count, *size);
 
         if (bytes_per_line < 0)
                 return bytes_per_line;
@@ -457,7 +456,7 @@ static void free_buffer(struct videobuf_queue *vq, struct mx2_buffer *buf)
         struct soc_camera_device *icd = vq->priv_data;
         struct videobuf_buffer *vb = &buf->vb;
 
-        dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
+        dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
                 vb, vb->baddr, vb->bsize);
 
         /*
@@ -467,7 +466,7 @@ static void free_buffer(struct videobuf_queue *vq, struct mx2_buffer *buf)
         videobuf_waiton(vq, vb, 0, 0);
 
         videobuf_dma_contig_free(vq, vb);
-        dev_dbg(&icd->dev, "%s freed\n", __func__);
+        dev_dbg(icd->parent, "%s freed\n", __func__);
 
         vb->state = VIDEOBUF_NEEDS_INIT;
 }
@@ -481,7 +480,7 @@ static int mx2_videobuf_prepare(struct videobuf_queue *vq,
                         icd->current_fmt->host_fmt);
         int ret = 0;
 
-        dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
+        dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
                 vb, vb->baddr, vb->bsize);
 
         if (bytes_per_line < 0)
@@ -533,12 +532,12 @@ static void mx2_videobuf_queue(struct videobuf_queue *vq,
 {
         struct soc_camera_device *icd = vq->priv_data;
         struct soc_camera_host *ici =
-                to_soc_camera_host(icd->dev.parent);
+                to_soc_camera_host(icd->parent);
         struct mx2_camera_dev *pcdev = ici->priv;
         struct mx2_buffer *buf = container_of(vb, struct mx2_buffer, vb);
         unsigned long flags;
 
-        dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
+        dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
                 vb, vb->baddr, vb->bsize);
 
         spin_lock_irqsave(&pcdev->lock, flags);
@@ -611,27 +610,27 @@ static void mx2_videobuf_release(struct videobuf_queue *vq,
                                  struct videobuf_buffer *vb)
 {
         struct soc_camera_device *icd = vq->priv_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx2_camera_dev *pcdev = ici->priv;
         struct mx2_buffer *buf = container_of(vb, struct mx2_buffer, vb);
         unsigned long flags;
 
 #ifdef DEBUG
-        dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
+        dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
                 vb, vb->baddr, vb->bsize);
 
         switch (vb->state) {
         case VIDEOBUF_ACTIVE:
-                dev_info(&icd->dev, "%s (active)\n", __func__);
+                dev_info(icd->parent, "%s (active)\n", __func__);
                 break;
         case VIDEOBUF_QUEUED:
-                dev_info(&icd->dev, "%s (queued)\n", __func__);
+                dev_info(icd->parent, "%s (queued)\n", __func__);
                 break;
         case VIDEOBUF_PREPARED:
-                dev_info(&icd->dev, "%s (prepared)\n", __func__);
+                dev_info(icd->parent, "%s (prepared)\n", __func__);
                 break;
         default:
-                dev_info(&icd->dev, "%s (unknown) %d\n", __func__,
+                dev_info(icd->parent, "%s (unknown) %d\n", __func__,
                                 vb->state);
                 break;
         }
@@ -678,7 +677,7 @@ static struct videobuf_queue_ops mx2_videobuf_ops = {
 static void mx2_camera_init_videobuf(struct videobuf_queue *q,
                               struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx2_camera_dev *pcdev = ici->priv;
 
         videobuf_queue_dma_contig_init(q, &mx2_videobuf_ops, pcdev->dev,
@@ -719,7 +718,7 @@ static void mx27_camera_emma_buf_init(struct soc_camera_device *icd,
                 int bytesperline)
 {
         struct soc_camera_host *ici =
-                to_soc_camera_host(icd->dev.parent);
+                to_soc_camera_host(icd->parent);
         struct mx2_camera_dev *pcdev = ici->priv;
 
         writel(pcdev->discard_buffer_dma,
@@ -772,7 +771,7 @@ static int mx2_camera_set_bus_param(struct soc_camera_device *icd,
                 __u32 pixfmt)
 {
         struct soc_camera_host *ici =
-                to_soc_camera_host(icd->dev.parent);
+                to_soc_camera_host(icd->parent);
         struct mx2_camera_dev *pcdev = ici->priv;
         unsigned long camera_flags, common_flags;
         int ret = 0;
@@ -891,7 +890,7 @@ static int mx2_camera_set_crop(struct soc_camera_device *icd,
         if (ret < 0)
                 return ret;
 
-        dev_dbg(icd->dev.parent, "Sensor cropped %dx%d\n",
+        dev_dbg(icd->parent, "Sensor cropped %dx%d\n",
                 mf.width, mf.height);
 
         icd->user_width         = mf.width;
@@ -911,7 +910,7 @@ static int mx2_camera_set_fmt(struct soc_camera_device *icd,
 
         xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
         if (!xlate) {
-                dev_warn(icd->dev.parent, "Format %x not found\n",
+                dev_warn(icd->parent, "Format %x not found\n",
                                 pix->pixelformat);
                 return -EINVAL;
         }
@@ -951,7 +950,7 @@ static int mx2_camera_try_fmt(struct soc_camera_device *icd,
 
         xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
         if (pixfmt && !xlate) {
-                dev_warn(icd->dev.parent, "Format %x not found\n", pixfmt);
+                dev_warn(icd->parent, "Format %x not found\n", pixfmt);
                 return -EINVAL;
         }
 
@@ -974,11 +973,16 @@ static int mx2_camera_try_fmt(struct soc_camera_device *icd,
                 if (pix->bytesperline < 0)
                         return pix->bytesperline;
                 pix->sizeimage = pix->height * pix->bytesperline;
-                if (pix->sizeimage > (4 * 0x3ffff)) { /* CSIRXCNT limit */
-                        dev_warn(icd->dev.parent,
-                                        "Image size (%u) above limit\n",
-                                        pix->sizeimage);
-                        return -EINVAL;
+                /* Check against the CSIRXCNT limit */
+                if (pix->sizeimage > 4 * 0x3ffff) {
+                        /* Adjust geometry, preserve aspect ratio */
+                        unsigned int new_height = int_sqrt(4 * 0x3ffff *
+                                        pix->height / pix->bytesperline);
+                        pix->width = new_height * pix->width / pix->height;
+                        pix->height = new_height;
+                        pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
+                                                        xlate->host_fmt);
+                        BUG_ON(pix->bytesperline < 0);
                 }
         }
 
@@ -996,7 +1000,7 @@ static int mx2_camera_try_fmt(struct soc_camera_device *icd,
         if (mf.field == V4L2_FIELD_ANY)
                 mf.field = V4L2_FIELD_NONE;
         if (mf.field != V4L2_FIELD_NONE) {
-                dev_err(icd->dev.parent, "Field type %d unsupported.\n",
+                dev_err(icd->parent, "Field type %d unsupported.\n",
                                 mf.field);
                 return -EINVAL;
         }
@@ -1014,7 +1018,6 @@ static int mx2_camera_querycap(struct soc_camera_host *ici,
 {
         /* cap->name is set by the friendly caller:-> */
         strlcpy(cap->card, MX2_CAM_DRIVER_DESCRIPTION, sizeof(cap->card));
-        cap->version = MX2_CAM_VERSION_CODE;
         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 
         return 0;
@@ -1523,3 +1526,4 @@ module_exit(mx2_camera_exit);
 MODULE_DESCRIPTION("i.MX27/i.MX25 SoC Camera Host driver");
 MODULE_AUTHOR("Sascha Hauer <sha@pengutronix.de>");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(MX2_CAM_VERSION);
diff --git a/drivers/media/video/mx3_camera.c b/drivers/media/video/mx3_camera.c
index c7680eb8366..c045b47803a 100644
--- a/drivers/media/video/mx3_camera.c
+++ b/drivers/media/video/mx3_camera.c
@@ -11,7 +11,6 @@
 
 #include <linux/init.h>
 #include <linux/module.h>
-#include <linux/version.h>
 #include <linux/videodev2.h>
 #include <linux/platform_device.h>
 #include <linux/clk.h>
@@ -195,7 +194,7 @@ static int mx3_videobuf_setup(struct vb2_queue *vq,
                         unsigned long sizes[], void *alloc_ctxs[])
 {
         struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx3_camera_dev *mx3_cam = ici->priv;
         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
                                                 icd->current_fmt->host_fmt);
@@ -224,7 +223,7 @@ static int mx3_videobuf_setup(struct vb2_queue *vq,
 static int mx3_videobuf_prepare(struct vb2_buffer *vb)
 {
         struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx3_camera_dev *mx3_cam = ici->priv;
         struct idmac_channel *ichan = mx3_cam->idmac_channel[0];
         struct scatterlist *sg;
@@ -242,7 +241,7 @@ static int mx3_videobuf_prepare(struct vb2_buffer *vb)
         new_size = bytes_per_line * icd->user_height;
 
         if (vb2_plane_size(vb, 0) < new_size) {
-                dev_err(icd->dev.parent, "Buffer too small (%lu < %zu)\n",
+                dev_err(icd->parent, "Buffer too small (%lu < %zu)\n",
                         vb2_plane_size(vb, 0), new_size);
                 return -ENOBUFS;
         }
@@ -284,7 +283,7 @@ static enum pixel_fmt fourcc_to_ipu_pix(__u32 fourcc)
 static void mx3_videobuf_queue(struct vb2_buffer *vb)
 {
         struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx3_camera_dev *mx3_cam = ici->priv;
         struct mx3_camera_buffer *buf = to_mx3_vb(vb);
         struct dma_async_tx_descriptor *txd = buf->txd;
@@ -337,7 +336,7 @@ static void mx3_videobuf_queue(struct vb2_buffer *vb)
         spin_unlock_irq(&mx3_cam->lock);
 
         cookie = txd->tx_submit(txd);
-        dev_dbg(icd->dev.parent, "Submitted cookie %d DMA 0x%08x\n",
+        dev_dbg(icd->parent, "Submitted cookie %d DMA 0x%08x\n",
                 cookie, sg_dma_address(&buf->sg));
 
         if (cookie >= 0)
@@ -358,13 +357,13 @@ static void mx3_videobuf_queue(struct vb2_buffer *vb)
 static void mx3_videobuf_release(struct vb2_buffer *vb)
 {
         struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx3_camera_dev *mx3_cam = ici->priv;
         struct mx3_camera_buffer *buf = to_mx3_vb(vb);
         struct dma_async_tx_descriptor *txd = buf->txd;
         unsigned long flags;
 
-        dev_dbg(icd->dev.parent,
+        dev_dbg(icd->parent,
                 "Release%s DMA 0x%08x, queue %sempty\n",
                 mx3_cam->active == buf ? " active" : "", sg_dma_address(&buf->sg),
                 list_empty(&buf->queue) ? "" : "not ");
@@ -403,7 +402,7 @@ static int mx3_videobuf_init(struct vb2_buffer *vb)
 static int mx3_stop_streaming(struct vb2_queue *q)
 {
         struct soc_camera_device *icd = soc_camera_from_vb2q(q);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx3_camera_dev *mx3_cam = ici->priv;
         struct idmac_channel *ichan = mx3_cam->idmac_channel[0];
         struct dma_chan *chan;
@@ -499,7 +498,7 @@ static void mx3_camera_activate(struct mx3_camera_dev *mx3_cam,
 
         clk_enable(mx3_cam->clk);
         rate = clk_round_rate(mx3_cam->clk, mx3_cam->mclk);
-        dev_dbg(icd->dev.parent, "Set SENS_CONF to %x, rate %ld\n", conf, rate);
+        dev_dbg(icd->parent, "Set SENS_CONF to %x, rate %ld\n", conf, rate);
         if (rate)
                 clk_set_rate(mx3_cam->clk, rate);
 }
@@ -507,7 +506,7 @@ static void mx3_camera_activate(struct mx3_camera_dev *mx3_cam,
 /* Called with .video_lock held */
 static int mx3_camera_add_device(struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx3_camera_dev *mx3_cam = ici->priv;
 
         if (mx3_cam->icd)
@@ -517,7 +516,7 @@ static int mx3_camera_add_device(struct soc_camera_device *icd)
 
         mx3_cam->icd = icd;
 
-        dev_info(icd->dev.parent, "MX3 Camera driver attached to camera %d\n",
+        dev_info(icd->parent, "MX3 Camera driver attached to camera %d\n",
                  icd->devnum);
 
         return 0;
@@ -526,7 +525,7 @@ static int mx3_camera_add_device(struct soc_camera_device *icd)
 /* Called with .video_lock held */
 static void mx3_camera_remove_device(struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx3_camera_dev *mx3_cam = ici->priv;
         struct idmac_channel **ichan = &mx3_cam->idmac_channel[0];
 
@@ -541,7 +540,7 @@ static void mx3_camera_remove_device(struct soc_camera_device *icd)
 
         mx3_cam->icd = NULL;
 
-        dev_info(icd->dev.parent, "MX3 Camera driver detached from camera %d\n",
+        dev_info(icd->parent, "MX3 Camera driver detached from camera %d\n",
                  icd->devnum);
 }
 
@@ -608,12 +607,12 @@ static int test_platform_param(struct mx3_camera_dev *mx3_cam,
 static int mx3_camera_try_bus_param(struct soc_camera_device *icd,
                                     const unsigned int depth)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx3_camera_dev *mx3_cam = ici->priv;
         unsigned long bus_flags, camera_flags;
         int ret = test_platform_param(mx3_cam, depth, &bus_flags);
 
-        dev_dbg(icd->dev.parent, "request bus width %d bit: %d\n", depth, ret);
+        dev_dbg(icd->parent, "request bus width %d bit: %d\n", depth, ret);
 
         if (ret < 0)
                 return ret;
@@ -622,7 +621,7 @@ static int mx3_camera_try_bus_param(struct soc_camera_device *icd,
 
         ret = soc_camera_bus_param_compatible(camera_flags, bus_flags);
         if (ret < 0)
-                dev_warn(icd->dev.parent,
+                dev_warn(icd->parent,
                          "Flags incompatible: camera %lx, host %lx\n",
                          camera_flags, bus_flags);
 
@@ -676,7 +675,7 @@ static int mx3_camera_get_formats(struct soc_camera_device *icd, unsigned int id
                                   struct soc_camera_format_xlate *xlate)
 {
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        struct device *dev = icd->dev.parent;
+        struct device *dev = icd->parent;
         int formats = 0, ret;
         enum v4l2_mbus_pixelcode code;
         const struct soc_mbus_pixelfmt *fmt;
@@ -688,7 +687,7 @@ static int mx3_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 
         fmt = soc_mbus_get_fmtdesc(code);
         if (!fmt) {
-                dev_warn(icd->dev.parent,
+                dev_warn(icd->parent,
                          "Unsupported format code #%u: %d\n", idx, code);
                 return 0;
         }
@@ -816,7 +815,7 @@ static int mx3_camera_set_crop(struct soc_camera_device *icd,
                                struct v4l2_crop *a)
 {
         struct v4l2_rect *rect = &a->c;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx3_camera_dev *mx3_cam = ici->priv;
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
         struct v4l2_mbus_framefmt mf;
@@ -849,7 +848,7 @@ static int mx3_camera_set_crop(struct soc_camera_device *icd,
                 configure_geometry(mx3_cam, mf.width, mf.height,
                                    icd->current_fmt->host_fmt);
 
-        dev_dbg(icd->dev.parent, "Sensor cropped %dx%d\n",
+        dev_dbg(icd->parent, "Sensor cropped %dx%d\n",
                 mf.width, mf.height);
 
         icd->user_width         = mf.width;
@@ -861,7 +860,7 @@ static int mx3_camera_set_crop(struct soc_camera_device *icd,
 static int mx3_camera_set_fmt(struct soc_camera_device *icd,
                               struct v4l2_format *f)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx3_camera_dev *mx3_cam = ici->priv;
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
         const struct soc_camera_format_xlate *xlate;
@@ -871,13 +870,13 @@ static int mx3_camera_set_fmt(struct soc_camera_device *icd,
 
         xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
         if (!xlate) {
-                dev_warn(icd->dev.parent, "Format %x not found\n",
+                dev_warn(icd->parent, "Format %x not found\n",
                          pix->pixelformat);
                 return -EINVAL;
         }
 
         stride_align(&pix->width);
-        dev_dbg(icd->dev.parent, "Set format %dx%d\n", pix->width, pix->height);
+        dev_dbg(icd->parent, "Set format %dx%d\n", pix->width, pix->height);
 
         /*
          * Might have to perform a complete interface initialisation like in
@@ -913,13 +912,7 @@ static int mx3_camera_set_fmt(struct soc_camera_device *icd,
         pix->colorspace         = mf.colorspace;
         icd->current_fmt        = xlate;
 
-        pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
-                                                    xlate->host_fmt);
-        if (pix->bytesperline < 0)
-                return pix->bytesperline;
-        pix->sizeimage = pix->height * pix->bytesperline;
-
-        dev_dbg(icd->dev.parent, "Sensor set %dx%d\n", pix->width, pix->height);
+        dev_dbg(icd->parent, "Sensor set %dx%d\n", pix->width, pix->height);
 
         return ret;
 }
@@ -936,7 +929,7 @@ static int mx3_camera_try_fmt(struct soc_camera_device *icd,
 
         xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
         if (pixfmt && !xlate) {
-                dev_warn(icd->dev.parent, "Format %x not found\n", pixfmt);
+                dev_warn(icd->parent, "Format %x not found\n", pixfmt);
                 return -EINVAL;
         }
 
@@ -946,12 +939,6 @@ static int mx3_camera_try_fmt(struct soc_camera_device *icd,
         if (pix->width > 4096)
                 pix->width = 4096;
 
-        pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
-                                                    xlate->host_fmt);
-        if (pix->bytesperline < 0)
-                return pix->bytesperline;
-        pix->sizeimage = pix->height * pix->bytesperline;
-
         /* limit to sensor capabilities */
         mf.width        = pix->width;
         mf.height       = pix->height;
@@ -974,7 +961,7 @@ static int mx3_camera_try_fmt(struct soc_camera_device *icd,
         case V4L2_FIELD_NONE:
                 break;
         default:
-                dev_err(icd->dev.parent, "Field type %d unsupported.\n",
+                dev_err(icd->parent, "Field type %d unsupported.\n",
                         mf.field);
                 ret = -EINVAL;
         }
@@ -1000,7 +987,6 @@ static int mx3_camera_querycap(struct soc_camera_host *ici,
 {
         /* cap->name is set by the firendly caller:-> */
         strlcpy(cap->card, "i.MX3x Camera", sizeof(cap->card));
-        cap->version = KERNEL_VERSION(0, 2, 2);
         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 
         return 0;
@@ -1008,7 +994,7 @@ static int mx3_camera_querycap(struct soc_camera_host *ici,
 
 static int mx3_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx3_camera_dev *mx3_cam = ici->priv;
         unsigned long bus_flags, camera_flags, common_flags;
         u32 dw, sens_conf;
@@ -1016,7 +1002,7 @@ static int mx3_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
         int buswidth;
         int ret;
         const struct soc_camera_format_xlate *xlate;
-        struct device *dev = icd->dev.parent;
+        struct device *dev = icd->parent;
 
         fmt = soc_mbus_get_fmtdesc(icd->current_fmt->code);
         if (!fmt)
@@ -1325,4 +1311,5 @@ module_exit(mx3_camera_exit);
 MODULE_DESCRIPTION("i.MX3x SoC Camera Host driver");
 MODULE_AUTHOR("Guennadi Liakhovetski <lg@denx.de>");
 MODULE_LICENSE("GPL v2");
+MODULE_VERSION("0.2.3");
 MODULE_ALIAS("platform:" MX3_CAM_DRV_NAME);
diff --git a/drivers/media/video/omap/Kconfig b/drivers/media/video/omap/Kconfig
index e63233fd2aa..390ab094f9f 100644
--- a/drivers/media/video/omap/Kconfig
+++ b/drivers/media/video/omap/Kconfig
@@ -1,11 +1,14 @@
+config VIDEO_OMAP2_VOUT_VRFB
+        bool
+
 config VIDEO_OMAP2_VOUT
         tristate "OMAP2/OMAP3 V4L2-Display driver"
         depends on ARCH_OMAP2 || ARCH_OMAP3
         select VIDEOBUF_GEN
         select VIDEOBUF_DMA_CONTIG
         select OMAP2_DSS
-        select OMAP2_VRAM
-        select OMAP2_VRFB
+        select OMAP2_VRFB if ARCH_OMAP2 || ARCH_OMAP3
+        select VIDEO_OMAP2_VOUT_VRFB if VIDEO_OMAP2_VOUT && OMAP2_VRFB
         default n
         ---help---
           V4L2 Display driver support for OMAP2/3 based boards.
diff --git a/drivers/media/video/omap/Makefile b/drivers/media/video/omap/Makefile
index b28788070ae..fc410b438f7 100644
--- a/drivers/media/video/omap/Makefile
+++ b/drivers/media/video/omap/Makefile
@@ -4,4 +4,5 @@
 
 # OMAP2/3 Display driver
 omap-vout-y := omap_vout.o omap_voutlib.o
+omap-vout-$(CONFIG_VIDEO_OMAP2_VOUT_VRFB) += omap_vout_vrfb.o
 obj-$(CONFIG_VIDEO_OMAP2_VOUT) += omap-vout.o
diff --git a/drivers/media/video/omap/omap_vout.c b/drivers/media/video/omap/omap_vout.c
index a647894d3a7..b5ef3622244 100644
--- a/drivers/media/video/omap/omap_vout.c
+++ b/drivers/media/video/omap/omap_vout.c
@@ -35,28 +35,26 @@
 #include <linux/sched.h>
 #include <linux/types.h>
 #include <linux/platform_device.h>
-#include <linux/dma-mapping.h>
 #include <linux/irq.h>
 #include <linux/videodev2.h>
-#include <linux/slab.h>
+#include <linux/dma-mapping.h>
 
 #include <media/videobuf-dma-contig.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 
 #include <plat/dma.h>
-#include <plat/vram.h>
 #include <plat/vrfb.h>
 #include <video/omapdss.h>
 
 #include "omap_voutlib.h"
 #include "omap_voutdef.h"
+#include "omap_vout_vrfb.h"
 
 MODULE_AUTHOR("Texas Instruments");
 MODULE_DESCRIPTION("OMAP Video for Linux Video out driver");
 MODULE_LICENSE("GPL");
 
-
 /* Driver Configuration macros */
 #define VOUT_NAME               "omap_vout"
 
@@ -65,31 +63,6 @@ enum omap_vout_channels {
         OMAP_VIDEO2,
 };
 
-enum dma_channel_state {
-        DMA_CHAN_NOT_ALLOTED,
-        DMA_CHAN_ALLOTED,
-};
-
-#define QQVGA_WIDTH             160
-#define QQVGA_HEIGHT            120
-
-/* Max Resolution supported by the driver */
-#define VID_MAX_WIDTH           1280    /* Largest width */
-#define VID_MAX_HEIGHT          720     /* Largest height */
-
-/* Mimimum requirement is 2x2 for DSS */
-#define VID_MIN_WIDTH           2
-#define VID_MIN_HEIGHT          2
-
-/* 2048 x 2048 is max res supported by OMAP display controller */
-#define MAX_PIXELS_PER_LINE     2048
-
-#define VRFB_TX_TIMEOUT         1000
-#define VRFB_NUM_BUFS           4
-
-/* Max buffer size tobe allocated during init */
-#define OMAP_VOUT_MAX_BUF_SIZE (VID_MAX_WIDTH*VID_MAX_HEIGHT*4)
-
 static struct videobuf_queue_ops video_vbq_ops;
 /* Variables configurable through module params*/
 static u32 video1_numbuffers = 3;
@@ -172,84 +145,6 @@ static const struct v4l2_fmtdesc omap_formats[] = {
 #define NUM_OUTPUT_FORMATS (ARRAY_SIZE(omap_formats))
 
 /*
- * Allocate buffers
- */
-static unsigned long omap_vout_alloc_buffer(u32 buf_size, u32 *phys_addr)
-{
-        u32 order, size;
-        unsigned long virt_addr, addr;
-
-        size = PAGE_ALIGN(buf_size);
-        order = get_order(size);
-        virt_addr = __get_free_pages(GFP_KERNEL | GFP_DMA, order);
-        addr = virt_addr;
-
-        if (virt_addr) {
-                while (size > 0) {
-                        SetPageReserved(virt_to_page(addr));
-                        addr += PAGE_SIZE;
-                        size -= PAGE_SIZE;
-                }
-        }
-        *phys_addr = (u32) virt_to_phys((void *) virt_addr);
-        return virt_addr;
-}
-
-/*
- * Free buffers
- */
-static void omap_vout_free_buffer(unsigned long virtaddr, u32 buf_size)
-{
-        u32 order, size;
-        unsigned long addr = virtaddr;
-
-        size = PAGE_ALIGN(buf_size);
-        order = get_order(size);
-
-        while (size > 0) {
-                ClearPageReserved(virt_to_page(addr));
-                addr += PAGE_SIZE;
-                size -= PAGE_SIZE;
-        }
-        free_pages((unsigned long) virtaddr, order);
-}
-
-/*
- * Function for allocating video buffers
- */
-static int omap_vout_allocate_vrfb_buffers(struct omap_vout_device *vout,
-                unsigned int *count, int startindex)
-{
-        int i, j;
-
-        for (i = 0; i < *count; i++) {
-                if (!vout->smsshado_virt_addr[i]) {
-                        vout->smsshado_virt_addr[i] =
-                                omap_vout_alloc_buffer(vout->smsshado_size,
-                                                &vout->smsshado_phy_addr[i]);
-                }
-                if (!vout->smsshado_virt_addr[i] && startindex != -1) {
-                        if (V4L2_MEMORY_MMAP == vout->memory && i >= startindex)
-                                break;
-                }
-                if (!vout->smsshado_virt_addr[i]) {
-                        for (j = 0; j < i; j++) {
-                                omap_vout_free_buffer(
-                                                vout->smsshado_virt_addr[j],
-                                                vout->smsshado_size);
-                                vout->smsshado_virt_addr[j] = 0;
-                                vout->smsshado_phy_addr[j] = 0;
-                        }
-                        *count = 0;
-                        return -ENOMEM;
-                }
-                memset((void *) vout->smsshado_virt_addr[i], 0,
-                                vout->smsshado_size);
-        }
-        return 0;
-}
-
-/*
  * Try format
  */
 static int omap_vout_try_format(struct v4l2_pix_format *pix)
@@ -342,73 +237,9 @@ static u32 omap_vout_uservirt_to_phys(u32 virtp)
 }
 
 /*
- * Wakes up the application once the DMA transfer to VRFB space is completed.
- */
-static void omap_vout_vrfb_dma_tx_callback(int lch, u16 ch_status, void *data)
-{
-        struct vid_vrfb_dma *t = (struct vid_vrfb_dma *) data;
-
-        t->tx_status = 1;
-        wake_up_interruptible(&t->wait);
-}
-
-/*
- * Release the VRFB context once the module exits
- */
-static void omap_vout_release_vrfb(struct omap_vout_device *vout)
-{
-        int i;
-
-        for (i = 0; i < VRFB_NUM_BUFS; i++)
-                omap_vrfb_release_ctx(&vout->vrfb_context[i]);
-
-        if (vout->vrfb_dma_tx.req_status == DMA_CHAN_ALLOTED) {
-                vout->vrfb_dma_tx.req_status = DMA_CHAN_NOT_ALLOTED;
-                omap_free_dma(vout->vrfb_dma_tx.dma_ch);
-        }
-}
-
-/*
- * Return true if rotation is 90 or 270
- */
-static inline int rotate_90_or_270(const struct omap_vout_device *vout)
-{
-        return (vout->rotation == dss_rotation_90_degree ||
-                        vout->rotation == dss_rotation_270_degree);
-}
-
-/*
- * Return true if rotation is enabled
- */
-static inline int rotation_enabled(const struct omap_vout_device *vout)
-{
-        return vout->rotation || vout->mirror;
-}
-
-/*
- * Reverse the rotation degree if mirroring is enabled
- */
-static inline int calc_rotation(const struct omap_vout_device *vout)
-{
-        if (!vout->mirror)
-                return vout->rotation;
-
-        switch (vout->rotation) {
-        case dss_rotation_90_degree:
-                return dss_rotation_270_degree;
-        case dss_rotation_270_degree:
-                return dss_rotation_90_degree;
-        case dss_rotation_180_degree:
-                return dss_rotation_0_degree;
-        default:
-                return dss_rotation_180_degree;
-        }
-}
-
-/*
  * Free the V4L2 buffers
  */
-static void omap_vout_free_buffers(struct omap_vout_device *vout)
+void omap_vout_free_buffers(struct omap_vout_device *vout)
 {
         int i, numbuffers;
 
@@ -425,52 +256,6 @@ static void omap_vout_free_buffers(struct omap_vout_device *vout)
 }
 
 /*
- * Free VRFB buffers
- */
-static void omap_vout_free_vrfb_buffers(struct omap_vout_device *vout)
-{
-        int j;
-
-        for (j = 0; j < VRFB_NUM_BUFS; j++) {
-                omap_vout_free_buffer(vout->smsshado_virt_addr[j],
-                                vout->smsshado_size);
-                vout->smsshado_virt_addr[j] = 0;
-                vout->smsshado_phy_addr[j] = 0;
-        }
-}
-
-/*
- * Allocate the buffers for the VRFB space.  Data is copied from V4L2
- * buffers to the VRFB buffers using the DMA engine.
- */
-static int omap_vout_vrfb_buffer_setup(struct omap_vout_device *vout,
-                          unsigned int *count, unsigned int startindex)
-{
-        int i;
-        bool yuv_mode;
-
-        /* Allocate the VRFB buffers only if the buffers are not
-         * allocated during init time.
-         */
-        if ((rotation_enabled(vout)) && !vout->vrfb_static_allocation)
-                if (omap_vout_allocate_vrfb_buffers(vout, count, startindex))
-                        return -ENOMEM;
-
-        if (vout->dss_mode == OMAP_DSS_COLOR_YUV2 ||
-                        vout->dss_mode == OMAP_DSS_COLOR_UYVY)
-                yuv_mode = true;
-        else
-                yuv_mode = false;
-
-        for (i = 0; i < *count; i++)
-                omap_vrfb_setup(&vout->vrfb_context[i],
-                                vout->smsshado_phy_addr[i], vout->pix.width,
-                                vout->pix.height, vout->bpp, yuv_mode);
-
-        return 0;
-}
-
-/*
  * Convert V4L2 rotation to DSS rotation
  *      V4L2 understand 0, 90, 180, 270.
  *      Convert to 0, 1, 2 and 3 respectively for DSS
@@ -499,124 +284,38 @@ static int v4l2_rot_to_dss_rot(int v4l2_rotation,
         return ret;
 }
 
-/*
- * Calculate the buffer offsets from which the streaming should
- * start. This offset calculation is mainly required because of
- * the VRFB 32 pixels alignment with rotation.
- */
 static int omap_vout_calculate_offset(struct omap_vout_device *vout)
 {
-        struct omap_overlay *ovl;
-        enum dss_rotation rotation;
         struct omapvideo_info *ovid;
-        bool mirroring = vout->mirror;
-        struct omap_dss_device *cur_display;
         struct v4l2_rect *crop = &vout->crop;
         struct v4l2_pix_format *pix = &vout->pix;
         int *cropped_offset = &vout->cropped_offset;
-        int vr_ps = 1, ps = 2, temp_ps = 2;
-        int offset = 0, ctop = 0, cleft = 0, line_length = 0;
+        int ps = 2, line_length = 0;
 
         ovid = &vout->vid_info;
-        ovl = ovid->overlays[0];
-        /* get the display device attached to the overlay */
-        if (!ovl->manager || !ovl->manager->device)
-                return -1;
 
-        cur_display = ovl->manager->device;
-        rotation = calc_rotation(vout);
+        if (ovid->rotation_type == VOUT_ROT_VRFB) {
+                omap_vout_calculate_vrfb_offset(vout);
+        } else {
+                vout->line_length = line_length = pix->width;
 
-        if (V4L2_PIX_FMT_YUYV == pix->pixelformat ||
-                        V4L2_PIX_FMT_UYVY == pix->pixelformat) {
-                if (rotation_enabled(vout)) {
-                        /*
-                         * ps    - Actual pixel size for YUYV/UYVY for
-                         *         VRFB/Mirroring is 4 bytes
-                         * vr_ps - Virtually pixel size for YUYV/UYVY is
-                         *         2 bytes
-                         */
+                if (V4L2_PIX_FMT_YUYV == pix->pixelformat ||
+                        V4L2_PIX_FMT_UYVY == pix->pixelformat)
+                        ps = 2;
+                else if (V4L2_PIX_FMT_RGB32 == pix->pixelformat)
                         ps = 4;
-                        vr_ps = 2;
-                } else {
-                        ps = 2; /* otherwise the pixel size is 2 byte */
-                }
-        } else if (V4L2_PIX_FMT_RGB32 == pix->pixelformat) {
-                ps = 4;
-        } else if (V4L2_PIX_FMT_RGB24 == pix->pixelformat) {
-                ps = 3;
-        }
-        vout->ps = ps;
-        vout->vr_ps = vr_ps;
-
-        if (rotation_enabled(vout)) {
-                line_length = MAX_PIXELS_PER_LINE;
-                ctop = (pix->height - crop->height) - crop->top;
-                cleft = (pix->width - crop->width) - crop->left;
-        } else {
-                line_length = pix->width;
-        }
-        vout->line_length = line_length;
-        switch (rotation) {
-        case dss_rotation_90_degree:
-                offset = vout->vrfb_context[0].yoffset *
-                        vout->vrfb_context[0].bytespp;
-                temp_ps = ps / vr_ps;
-                if (mirroring == 0) {
-                        *cropped_offset = offset + line_length *
-                                temp_ps * cleft + crop->top * temp_ps;
-                } else {
-                        *cropped_offset = offset + line_length * temp_ps *
-                                cleft + crop->top * temp_ps + (line_length *
-                                ((crop->width / (vr_ps)) - 1) * ps);
-                }
-                break;
-        case dss_rotation_180_degree:
-                offset = ((MAX_PIXELS_PER_LINE * vout->vrfb_context[0].yoffset *
-                        vout->vrfb_context[0].bytespp) +
-                        (vout->vrfb_context[0].xoffset *
-                        vout->vrfb_context[0].bytespp));
-                if (mirroring == 0) {
-                        *cropped_offset = offset + (line_length * ps * ctop) +
-                                (cleft / vr_ps) * ps;
+                else if (V4L2_PIX_FMT_RGB24 == pix->pixelformat)
+                        ps = 3;
 
-                } else {
-                        *cropped_offset = offset + (line_length * ps * ctop) +
-                                (cleft / vr_ps) * ps + (line_length *
-                                (crop->height - 1) * ps);
-                }
-                break;
-        case dss_rotation_270_degree:
-                offset = MAX_PIXELS_PER_LINE * vout->vrfb_context[0].xoffset *
-                        vout->vrfb_context[0].bytespp;
-                temp_ps = ps / vr_ps;
-                if (mirroring == 0) {
-                        *cropped_offset = offset + line_length *
-                            temp_ps * crop->left + ctop * ps;
-                } else {
-                        *cropped_offset = offset + line_length *
-                                temp_ps * crop->left + ctop * ps +
-                                (line_length * ((crop->width / vr_ps) - 1) *
-                                 ps);
-                }
-                break;
-        case dss_rotation_0_degree:
-                if (mirroring == 0) {
-                        *cropped_offset = (line_length * ps) *
-                                crop->top + (crop->left / vr_ps) * ps;
-                } else {
-                        *cropped_offset = (line_length * ps) *
-                                crop->top + (crop->left / vr_ps) * ps +
-                                (line_length * (crop->height - 1) * ps);
-                }
-                break;
-        default:
-                *cropped_offset = (line_length * ps * crop->top) /
-                        vr_ps + (crop->left * ps) / vr_ps +
-                        ((crop->width / vr_ps) - 1) * ps;
-                break;
+                vout->ps = ps;
+
+                *cropped_offset = (line_length * ps) *
+                        crop->top + crop->left * ps;
         }
+
         v4l2_dbg(1, debug, &vout->vid_dev->v4l2_dev, "%s Offset:%x\n",
-                        __func__, *cropped_offset);
+                        __func__, vout->cropped_offset);
+
         return 0;
 }
 
@@ -664,7 +363,7 @@ static int video_mode_to_dss_mode(struct omap_vout_device *vout)
 /*
  * Setup the overlay
  */
-int omapvid_setup_overlay(struct omap_vout_device *vout,
+static int omapvid_setup_overlay(struct omap_vout_device *vout,
                 struct omap_overlay *ovl, int posx, int posy, int outw,
                 int outh, u32 addr)
 {
@@ -687,7 +386,7 @@ int omapvid_setup_overlay(struct omap_vout_device *vout,
         /* Setup the input plane parameters according to
          * rotation value selected.
          */
-        if (rotate_90_or_270(vout)) {
+        if (is_rotation_90_or_270(vout)) {
                 cropheight = vout->crop.width;
                 cropwidth = vout->crop.height;
                 pixheight = vout->pix.width;
@@ -711,7 +410,7 @@ int omapvid_setup_overlay(struct omap_vout_device *vout,
         info.out_width = outw;
         info.out_height = outh;
         info.global_alpha = vout->win.global_alpha;
-        if (!rotation_enabled(vout)) {
+        if (!is_rotation_enabled(vout)) {
                 info.rotation = 0;
                 info.rotation_type = OMAP_DSS_ROT_DMA;
                 info.screen_width = pixwidth;
@@ -744,7 +443,7 @@ setup_ovl_err:
 /*
  * Initialize the overlay structure
  */
-int omapvid_init(struct omap_vout_device *vout, u32 addr)
+static int omapvid_init(struct omap_vout_device *vout, u32 addr)
 {
         int ret = 0, i;
         struct v4l2_window *win;
@@ -809,7 +508,7 @@ omapvid_init_err:
 /*
  * Apply the changes set the go bit of DSS
  */
-int omapvid_apply_changes(struct omap_vout_device *vout)
+static int omapvid_apply_changes(struct omap_vout_device *vout)
 {
         int i;
         struct omap_overlay *ovl;
@@ -825,7 +524,7 @@ int omapvid_apply_changes(struct omap_vout_device *vout)
         return 0;
 }
 
-void omap_vout_isr(void *arg, unsigned int irqstatus)
+static void omap_vout_isr(void *arg, unsigned int irqstatus)
 {
         int ret;
         u32 addr, fid;
@@ -848,10 +547,20 @@ void omap_vout_isr(void *arg, unsigned int irqstatus)
 
         spin_lock(&vout->vbq_lock);
         do_gettimeofday(&timevalue);
-        if (cur_display->type == OMAP_DISPLAY_TYPE_DPI) {
-                if (!(irqstatus & DISPC_IRQ_VSYNC))
-                        goto vout_isr_err;
 
+        if (cur_display->type != OMAP_DISPLAY_TYPE_VENC) {
+                switch (cur_display->type) {
+                case OMAP_DISPLAY_TYPE_DPI:
+                        if (!(irqstatus & (DISPC_IRQ_VSYNC | DISPC_IRQ_VSYNC2)))
+                                goto vout_isr_err;
+                        break;
+                case OMAP_DISPLAY_TYPE_HDMI:
+                        if (!(irqstatus & DISPC_IRQ_EVSYNC_EVEN))
+                                goto vout_isr_err;
+                        break;
+                default:
+                        goto vout_isr_err;
+                }
                 if (!vout->first_int && (vout->cur_frm != vout->next_frm)) {
                         vout->cur_frm->ts = timevalue;
                         vout->cur_frm->state = VIDEOBUF_DONE;
@@ -875,7 +584,7 @@ void omap_vout_isr(void *arg, unsigned int irqstatus)
                 ret = omapvid_init(vout, addr);
                 if (ret)
                         printk(KERN_ERR VOUT_NAME
-                                        "failed to set overlay info\n");
+                                "failed to set overlay info\n");
                 /* Enable the pipeline and set the Go bit */
                 ret = omapvid_apply_changes(vout);
                 if (ret)
@@ -954,6 +663,7 @@ static int omap_vout_buffer_setup(struct videobuf_queue *q, unsigned int *count,
         int startindex = 0, i, j;
         u32 phy_addr = 0, virt_addr = 0;
         struct omap_vout_device *vout = q->priv_data;
+        struct omapvideo_info *ovid = &vout->vid_info;
 
         if (!vout)
                 return -EINVAL;
@@ -966,13 +676,10 @@ static int omap_vout_buffer_setup(struct videobuf_queue *q, unsigned int *count,
         if (V4L2_MEMORY_MMAP == vout->memory && *count < startindex)
                 *count = startindex;
 
-        if ((rotation_enabled(vout)) && *count > VRFB_NUM_BUFS)
-                *count = VRFB_NUM_BUFS;
-
-        /* If rotation is enabled, allocate memory for VRFB space also */
-        if (rotation_enabled(vout))
+        if (ovid->rotation_type == VOUT_ROT_VRFB) {
                 if (omap_vout_vrfb_buffer_setup(vout, count, startindex))
                         return -ENOMEM;
+        }
 
         if (V4L2_MEMORY_MMAP != vout->memory)
                 return 0;
@@ -996,8 +703,11 @@ static int omap_vout_buffer_setup(struct videobuf_queue *q, unsigned int *count,
                 virt_addr = omap_vout_alloc_buffer(vout->buffer_size,
                                 &phy_addr);
                 if (!virt_addr) {
-                        if (!rotation_enabled(vout))
+                        if (ovid->rotation_type == VOUT_ROT_NONE) {
                                 break;
+                        } else {
+                                if (!is_rotation_enabled(vout))
+                                        break;
                         /* Free the VRFB buffers if no space for V4L2 buffers */
                         for (j = i; j < *count; j++) {
                                 omap_vout_free_buffer(
@@ -1005,6 +715,7 @@ static int omap_vout_buffer_setup(struct videobuf_queue *q, unsigned int *count,
                                                 vout->smsshado_size);
                                 vout->smsshado_virt_addr[j] = 0;
                                 vout->smsshado_phy_addr[j] = 0;
+                                }
                         }
                 }
                 vout->buf_virt_addr[i] = virt_addr;
@@ -1017,9 +728,9 @@ static int omap_vout_buffer_setup(struct videobuf_queue *q, unsigned int *count,
 
 /*
  * Free the V4L2 buffers additionally allocated than default
- * number of buffers and free all the VRFB buffers
+ * number of buffers
  */
-static void omap_vout_free_allbuffers(struct omap_vout_device *vout)
+static void omap_vout_free_extra_buffers(struct omap_vout_device *vout)
 {
         int num_buffers = 0, i;
 
@@ -1034,20 +745,6 @@ static void omap_vout_free_allbuffers(struct omap_vout_device *vout)
                 vout->buf_virt_addr[i] = 0;
                 vout->buf_phy_addr[i] = 0;
         }
-        /* Free the VRFB buffers only if they are allocated
-         * during reqbufs.  Don't free if init time allocated
-         */
-        if (!vout->vrfb_static_allocation) {
-                for (i = 0; i < VRFB_NUM_BUFS; i++) {
-                        if (vout->smsshado_virt_addr[i]) {
-                                omap_vout_free_buffer(
-                                                vout->smsshado_virt_addr[i],
-                                                vout->smsshado_size);
-                                vout->smsshado_virt_addr[i] = 0;
-                                vout->smsshado_phy_addr[i] = 0;
-                        }
-                }
-        }
         vout->buffer_allocated = num_buffers;
 }
 
@@ -1059,16 +756,11 @@ static void omap_vout_free_allbuffers(struct omap_vout_device *vout)
  * buffer into VRFB memory space before giving it to the DSS.
  */
 static int omap_vout_buffer_prepare(struct videobuf_queue *q,
-                            struct videobuf_buffer *vb,
-                            enum v4l2_field field)
+                        struct videobuf_buffer *vb,
+                        enum v4l2_field field)
 {
-        dma_addr_t dmabuf;
-        struct vid_vrfb_dma *tx;
-        enum dss_rotation rotation;
         struct omap_vout_device *vout = q->priv_data;
-        u32 dest_frame_index = 0, src_element_index = 0;
-        u32 dest_element_index = 0, src_frame_index = 0;
-        u32 elem_count = 0, frame_count = 0, pixsize = 2;
+        struct omapvideo_info *ovid = &vout->vid_info;
 
         if (VIDEOBUF_NEEDS_INIT == vb->state) {
                 vb->width = vout->pix.width;
@@ -1087,66 +779,24 @@ static int omap_vout_buffer_prepare(struct videobuf_queue *q,
                 vout->queued_buf_addr[vb->i] = (u8 *)
                         omap_vout_uservirt_to_phys(vb->baddr);
         } else {
-                vout->queued_buf_addr[vb->i] = (u8 *)vout->buf_phy_addr[vb->i];
-        }
+                u32 addr, dma_addr;
+                unsigned long size;
 
-        if (!rotation_enabled(vout))
-                return 0;
+                addr = (unsigned long) vout->buf_virt_addr[vb->i];
+                size = (unsigned long) vb->size;
 
-        dmabuf = vout->buf_phy_addr[vb->i];
-        /* If rotation is enabled, copy input buffer into VRFB
-         * memory space using DMA. We are copying input buffer
-         * into VRFB memory space of desired angle and DSS will
-         * read image VRFB memory for 0 degree angle
-         */
-        pixsize = vout->bpp * vout->vrfb_bpp;
-        /*
-         * DMA transfer in double index mode
-         */
+                dma_addr = dma_map_single(vout->vid_dev->v4l2_dev.dev, (void *) addr,
+                                size, DMA_TO_DEVICE);
+                if (dma_mapping_error(vout->vid_dev->v4l2_dev.dev, dma_addr))
+                        v4l2_err(&vout->vid_dev->v4l2_dev, "dma_map_single failed\n");
 
-        /* Frame index */
-        dest_frame_index = ((MAX_PIXELS_PER_LINE * pixsize) -
-                        (vout->pix.width * vout->bpp)) + 1;
-
-        /* Source and destination parameters */
-        src_element_index = 0;
-        src_frame_index = 0;
-        dest_element_index = 1;
-        /* Number of elements per frame */
-        elem_count = vout->pix.width * vout->bpp;
-        frame_count = vout->pix.height;
-        tx = &vout->vrfb_dma_tx;
-        tx->tx_status = 0;
-        omap_set_dma_transfer_params(tx->dma_ch, OMAP_DMA_DATA_TYPE_S32,
-                        (elem_count / 4), frame_count, OMAP_DMA_SYNC_ELEMENT,
-                        tx->dev_id, 0x0);
-        /* src_port required only for OMAP1 */
-        omap_set_dma_src_params(tx->dma_ch, 0, OMAP_DMA_AMODE_POST_INC,
-                        dmabuf, src_element_index, src_frame_index);
-        /*set dma source burst mode for VRFB */
-        omap_set_dma_src_burst_mode(tx->dma_ch, OMAP_DMA_DATA_BURST_16);
-        rotation = calc_rotation(vout);
-
-        /* dest_port required only for OMAP1 */
-        omap_set_dma_dest_params(tx->dma_ch, 0, OMAP_DMA_AMODE_DOUBLE_IDX,
-                        vout->vrfb_context[vb->i].paddr[0], dest_element_index,
-                        dest_frame_index);
-        /*set dma dest burst mode for VRFB */
-        omap_set_dma_dest_burst_mode(tx->dma_ch, OMAP_DMA_DATA_BURST_16);
-        omap_dma_set_global_params(DMA_DEFAULT_ARB_RATE, 0x20, 0);
-
-        omap_start_dma(tx->dma_ch);
-        interruptible_sleep_on_timeout(&tx->wait, VRFB_TX_TIMEOUT);
-
-        if (tx->tx_status == 0) {
-                omap_stop_dma(tx->dma_ch);
-                return -EINVAL;
+                vout->queued_buf_addr[vb->i] = (u8 *)vout->buf_phy_addr[vb->i];
         }
-        /* Store buffers physical address into an array. Addresses
-         * from this array will be used to configure DSS */
-        vout->queued_buf_addr[vb->i] = (u8 *)
-                vout->vrfb_context[vb->i].paddr[rotation];
-        return 0;
+
+        if (ovid->rotation_type == VOUT_ROT_VRFB)
+                return omap_vout_prepare_vrfb(vout, vb);
+        else
+                return 0;
 }
 
 /*
@@ -1298,7 +948,15 @@ static int omap_vout_release(struct file *file)
                                 "Unable to apply changes\n");
 
         /* Free all buffers */
-        omap_vout_free_allbuffers(vout);
+        omap_vout_free_extra_buffers(vout);
+
+        /* Free the VRFB buffers only if they are allocated
+         * during reqbufs.  Don't free if init time allocated
+         */
+        if (ovid->rotation_type == VOUT_ROT_VRFB) {
+                if (!vout->vrfb_static_allocation)
+                        omap_vout_free_vrfb_buffers(vout);
+        }
         videobuf_mmap_free(q);
 
         /* Even if apply changes fails we should continue
@@ -1307,7 +965,7 @@ static int omap_vout_release(struct file *file)
                 u32 mask = 0;
 
                 mask = DISPC_IRQ_VSYNC | DISPC_IRQ_EVSYNC_EVEN |
-                        DISPC_IRQ_EVSYNC_ODD;
+                        DISPC_IRQ_EVSYNC_ODD | DISPC_IRQ_VSYNC2;
                 omap_dispc_unregister_isr(omap_vout_isr, vout, mask);
                 vout->streaming = 0;
 
@@ -1383,10 +1041,7 @@ static int vidioc_enum_fmt_vid_out(struct file *file, void *fh,
                         struct v4l2_fmtdesc *fmt)
 {
         int index = fmt->index;
-        enum v4l2_buf_type type = fmt->type;
 
-        fmt->index = index;
-        fmt->type = type;
         if (index >= NUM_OUTPUT_FORMATS)
                 return -EINVAL;
 
@@ -1457,7 +1112,7 @@ static int vidioc_s_fmt_vid_out(struct file *file, void *fh,
 
         /* We dont support RGB24-packed mode if vrfb rotation
          * is enabled*/
-        if ((rotation_enabled(vout)) &&
+        if ((is_rotation_enabled(vout)) &&
                         f->fmt.pix.pixelformat == V4L2_PIX_FMT_RGB24) {
                 ret = -EINVAL;
                 goto s_fmt_vid_out_exit;
@@ -1465,7 +1120,7 @@ static int vidioc_s_fmt_vid_out(struct file *file, void *fh,
 
         /* get the framebuffer parameters */
 
-        if (rotate_90_or_270(vout)) {
+        if (is_rotation_90_or_270(vout)) {
                 vout->fbuf.fmt.height = timing->x_res;
                 vout->fbuf.fmt.width = timing->y_res;
         } else {
@@ -1555,10 +1210,7 @@ static int vidioc_enum_fmt_vid_overlay(struct file *file, void *fh,
                         struct v4l2_fmtdesc *fmt)
 {
         int index = fmt->index;
-        enum v4l2_buf_type type = fmt->type;
 
-        fmt->index = index;
-        fmt->type = type;
         if (index >= NUM_OUTPUT_FORMATS)
                 return -EINVAL;
 
@@ -1645,7 +1297,7 @@ static int vidioc_s_crop(struct file *file, void *fh, struct v4l2_crop *crop)
         /* get the display device attached to the overlay */
         timing = &ovl->manager->device->panel.timings;
 
-        if (rotate_90_or_270(vout)) {
+        if (is_rotation_90_or_270(vout)) {
                 vout->fbuf.fmt.height = timing->x_res;
                 vout->fbuf.fmt.width = timing->y_res;
         } else {
@@ -1725,9 +1377,17 @@ static int vidioc_s_ctrl(struct file *file, void *fh, struct v4l2_control *a)
         switch (a->id) {
         case V4L2_CID_ROTATE:
         {
+                struct omapvideo_info *ovid;
                 int rotation = a->value;
 
+                ovid = &vout->vid_info;
+
                 mutex_lock(&vout->lock);
+                if (rotation && ovid->rotation_type == VOUT_ROT_NONE) {
+                        mutex_unlock(&vout->lock);
+                        ret = -ERANGE;
+                        break;
+                }
 
                 if (rotation && vout->pix.pixelformat == V4L2_PIX_FMT_RGB24) {
                         mutex_unlock(&vout->lock);
@@ -1783,6 +1443,11 @@ static int vidioc_s_ctrl(struct file *file, void *fh, struct v4l2_control *a)
                 ovl = ovid->overlays[0];
 
                 mutex_lock(&vout->lock);
+                if (mirror && ovid->rotation_type == VOUT_ROT_NONE) {
+                        mutex_unlock(&vout->lock);
+                        ret = -ERANGE;
+                        break;
+                }
 
                 if (mirror  && vout->pix.pixelformat == V4L2_PIX_FMT_RGB24) {
                         mutex_unlock(&vout->lock);
@@ -1893,7 +1558,7 @@ static int vidioc_qbuf(struct file *file, void *fh,
                 }
         }
 
-        if ((rotation_enabled(vout)) &&
+        if ((is_rotation_enabled(vout)) &&
                         vout->vrfb_dma_tx.req_status == DMA_CHAN_NOT_ALLOTED) {
                 v4l2_warn(&vout->vid_dev->v4l2_dev,
                                 "DMA Channel not allocated for Rotation\n");
@@ -1908,15 +1573,28 @@ static int vidioc_dqbuf(struct file *file, void *fh, struct v4l2_buffer *b)
         struct omap_vout_device *vout = fh;
         struct videobuf_queue *q = &vout->vbq;
 
+        int ret;
+        u32 addr;
+        unsigned long size;
+        struct videobuf_buffer *vb;
+
+        vb = q->bufs[b->index];
+
         if (!vout->streaming)
                 return -EINVAL;
 
         if (file->f_flags & O_NONBLOCK)
                 /* Call videobuf_dqbuf for non blocking mode */
-                return videobuf_dqbuf(q, (struct v4l2_buffer *)b, 1);
+                ret = videobuf_dqbuf(q, (struct v4l2_buffer *)b, 1);
         else
                 /* Call videobuf_dqbuf for  blocking mode */
-                return videobuf_dqbuf(q, (struct v4l2_buffer *)b, 0);
+                ret = videobuf_dqbuf(q, (struct v4l2_buffer *)b, 0);
+
+        addr = (unsigned long) vout->buf_phy_addr[vb->i];
+        size = (unsigned long) vb->size;
+        dma_unmap_single(vout->vid_dev->v4l2_dev.dev,  addr,
+                                size, DMA_TO_DEVICE);
+        return ret;
 }
 
 static int vidioc_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
@@ -1965,7 +1643,8 @@ static int vidioc_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
         addr = (unsigned long) vout->queued_buf_addr[vout->cur_frm->i]
                 + vout->cropped_offset;
 
-        mask = DISPC_IRQ_VSYNC | DISPC_IRQ_EVSYNC_EVEN | DISPC_IRQ_EVSYNC_ODD;
+        mask = DISPC_IRQ_VSYNC | DISPC_IRQ_EVSYNC_EVEN | DISPC_IRQ_EVSYNC_ODD
+                | DISPC_IRQ_VSYNC2;
 
         omap_dispc_register_isr(omap_vout_isr, vout, mask);
 
@@ -2015,7 +1694,8 @@ static int vidioc_streamoff(struct file *file, void *fh, enum v4l2_buf_type i)
                 return -EINVAL;
 
         vout->streaming = 0;
-        mask = DISPC_IRQ_VSYNC | DISPC_IRQ_EVSYNC_EVEN | DISPC_IRQ_EVSYNC_ODD;
+        mask = DISPC_IRQ_VSYNC | DISPC_IRQ_EVSYNC_EVEN | DISPC_IRQ_EVSYNC_ODD
+                | DISPC_IRQ_VSYNC2;
 
         omap_dispc_unregister_isr(omap_vout_isr, vout, mask);
 
@@ -2228,7 +1908,8 @@ static int __init omap_vout_setup_video_data(struct omap_vout_device *vout)
         vout->mirror = 0;
         vout->control[2].id = V4L2_CID_HFLIP;
         vout->control[2].value = 0;
-        vout->vrfb_bpp = 2;
+        if (vout->vid_info.rotation_type == VOUT_ROT_VRFB)
+                vout->vrfb_bpp = 2;
 
         control[1].id = V4L2_CID_BG_COLOR;
         control[1].value = 0;
@@ -2260,17 +1941,15 @@ static int __init omap_vout_setup_video_bufs(struct platform_device *pdev,
                 int vid_num)
 {
         u32 numbuffers;
-        int ret = 0, i, j;
-        int image_width, image_height;
-        struct video_device *vfd;
+        int ret = 0, i;
+        struct omapvideo_info *ovid;
         struct omap_vout_device *vout;
-        int static_vrfb_allocation = 0, vrfb_num_bufs = VRFB_NUM_BUFS;
         struct v4l2_device *v4l2_dev = platform_get_drvdata(pdev);
         struct omap2video_device *vid_dev =
                 container_of(v4l2_dev, struct omap2video_device, v4l2_dev);
 
         vout = vid_dev->vouts[vid_num];
-        vfd = vout->vfd;
+        ovid = &vout->vid_info;
 
         numbuffers = (vid_num == 0) ? video1_numbuffers : video2_numbuffers;
         vout->buffer_size = (vid_num == 0) ? video1_bufsize : video2_bufsize;
@@ -2287,66 +1966,16 @@ static int __init omap_vout_setup_video_bufs(struct platform_device *pdev,
                 }
         }
 
-        for (i = 0; i < VRFB_NUM_BUFS; i++) {
-                if (omap_vrfb_request_ctx(&vout->vrfb_context[i])) {
-                        dev_info(&pdev->dev, ": VRFB allocation failed\n");
-                        for (j = 0; j < i; j++)
-                                omap_vrfb_release_ctx(&vout->vrfb_context[j]);
-                        ret = -ENOMEM;
-                        goto free_buffers;
-                }
-        }
         vout->cropped_offset = 0;
 
-        /* Calculate VRFB memory size */
-        /* allocate for worst case size */
-        image_width = VID_MAX_WIDTH / TILE_SIZE;
-        if (VID_MAX_WIDTH % TILE_SIZE)
-                image_width++;
-
-        image_width = image_width * TILE_SIZE;
-        image_height = VID_MAX_HEIGHT / TILE_SIZE;
-
-        if (VID_MAX_HEIGHT % TILE_SIZE)
-                image_height++;
-
-        image_height = image_height * TILE_SIZE;
-        vout->smsshado_size = PAGE_ALIGN(image_width * image_height * 2 * 2);
-
-        /*
-         * Request and Initialize DMA, for DMA based VRFB transfer
-         */
-        vout->vrfb_dma_tx.dev_id = OMAP_DMA_NO_DEVICE;
-        vout->vrfb_dma_tx.dma_ch = -1;
-        vout->vrfb_dma_tx.req_status = DMA_CHAN_ALLOTED;
-        ret = omap_request_dma(vout->vrfb_dma_tx.dev_id, "VRFB DMA TX",
-                        omap_vout_vrfb_dma_tx_callback,
-                        (void *) &vout->vrfb_dma_tx, &vout->vrfb_dma_tx.dma_ch);
-        if (ret < 0) {
-                vout->vrfb_dma_tx.req_status = DMA_CHAN_NOT_ALLOTED;
-                dev_info(&pdev->dev, ": failed to allocate DMA Channel for"
-                                " video%d\n", vfd->minor);
-        }
-        init_waitqueue_head(&vout->vrfb_dma_tx.wait);
-
-        /* Allocate VRFB buffers if selected through bootargs */
-        static_vrfb_allocation = (vid_num == 0) ?
-                vid1_static_vrfb_alloc : vid2_static_vrfb_alloc;
-
-        /* statically allocated the VRFB buffer is done through
-           commands line aruments */
-        if (static_vrfb_allocation) {
-                if (omap_vout_allocate_vrfb_buffers(vout, &vrfb_num_bufs, -1)) {
-                        ret =  -ENOMEM;
-                        goto release_vrfb_ctx;
-                }
-                vout->vrfb_static_allocation = 1;
+        if (ovid->rotation_type == VOUT_ROT_VRFB) {
+                int static_vrfb_allocation = (vid_num == 0) ?
+                        vid1_static_vrfb_alloc : vid2_static_vrfb_alloc;
+                ret = omap_vout_setup_vrfb_bufs(pdev, vid_num,
+                                static_vrfb_allocation);
         }
-        return 0;
 
-release_vrfb_ctx:
-        for (j = 0; j < VRFB_NUM_BUFS; j++)
-                omap_vrfb_release_ctx(&vout->vrfb_context[j]);
+        return ret;
 
 free_buffers:
         for (i = 0; i < numbuffers; i++) {
@@ -2389,6 +2018,10 @@ static int __init omap_vout_create_video_devices(struct platform_device *pdev)
                 vout->vid_info.num_overlays = 1;
                 vout->vid_info.id = k + 1;
 
+                /* Set VRFB as rotation_type for omap2 and omap3 */
+                if (cpu_is_omap24xx() || cpu_is_omap34xx())
+                        vout->vid_info.rotation_type = VOUT_ROT_VRFB;
+
                 /* Setup the default configuration for the video devices
                  */
                 if (omap_vout_setup_video_data(vout) != 0) {
@@ -2422,7 +2055,8 @@ static int __init omap_vout_create_video_devices(struct platform_device *pdev)
                         goto success;
 
 error2:
-                omap_vout_release_vrfb(vout);
+                if (vout->vid_info.rotation_type == VOUT_ROT_VRFB)
+                        omap_vout_release_vrfb(vout);
                 omap_vout_free_buffers(vout);
 error1:
                 video_device_release(vfd);
@@ -2443,11 +2077,13 @@ success:
 static void omap_vout_cleanup_device(struct omap_vout_device *vout)
 {
         struct video_device *vfd;
+        struct omapvideo_info *ovid;
 
         if (!vout)
                 return;
 
         vfd = vout->vfd;
+        ovid = &vout->vid_info;
         if (vfd) {
                 if (!video_is_registered(vfd)) {
                         /*
@@ -2463,14 +2099,15 @@ static void omap_vout_cleanup_device(struct omap_vout_device *vout)
                         video_unregister_device(vfd);
                 }
         }
-
-        omap_vout_release_vrfb(vout);
+        if (ovid->rotation_type == VOUT_ROT_VRFB) {
+                omap_vout_release_vrfb(vout);
+                /* Free the VRFB buffer if allocated
+                 * init time
+                 */
+                if (vout->vrfb_static_allocation)
+                        omap_vout_free_vrfb_buffers(vout);
+        }
         omap_vout_free_buffers(vout);
-        /* Free the VRFB buffer if allocated
-         * init time
-         */
-        if (vout->vrfb_static_allocation)
-                omap_vout_free_vrfb_buffers(vout);
 
         kfree(vout);
 }
diff --git a/drivers/media/video/omap/omap_vout_vrfb.c b/drivers/media/video/omap/omap_vout_vrfb.c
new file mode 100644
index 00000000000..ebebcac4922
--- /dev/null
+++ b/drivers/media/video/omap/omap_vout_vrfb.c
@@ -0,0 +1,390 @@
+/*
+ * omap_vout_vrfb.c
+ *
+ * Copyright (C) 2010 Texas Instruments.
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ *
+ */
+
+#include <linux/sched.h>
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+
+#include <media/videobuf-dma-contig.h>
+#include <media/v4l2-device.h>
+
+#include <plat/dma.h>
+#include <plat/vrfb.h>
+
+#include "omap_voutdef.h"
+#include "omap_voutlib.h"
+
+/*
+ * Function for allocating video buffers
+ */
+static int omap_vout_allocate_vrfb_buffers(struct omap_vout_device *vout,
+                unsigned int *count, int startindex)
+{
+        int i, j;
+
+        for (i = 0; i < *count; i++) {
+                if (!vout->smsshado_virt_addr[i]) {
+                        vout->smsshado_virt_addr[i] =
+                                omap_vout_alloc_buffer(vout->smsshado_size,
+                                                &vout->smsshado_phy_addr[i]);
+                }
+                if (!vout->smsshado_virt_addr[i] && startindex != -1) {
+                        if (V4L2_MEMORY_MMAP == vout->memory && i >= startindex)
+                                break;
+                }
+                if (!vout->smsshado_virt_addr[i]) {
+                        for (j = 0; j < i; j++) {
+                                omap_vout_free_buffer(
+                                                vout->smsshado_virt_addr[j],
+                                                vout->smsshado_size);
+                                vout->smsshado_virt_addr[j] = 0;
+                                vout->smsshado_phy_addr[j] = 0;
+                        }
+                        *count = 0;
+                        return -ENOMEM;
+                }
+                memset((void *) vout->smsshado_virt_addr[i], 0,
+                                vout->smsshado_size);
+        }
+        return 0;
+}
+
+/*
+ * Wakes up the application once the DMA transfer to VRFB space is completed.
+ */
+static void omap_vout_vrfb_dma_tx_callback(int lch, u16 ch_status, void *data)
+{
+        struct vid_vrfb_dma *t = (struct vid_vrfb_dma *) data;
+
+        t->tx_status = 1;
+        wake_up_interruptible(&t->wait);
+}
+
+/*
+ * Free VRFB buffers
+ */
+void omap_vout_free_vrfb_buffers(struct omap_vout_device *vout)
+{
+        int j;
+
+        for (j = 0; j < VRFB_NUM_BUFS; j++) {
+                omap_vout_free_buffer(vout->smsshado_virt_addr[j],
+                                vout->smsshado_size);
+                vout->smsshado_virt_addr[j] = 0;
+                vout->smsshado_phy_addr[j] = 0;
+        }
+}
+
+int omap_vout_setup_vrfb_bufs(struct platform_device *pdev, int vid_num,
+                        u32 static_vrfb_allocation)
+{
+        int ret = 0, i, j;
+        struct omap_vout_device *vout;
+        struct video_device *vfd;
+        int image_width, image_height;
+        int vrfb_num_bufs = VRFB_NUM_BUFS;
+        struct v4l2_device *v4l2_dev = platform_get_drvdata(pdev);
+        struct omap2video_device *vid_dev =
+                container_of(v4l2_dev, struct omap2video_device, v4l2_dev);
+
+        vout = vid_dev->vouts[vid_num];
+        vfd = vout->vfd;
+
+        for (i = 0; i < VRFB_NUM_BUFS; i++) {
+                if (omap_vrfb_request_ctx(&vout->vrfb_context[i])) {
+                        dev_info(&pdev->dev, ": VRFB allocation failed\n");
+                        for (j = 0; j < i; j++)
+                                omap_vrfb_release_ctx(&vout->vrfb_context[j]);
+                        ret = -ENOMEM;
+                        goto free_buffers;
+                }
+        }
+
+        /* Calculate VRFB memory size */
+        /* allocate for worst case size */
+        image_width = VID_MAX_WIDTH / TILE_SIZE;
+        if (VID_MAX_WIDTH % TILE_SIZE)
+                image_width++;
+
+        image_width = image_width * TILE_SIZE;
+        image_height = VID_MAX_HEIGHT / TILE_SIZE;
+
+        if (VID_MAX_HEIGHT % TILE_SIZE)
+                image_height++;
+
+        image_height = image_height * TILE_SIZE;
+        vout->smsshado_size = PAGE_ALIGN(image_width * image_height * 2 * 2);
+
+        /*
+         * Request and Initialize DMA, for DMA based VRFB transfer
+         */
+        vout->vrfb_dma_tx.dev_id = OMAP_DMA_NO_DEVICE;
+        vout->vrfb_dma_tx.dma_ch = -1;
+        vout->vrfb_dma_tx.req_status = DMA_CHAN_ALLOTED;
+        ret = omap_request_dma(vout->vrfb_dma_tx.dev_id, "VRFB DMA TX",
+                        omap_vout_vrfb_dma_tx_callback,
+                        (void *) &vout->vrfb_dma_tx, &vout->vrfb_dma_tx.dma_ch);
+        if (ret < 0) {
+                vout->vrfb_dma_tx.req_status = DMA_CHAN_NOT_ALLOTED;
+                dev_info(&pdev->dev, ": failed to allocate DMA Channel for"
+                                " video%d\n", vfd->minor);
+        }
+        init_waitqueue_head(&vout->vrfb_dma_tx.wait);
+
+        /* statically allocated the VRFB buffer is done through
+           commands line aruments */
+        if (static_vrfb_allocation) {
+                if (omap_vout_allocate_vrfb_buffers(vout, &vrfb_num_bufs, -1)) {
+                        ret =  -ENOMEM;
+                        goto release_vrfb_ctx;
+                }
+                vout->vrfb_static_allocation = 1;
+        }
+        return 0;
+
+release_vrfb_ctx:
+        for (j = 0; j < VRFB_NUM_BUFS; j++)
+                omap_vrfb_release_ctx(&vout->vrfb_context[j]);
+free_buffers:
+        omap_vout_free_buffers(vout);
+
+        return ret;
+}
+
+/*
+ * Release the VRFB context once the module exits
+ */
+void omap_vout_release_vrfb(struct omap_vout_device *vout)
+{
+        int i;
+
+        for (i = 0; i < VRFB_NUM_BUFS; i++)
+                omap_vrfb_release_ctx(&vout->vrfb_context[i]);
+
+        if (vout->vrfb_dma_tx.req_status == DMA_CHAN_ALLOTED) {
+                vout->vrfb_dma_tx.req_status = DMA_CHAN_NOT_ALLOTED;
+                omap_free_dma(vout->vrfb_dma_tx.dma_ch);
+        }
+}
+
+/*
+ * Allocate the buffers for the VRFB space.  Data is copied from V4L2
+ * buffers to the VRFB buffers using the DMA engine.
+ */
+int omap_vout_vrfb_buffer_setup(struct omap_vout_device *vout,
+                          unsigned int *count, unsigned int startindex)
+{
+        int i;
+        bool yuv_mode;
+
+        if (!is_rotation_enabled(vout))
+                return 0;
+
+        /* If rotation is enabled, allocate memory for VRFB space also */
+        *count = *count > VRFB_NUM_BUFS ? VRFB_NUM_BUFS : *count;
+
+        /* Allocate the VRFB buffers only if the buffers are not
+         * allocated during init time.
+         */
+        if (!vout->vrfb_static_allocation)
+                if (omap_vout_allocate_vrfb_buffers(vout, count, startindex))
+                        return -ENOMEM;
+
+        if (vout->dss_mode == OMAP_DSS_COLOR_YUV2 ||
+                        vout->dss_mode == OMAP_DSS_COLOR_UYVY)
+                yuv_mode = true;
+        else
+                yuv_mode = false;
+
+        for (i = 0; i < *count; i++)
+                omap_vrfb_setup(&vout->vrfb_context[i],
+                                vout->smsshado_phy_addr[i], vout->pix.width,
+                                vout->pix.height, vout->bpp, yuv_mode);
+
+        return 0;
+}
+
+int omap_vout_prepare_vrfb(struct omap_vout_device *vout,
+                                struct videobuf_buffer *vb)
+{
+        dma_addr_t dmabuf;
+        struct vid_vrfb_dma *tx;
+        enum dss_rotation rotation;
+        u32 dest_frame_index = 0, src_element_index = 0;
+        u32 dest_element_index = 0, src_frame_index = 0;
+        u32 elem_count = 0, frame_count = 0, pixsize = 2;
+
+        if (!is_rotation_enabled(vout))
+                return 0;
+
+        dmabuf = vout->buf_phy_addr[vb->i];
+        /* If rotation is enabled, copy input buffer into VRFB
+         * memory space using DMA. We are copying input buffer
+         * into VRFB memory space of desired angle and DSS will
+         * read image VRFB memory for 0 degree angle
+         */
+        pixsize = vout->bpp * vout->vrfb_bpp;
+        /*
+         * DMA transfer in double index mode
+         */
+
+        /* Frame index */
+        dest_frame_index = ((MAX_PIXELS_PER_LINE * pixsize) -
+                        (vout->pix.width * vout->bpp)) + 1;
+
+        /* Source and destination parameters */
+        src_element_index = 0;
+        src_frame_index = 0;
+        dest_element_index = 1;
+        /* Number of elements per frame */
+        elem_count = vout->pix.width * vout->bpp;
+        frame_count = vout->pix.height;
+        tx = &vout->vrfb_dma_tx;
+        tx->tx_status = 0;
+        omap_set_dma_transfer_params(tx->dma_ch, OMAP_DMA_DATA_TYPE_S32,
+                        (elem_count / 4), frame_count, OMAP_DMA_SYNC_ELEMENT,
+                        tx->dev_id, 0x0);
+        /* src_port required only for OMAP1 */
+        omap_set_dma_src_params(tx->dma_ch, 0, OMAP_DMA_AMODE_POST_INC,
+                        dmabuf, src_element_index, src_frame_index);
+        /*set dma source burst mode for VRFB */
+        omap_set_dma_src_burst_mode(tx->dma_ch, OMAP_DMA_DATA_BURST_16);
+        rotation = calc_rotation(vout);
+
+        /* dest_port required only for OMAP1 */
+        omap_set_dma_dest_params(tx->dma_ch, 0, OMAP_DMA_AMODE_DOUBLE_IDX,
+                        vout->vrfb_context[vb->i].paddr[0], dest_element_index,
+                        dest_frame_index);
+        /*set dma dest burst mode for VRFB */
+        omap_set_dma_dest_burst_mode(tx->dma_ch, OMAP_DMA_DATA_BURST_16);
+        omap_dma_set_global_params(DMA_DEFAULT_ARB_RATE, 0x20, 0);
+
+        omap_start_dma(tx->dma_ch);
+        interruptible_sleep_on_timeout(&tx->wait, VRFB_TX_TIMEOUT);
+
+        if (tx->tx_status == 0) {
+                omap_stop_dma(tx->dma_ch);
+                return -EINVAL;
+        }
+        /* Store buffers physical address into an array. Addresses
+         * from this array will be used to configure DSS */
+        vout->queued_buf_addr[vb->i] = (u8 *)
+                vout->vrfb_context[vb->i].paddr[rotation];
+        return 0;
+}
+
+/*
+ * Calculate the buffer offsets from which the streaming should
+ * start. This offset calculation is mainly required because of
+ * the VRFB 32 pixels alignment with rotation.
+ */
+void omap_vout_calculate_vrfb_offset(struct omap_vout_device *vout)
+{
+        enum dss_rotation rotation;
+        bool mirroring = vout->mirror;
+        struct v4l2_rect *crop = &vout->crop;
+        struct v4l2_pix_format *pix = &vout->pix;
+        int *cropped_offset = &vout->cropped_offset;
+        int vr_ps = 1, ps = 2, temp_ps = 2;
+        int offset = 0, ctop = 0, cleft = 0, line_length = 0;
+
+        rotation = calc_rotation(vout);
+
+        if (V4L2_PIX_FMT_YUYV == pix->pixelformat ||
+                        V4L2_PIX_FMT_UYVY == pix->pixelformat) {
+                if (is_rotation_enabled(vout)) {
+                        /*
+                         * ps    - Actual pixel size for YUYV/UYVY for
+                         *         VRFB/Mirroring is 4 bytes
+                         * vr_ps - Virtually pixel size for YUYV/UYVY is
+                         *         2 bytes
+                         */
+                        ps = 4;
+                        vr_ps = 2;
+                } else {
+                        ps = 2; /* otherwise the pixel size is 2 byte */
+                }
+        } else if (V4L2_PIX_FMT_RGB32 == pix->pixelformat) {
+                ps = 4;
+        } else if (V4L2_PIX_FMT_RGB24 == pix->pixelformat) {
+                ps = 3;
+        }
+        vout->ps = ps;
+        vout->vr_ps = vr_ps;
+
+        if (is_rotation_enabled(vout)) {
+                line_length = MAX_PIXELS_PER_LINE;
+                ctop = (pix->height - crop->height) - crop->top;
+                cleft = (pix->width - crop->width) - crop->left;
+        } else {
+                line_length = pix->width;
+        }
+        vout->line_length = line_length;
+        switch (rotation) {
+        case dss_rotation_90_degree:
+                offset = vout->vrfb_context[0].yoffset *
+                        vout->vrfb_context[0].bytespp;
+                temp_ps = ps / vr_ps;
+                if (mirroring == 0) {
+                        *cropped_offset = offset + line_length *
+                                temp_ps * cleft + crop->top * temp_ps;
+                } else {
+                        *cropped_offset = offset + line_length * temp_ps *
+                                cleft + crop->top * temp_ps + (line_length *
+                                ((crop->width / (vr_ps)) - 1) * ps);
+                }
+                break;
+        case dss_rotation_180_degree:
+                offset = ((MAX_PIXELS_PER_LINE * vout->vrfb_context[0].yoffset *
+                        vout->vrfb_context[0].bytespp) +
+                        (vout->vrfb_context[0].xoffset *
+                        vout->vrfb_context[0].bytespp));
+                if (mirroring == 0) {
+                        *cropped_offset = offset + (line_length * ps * ctop) +
+                                (cleft / vr_ps) * ps;
+
+                } else {
+                        *cropped_offset = offset + (line_length * ps * ctop) +
+                                (cleft / vr_ps) * ps + (line_length *
+                                (crop->height - 1) * ps);
+                }
+                break;
+        case dss_rotation_270_degree:
+                offset = MAX_PIXELS_PER_LINE * vout->vrfb_context[0].xoffset *
+                        vout->vrfb_context[0].bytespp;
+                temp_ps = ps / vr_ps;
+                if (mirroring == 0) {
+                        *cropped_offset = offset + line_length *
+                            temp_ps * crop->left + ctop * ps;
+                } else {
+                        *cropped_offset = offset + line_length *
+                                temp_ps * crop->left + ctop * ps +
+                                (line_length * ((crop->width / vr_ps) - 1) *
+                                 ps);
+                }
+                break;
+        case dss_rotation_0_degree:
+                if (mirroring == 0) {
+                        *cropped_offset = (line_length * ps) *
+                                crop->top + (crop->left / vr_ps) * ps;
+                } else {
+                        *cropped_offset = (line_length * ps) *
+                                crop->top + (crop->left / vr_ps) * ps +
+                                (line_length * (crop->height - 1) * ps);
+                }
+                break;
+        default:
+                *cropped_offset = (line_length * ps * crop->top) /
+                        vr_ps + (crop->left * ps) / vr_ps +
+                        ((crop->width / vr_ps) - 1) * ps;
+                break;
+        }
+}
diff --git a/drivers/media/video/omap/omap_vout_vrfb.h b/drivers/media/video/omap/omap_vout_vrfb.h
new file mode 100644
index 00000000000..ffde741e059
--- /dev/null
+++ b/drivers/media/video/omap/omap_vout_vrfb.h
@@ -0,0 +1,40 @@
+/*
+ * omap_vout_vrfb.h
+ *
+ * Copyright (C) 2010 Texas Instruments.
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ *
+ */
+
+#ifndef OMAP_VOUT_VRFB_H
+#define OMAP_VOUT_VRFB_H
+
+#ifdef CONFIG_VIDEO_OMAP2_VOUT_VRFB
+void omap_vout_free_vrfb_buffers(struct omap_vout_device *vout);
+int omap_vout_setup_vrfb_bufs(struct platform_device *pdev, int vid_num,
+                        u32 static_vrfb_allocation);
+void omap_vout_release_vrfb(struct omap_vout_device *vout);
+int omap_vout_vrfb_buffer_setup(struct omap_vout_device *vout,
+                        unsigned int *count, unsigned int startindex);
+int omap_vout_prepare_vrfb(struct omap_vout_device *vout,
+                        struct videobuf_buffer *vb);
+void omap_vout_calculate_vrfb_offset(struct omap_vout_device *vout);
+#else
+void omap_vout_free_vrfb_buffers(struct omap_vout_device *vout) { }
+int omap_vout_setup_vrfb_bufs(struct platform_device *pdev, int vid_num,
+                        u32 static_vrfb_allocation)
+                { return 0; }
+void omap_vout_release_vrfb(struct omap_vout_device *vout) { }
+int omap_vout_vrfb_buffer_setup(struct omap_vout_device *vout,
+                        unsigned int *count, unsigned int startindex)
+                { return 0; }
+int omap_vout_prepare_vrfb(struct omap_vout_device *vout,
+                        struct videobuf_buffer *vb)
+                { return 0; }
+void omap_vout_calculate_vrfb_offset(struct omap_vout_device *vout) { }
+#endif
+
+#endif
diff --git a/drivers/media/video/omap/omap_voutdef.h b/drivers/media/video/omap/omap_voutdef.h
index 659497b8499..d793501cafc 100644
--- a/drivers/media/video/omap/omap_voutdef.h
+++ b/drivers/media/video/omap/omap_voutdef.h
@@ -12,6 +12,7 @@
 #define OMAP_VOUTDEF_H
 
 #include <video/omapdss.h>
+#include <plat/vrfb.h>
 
 #define YUYV_BPP        2
 #define RGB565_BPP      2
@@ -27,6 +28,31 @@
 #define MAX_DISPLAYS    3
 #define MAX_MANAGERS    3
 
+#define QQVGA_WIDTH             160
+#define QQVGA_HEIGHT            120
+
+/* Max Resolution supported by the driver */
+#define VID_MAX_WIDTH           1280    /* Largest width */
+#define VID_MAX_HEIGHT          720     /* Largest height */
+
+/* Mimimum requirement is 2x2 for DSS */
+#define VID_MIN_WIDTH           2
+#define VID_MIN_HEIGHT          2
+
+/* 2048 x 2048 is max res supported by OMAP display controller */
+#define MAX_PIXELS_PER_LINE     2048
+
+#define VRFB_TX_TIMEOUT         1000
+#define VRFB_NUM_BUFS           4
+
+/* Max buffer size tobe allocated during init */
+#define OMAP_VOUT_MAX_BUF_SIZE (VID_MAX_WIDTH*VID_MAX_HEIGHT*4)
+
+enum dma_channel_state {
+        DMA_CHAN_NOT_ALLOTED,
+        DMA_CHAN_ALLOTED,
+};
+
 /* Enum for Rotation
  * DSS understands rotation in 0, 1, 2, 3 context
  * while V4L2 driver understands it as 0, 90, 180, 270
@@ -37,6 +63,18 @@ enum dss_rotation {
         dss_rotation_180_degree = 2,
         dss_rotation_270_degree = 3,
 };
+
+/* Enum for choosing rotation type for vout
+ * DSS2 doesn't understand no rotation as an
+ * option while V4L2 driver doesn't support
+ * rotation in the case where VRFB is not built in
+ * the kernel
+ */
+enum vout_rotaion_type {
+        VOUT_ROT_NONE   = 0,
+        VOUT_ROT_VRFB   = 1,
+};
+
 /*
  * This structure is used to store the DMA transfer parameters
  * for VRFB hidden buffer
@@ -53,6 +91,7 @@ struct omapvideo_info {
         int id;
         int num_overlays;
         struct omap_overlay *overlays[MAX_OVLS];
+        enum vout_rotaion_type rotation_type;
 };
 
 struct omap2video_device {
@@ -144,4 +183,43 @@ struct omap_vout_device {
         int io_allowed;
 
 };
+
+/*
+ * Return true if rotation is 90 or 270
+ */
+static inline int is_rotation_90_or_270(const struct omap_vout_device *vout)
+{
+        return (vout->rotation == dss_rotation_90_degree ||
+                        vout->rotation == dss_rotation_270_degree);
+}
+
+/*
+ * Return true if rotation is enabled
+ */
+static inline int is_rotation_enabled(const struct omap_vout_device *vout)
+{
+        return vout->rotation || vout->mirror;
+}
+
+/*
+ * Reverse the rotation degree if mirroring is enabled
+ */
+static inline int calc_rotation(const struct omap_vout_device *vout)
+{
+        if (!vout->mirror)
+                return vout->rotation;
+
+        switch (vout->rotation) {
+        case dss_rotation_90_degree:
+                return dss_rotation_270_degree;
+        case dss_rotation_270_degree:
+                return dss_rotation_90_degree;
+        case dss_rotation_180_degree:
+                return dss_rotation_0_degree;
+        default:
+                return dss_rotation_180_degree;
+        }
+}
+
+void omap_vout_free_buffers(struct omap_vout_device *vout);
 #endif  /* ifndef OMAP_VOUTDEF_H */
diff --git a/drivers/media/video/omap/omap_voutlib.c b/drivers/media/video/omap/omap_voutlib.c
index 8ae74817a11..115408b9274 100644
--- a/drivers/media/video/omap/omap_voutlib.c
+++ b/drivers/media/video/omap/omap_voutlib.c
@@ -24,8 +24,12 @@
 #include <linux/types.h>
 #include <linux/videodev2.h>
 
+#include <linux/dma-mapping.h>
+
 #include <plat/cpu.h>
 
+#include "omap_voutlib.h"
+
 MODULE_AUTHOR("Texas Instruments");
 MODULE_DESCRIPTION("OMAP Video library");
 MODULE_LICENSE("GPL");
@@ -291,3 +295,45 @@ void omap_vout_new_format(struct v4l2_pix_format *pix,
 }
 EXPORT_SYMBOL_GPL(omap_vout_new_format);
 
+/*
+ * Allocate buffers
+ */
+unsigned long omap_vout_alloc_buffer(u32 buf_size, u32 *phys_addr)
+{
+        u32 order, size;
+        unsigned long virt_addr, addr;
+
+        size = PAGE_ALIGN(buf_size);
+        order = get_order(size);
+        virt_addr = __get_free_pages(GFP_KERNEL, order);
+        addr = virt_addr;
+
+        if (virt_addr) {
+                while (size > 0) {
+                        SetPageReserved(virt_to_page(addr));
+                        addr += PAGE_SIZE;
+                        size -= PAGE_SIZE;
+                }
+        }
+        *phys_addr = (u32) virt_to_phys((void *) virt_addr);
+        return virt_addr;
+}
+
+/*
+ * Free buffers
+ */
+void omap_vout_free_buffer(unsigned long virtaddr, u32 buf_size)
+{
+        u32 order, size;
+        unsigned long addr = virtaddr;
+
+        size = PAGE_ALIGN(buf_size);
+        order = get_order(size);
+
+        while (size > 0) {
+                ClearPageReserved(virt_to_page(addr));
+                addr += PAGE_SIZE;
+                size -= PAGE_SIZE;
+        }
+        free_pages((unsigned long) virtaddr, order);
+}
diff --git a/drivers/media/video/omap/omap_voutlib.h b/drivers/media/video/omap/omap_voutlib.h
index a60b16e8bfc..e51750a597e 100644
--- a/drivers/media/video/omap/omap_voutlib.h
+++ b/drivers/media/video/omap/omap_voutlib.h
@@ -12,23 +12,25 @@
 #ifndef OMAP_VOUTLIB_H
 #define OMAP_VOUTLIB_H
 
-extern void omap_vout_default_crop(struct v4l2_pix_format *pix,
+void omap_vout_default_crop(struct v4l2_pix_format *pix,
                 struct v4l2_framebuffer *fbuf, struct v4l2_rect *crop);
 
-extern int omap_vout_new_crop(struct v4l2_pix_format *pix,
+int omap_vout_new_crop(struct v4l2_pix_format *pix,
                 struct v4l2_rect *crop, struct v4l2_window *win,
                 struct v4l2_framebuffer *fbuf,
                 const struct v4l2_rect *new_crop);
 
-extern int omap_vout_try_window(struct v4l2_framebuffer *fbuf,
+int omap_vout_try_window(struct v4l2_framebuffer *fbuf,
                 struct v4l2_window *new_win);
 
-extern int omap_vout_new_window(struct v4l2_rect *crop,
+int omap_vout_new_window(struct v4l2_rect *crop,
                 struct v4l2_window *win, struct v4l2_framebuffer *fbuf,
                 struct v4l2_window *new_win);
 
-extern void omap_vout_new_format(struct v4l2_pix_format *pix,
+void omap_vout_new_format(struct v4l2_pix_format *pix,
                 struct v4l2_framebuffer *fbuf, struct v4l2_rect *crop,
                 struct v4l2_window *win);
+unsigned long omap_vout_alloc_buffer(u32 buf_size, u32 *phys_addr);
+void omap_vout_free_buffer(unsigned long virtaddr, u32 buf_size);
 #endif  /* #ifndef OMAP_VOUTLIB_H */
 
diff --git a/drivers/media/video/omap1_camera.c b/drivers/media/video/omap1_camera.c
index e7cfc85b0a1..8a947e603ac 100644
--- a/drivers/media/video/omap1_camera.c
+++ b/drivers/media/video/omap1_camera.c
@@ -26,7 +26,6 @@
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
-#include <linux/version.h>
 
 #include <media/omap1_camera.h>
 #include <media/soc_camera.h>
@@ -38,7 +37,7 @@
 
 
 #define DRIVER_NAME             "omap1-camera"
-#define VERSION_CODE            KERNEL_VERSION(0, 0, 1)
+#define DRIVER_VERSION          "0.0.2"
 
 
 /*
@@ -208,7 +207,7 @@ static int omap1_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
         struct soc_camera_device *icd = vq->priv_data;
         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
                         icd->current_fmt->host_fmt);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct omap1_cam_dev *pcdev = ici->priv;
 
         if (bytes_per_line < 0)
@@ -222,7 +221,7 @@ static int omap1_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
         if (*size * *count > MAX_VIDEO_MEM * 1024 * 1024)
                 *count = (MAX_VIDEO_MEM * 1024 * 1024) / *size;
 
-        dev_dbg(icd->dev.parent,
+        dev_dbg(icd->parent,
                         "%s: count=%d, size=%d\n", __func__, *count, *size);
 
         return 0;
@@ -241,7 +240,7 @@ static void free_buffer(struct videobuf_queue *vq, struct omap1_cam_buf *buf,
                 videobuf_dma_contig_free(vq, vb);
         } else {
                 struct soc_camera_device *icd = vq->priv_data;
-                struct device *dev = icd->dev.parent;
+                struct device *dev = icd->parent;
                 struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
 
                 videobuf_dma_unmap(dev, dma);
@@ -258,7 +257,7 @@ static int omap1_videobuf_prepare(struct videobuf_queue *vq,
         struct omap1_cam_buf *buf = container_of(vb, struct omap1_cam_buf, vb);
         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
                         icd->current_fmt->host_fmt);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct omap1_cam_dev *pcdev = ici->priv;
         int ret;
 
@@ -490,7 +489,7 @@ static void omap1_videobuf_queue(struct videobuf_queue *vq,
                                                 struct videobuf_buffer *vb)
 {
         struct soc_camera_device *icd = vq->priv_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct omap1_cam_dev *pcdev = ici->priv;
         struct omap1_cam_buf *buf;
         u32 mode;
@@ -519,7 +518,7 @@ static void omap1_videobuf_queue(struct videobuf_queue *vq,
         pcdev->active = buf;
         pcdev->ready = NULL;
 
-        dev_dbg(icd->dev.parent,
+        dev_dbg(icd->parent,
                 "%s: capture not active, setup FIFO, start DMA\n", __func__);
         mode = CAM_READ_CACHE(pcdev, MODE) & ~THRESHOLD_MASK;
         mode |= THRESHOLD_LEVEL(pcdev->vb_mode) << THRESHOLD_SHIFT;
@@ -543,8 +542,8 @@ static void omap1_videobuf_release(struct videobuf_queue *vq,
         struct omap1_cam_buf *buf =
                         container_of(vb, struct omap1_cam_buf, vb);
         struct soc_camera_device *icd = vq->priv_data;
-        struct device *dev = icd->dev.parent;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct device *dev = icd->parent;
+        struct soc_camera_host *ici = to_soc_camera_host(dev);
         struct omap1_cam_dev *pcdev = ici->priv;
 
         switch (vb->state) {
@@ -573,7 +572,7 @@ static void videobuf_done(struct omap1_cam_dev *pcdev,
 {
         struct omap1_cam_buf *buf = pcdev->active;
         struct videobuf_buffer *vb;
-        struct device *dev = pcdev->icd->dev.parent;
+        struct device *dev = pcdev->icd->parent;
 
         if (WARN_ON(!buf)) {
                 suspend_capture(pcdev);
@@ -799,7 +798,7 @@ out:
 static irqreturn_t cam_isr(int irq, void *data)
 {
         struct omap1_cam_dev *pcdev = data;
-        struct device *dev = pcdev->icd->dev.parent;
+        struct device *dev = pcdev->icd->parent;
         struct omap1_cam_buf *buf = pcdev->active;
         u32 it_status;
         unsigned long flags;
@@ -909,7 +908,7 @@ static void sensor_reset(struct omap1_cam_dev *pcdev, bool reset)
  */
 static int omap1_cam_add_device(struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct omap1_cam_dev *pcdev = ici->priv;
         u32 ctrlclock;
 
@@ -952,14 +951,14 @@ static int omap1_cam_add_device(struct soc_camera_device *icd)
 
         pcdev->icd = icd;
 
-        dev_dbg(icd->dev.parent, "OMAP1 Camera driver attached to camera %d\n",
+        dev_dbg(icd->parent, "OMAP1 Camera driver attached to camera %d\n",
                         icd->devnum);
         return 0;
 }
 
 static void omap1_cam_remove_device(struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct omap1_cam_dev *pcdev = ici->priv;
         u32 ctrlclock;
 
@@ -985,7 +984,7 @@ static void omap1_cam_remove_device(struct soc_camera_device *icd)
 
         pcdev->icd = NULL;
 
-        dev_dbg(icd->dev.parent,
+        dev_dbg(icd->parent,
                 "OMAP1 Camera driver detached from camera %d\n", icd->devnum);
 }
 
@@ -1070,7 +1069,7 @@ static int omap1_cam_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);
-        struct device *dev = icd->dev.parent;
+        struct device *dev = icd->parent;
         int formats = 0, ret;
         enum v4l2_mbus_pixelcode code;
         const struct soc_mbus_pixelfmt *fmt;
@@ -1222,9 +1221,9 @@ static int omap1_cam_set_crop(struct soc_camera_device *icd,
         struct v4l2_rect *rect = &crop->c;
         const struct soc_camera_format_xlate *xlate = icd->current_fmt;
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct device *dev = icd->parent;
+        struct soc_camera_host *ici = to_soc_camera_host(dev);
         struct omap1_cam_dev *pcdev = ici->priv;
-        struct device *dev = icd->dev.parent;
         struct v4l2_mbus_framefmt mf;
         int ret;
 
@@ -1270,8 +1269,8 @@ static int omap1_cam_set_fmt(struct soc_camera_device *icd,
 {
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
         const struct soc_camera_format_xlate *xlate;
-        struct device *dev = icd->dev.parent;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct device *dev = icd->parent;
+        struct soc_camera_host *ici = to_soc_camera_host(dev);
         struct omap1_cam_dev *pcdev = ici->priv;
         struct v4l2_pix_format *pix = &f->fmt.pix;
         struct v4l2_mbus_framefmt mf;
@@ -1326,7 +1325,7 @@ static int omap1_cam_try_fmt(struct soc_camera_device *icd,
 
         xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
         if (!xlate) {
-                dev_warn(icd->dev.parent, "Format %#x not found\n",
+                dev_warn(icd->parent, "Format %#x not found\n",
                          pix->pixelformat);
                 return -EINVAL;
         }
@@ -1362,7 +1361,7 @@ static int omap1_cam_mmap_mapper(struct videobuf_queue *q,
                                   struct vm_area_struct *vma)
 {
         struct soc_camera_device *icd = q->priv_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct omap1_cam_dev *pcdev = ici->priv;
         int ret;
 
@@ -1377,17 +1376,17 @@ static int omap1_cam_mmap_mapper(struct videobuf_queue *q,
 static void omap1_cam_init_videobuf(struct videobuf_queue *q,
                                      struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct omap1_cam_dev *pcdev = ici->priv;
 
         if (!sg_mode)
                 videobuf_queue_dma_contig_init(q, &omap1_videobuf_ops,
-                                icd->dev.parent, &pcdev->lock,
+                                icd->parent, &pcdev->lock,
                                 V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_NONE,
                                 sizeof(struct omap1_cam_buf), icd, &icd->video_lock);
         else
                 videobuf_queue_sg_init(q, &omap1_videobuf_ops,
-                                icd->dev.parent, &pcdev->lock,
+                                icd->parent, &pcdev->lock,
                                 V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_NONE,
                                 sizeof(struct omap1_cam_buf), icd, &icd->video_lock);
 
@@ -1431,7 +1430,6 @@ static int omap1_cam_querycap(struct soc_camera_host *ici,
 {
         /* cap->name is set by the friendly caller:-> */
         strlcpy(cap->card, "OMAP1 Camera", sizeof(cap->card));
-        cap->version = VERSION_CODE;
         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 
         return 0;
@@ -1440,9 +1438,9 @@ static int omap1_cam_querycap(struct soc_camera_host *ici,
 static int omap1_cam_set_bus_param(struct soc_camera_device *icd,
                 __u32 pixfmt)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct device *dev = icd->parent;
+        struct soc_camera_host *ici = to_soc_camera_host(dev);
         struct omap1_cam_dev *pcdev = ici->priv;
-        struct device *dev = icd->dev.parent;
         const struct soc_camera_format_xlate *xlate;
         const struct soc_mbus_pixelfmt *fmt;
         unsigned long camera_flags, common_flags;
@@ -1718,4 +1716,5 @@ MODULE_PARM_DESC(sg_mode, "videobuf mode, 0: dma-contig (default), 1: dma-sg");
 MODULE_DESCRIPTION("OMAP1 Camera Interface driver");
 MODULE_AUTHOR("Janusz Krzysztofik <jkrzyszt@tis.icnet.pl>");
 MODULE_LICENSE("GPL v2");
+MODULE_LICENSE(DRIVER_VERSION);
 MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/media/video/omap24xxcam.c b/drivers/media/video/omap24xxcam.c
index 69b60ba5dd7..eb97bff7116 100644
--- a/drivers/media/video/omap24xxcam.c
+++ b/drivers/media/video/omap24xxcam.c
@@ -31,7 +31,6 @@
 #include <linux/interrupt.h>
 #include <linux/videodev2.h>
 #include <linux/pci.h>          /* needed for videobufs */
-#include <linux/version.h>
 #include <linux/platform_device.h>
 #include <linux/clk.h>
 #include <linux/io.h>
@@ -43,7 +42,7 @@
 
 #include "omap24xxcam.h"
 
-#define OMAP24XXCAM_VERSION KERNEL_VERSION(0, 0, 0)
+#define OMAP24XXCAM_VERSION "0.0.1"
 
 #define RESET_TIMEOUT_NS 10000
 
@@ -309,11 +308,11 @@ static int omap24xxcam_vbq_alloc_mmap_buffer(struct videobuf_buffer *vb)
                         order--;
 
                 /* try to allocate as many contiguous pages as possible */
-                page = alloc_pages(GFP_KERNEL | GFP_DMA, order);
+                page = alloc_pages(GFP_KERNEL, order);
                 /* if allocation fails, try to allocate smaller amount */
                 while (page == NULL) {
                         order--;
-                        page = alloc_pages(GFP_KERNEL | GFP_DMA, order);
+                        page = alloc_pages(GFP_KERNEL, order);
                         if (page == NULL && !order) {
                                 err = -ENOMEM;
                                 goto out;
@@ -993,7 +992,6 @@ static int vidioc_querycap(struct file *file, void *fh,
 
         strlcpy(cap->driver, CAM_NAME, sizeof(cap->driver));
         strlcpy(cap->card, cam->vfd->name, sizeof(cap->card));
-        cap->version = OMAP24XXCAM_VERSION;
         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 
         return 0;
@@ -1888,6 +1886,7 @@ static void __exit omap24xxcam_cleanup(void)
 MODULE_AUTHOR("Sakari Ailus <sakari.ailus@nokia.com>");
 MODULE_DESCRIPTION("OMAP24xx Video for Linux camera driver");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(OMAP24XXCAM_VERSION);
 module_param(video_nr, int, 0);
 MODULE_PARM_DESC(video_nr,
                  "Minor number for video device (-1 ==> auto assign)");
diff --git a/drivers/media/video/omap3isp/isp.c b/drivers/media/video/omap3isp/isp.c
index 94b6ed89e19..5cea2bbd701 100644
--- a/drivers/media/video/omap3isp/isp.c
+++ b/drivers/media/video/omap3isp/isp.c
@@ -2234,3 +2234,4 @@ module_exit(isp_cleanup);
 MODULE_AUTHOR("Nokia Corporation");
 MODULE_DESCRIPTION("TI OMAP3 ISP driver");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(ISP_VIDEO_DRIVER_VERSION);
diff --git a/drivers/media/video/omap3isp/isp.h b/drivers/media/video/omap3isp/isp.h
index 2620c405f5e..529e582ef94 100644
--- a/drivers/media/video/omap3isp/isp.h
+++ b/drivers/media/video/omap3isp/isp.h
@@ -139,6 +139,10 @@ struct isp_reg {
  *              3 - CAMEXT[13:6] -> CAM[7:0]
  * @clk_pol: Pixel clock polarity
  *              0 - Non Inverted, 1 - Inverted
+ * @hs_pol: Horizontal synchronization polarity
+ *              0 - Active high, 1 - Active low
+ * @vs_pol: Vertical synchronization polarity
+ *              0 - Active high, 1 - Active low
  * @bridge: CCDC Bridge input control
  *              ISPCTRL_PAR_BRIDGE_DISABLE - Disable
  *              ISPCTRL_PAR_BRIDGE_LENDIAN - Little endian
@@ -147,6 +151,8 @@ struct isp_reg {
 struct isp_parallel_platform_data {
         unsigned int data_lane_shift:2;
         unsigned int clk_pol:1;
+        unsigned int hs_pol:1;
+        unsigned int vs_pol:1;
         unsigned int bridge:4;
 };
 
diff --git a/drivers/media/video/omap3isp/ispccdc.c b/drivers/media/video/omap3isp/ispccdc.c
index 39d501bda63..9d3459de04b 100644
--- a/drivers/media/video/omap3isp/ispccdc.c
+++ b/drivers/media/video/omap3isp/ispccdc.c
@@ -1148,6 +1148,8 @@ static void ccdc_configure(struct isp_ccdc_device *ccdc)
         omap3isp_configure_bridge(isp, ccdc->input, pdata, shift);
 
         ccdc->syncif.datsz = depth_out;
+        ccdc->syncif.hdpol = pdata ? pdata->hs_pol : 0;
+        ccdc->syncif.vdpol = pdata ? pdata->vs_pol : 0;
         ccdc_config_sync_if(ccdc, &ccdc->syncif);
 
         /* CCDC_PAD_SINK */
@@ -1691,7 +1693,7 @@ static int ccdc_subscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
         if (sub->type != V4L2_EVENT_OMAP3ISP_HS_VS)
                 return -EINVAL;
 
-        return v4l2_event_subscribe(fh, sub);
+        return v4l2_event_subscribe(fh, sub, OMAP3ISP_CCDC_NEVENTS);
 }
 
 static int ccdc_unsubscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
@@ -2162,7 +2164,6 @@ static int ccdc_init_entities(struct isp_ccdc_device *ccdc)
         sd->grp_id = 1 << 16;   /* group ID for isp subdevs */
         v4l2_set_subdevdata(sd, ccdc);
         sd->flags |= V4L2_SUBDEV_FL_HAS_EVENTS | V4L2_SUBDEV_FL_HAS_DEVNODE;
-        sd->nevents = OMAP3ISP_CCDC_NEVENTS;
 
         pads[CCDC_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
         pads[CCDC_PAD_SOURCE_VP].flags = MEDIA_PAD_FL_SOURCE;
@@ -2257,8 +2258,6 @@ int omap3isp_ccdc_init(struct isp_device *isp)
         ccdc->syncif.fldout = 0;
         ccdc->syncif.fldpol = 0;
         ccdc->syncif.fldstat = 0;
-        ccdc->syncif.hdpol = 0;
-        ccdc->syncif.vdpol = 0;
 
         ccdc->clamp.oblen = 0;
         ccdc->clamp.dcsubval = 0;
diff --git a/drivers/media/video/omap3isp/ispccp2.c b/drivers/media/video/omap3isp/ispccp2.c
index 0e16cab8e08..ec9e395f333 100644
--- a/drivers/media/video/omap3isp/ispccp2.c
+++ b/drivers/media/video/omap3isp/ispccp2.c
@@ -30,6 +30,7 @@
 #include <linux/module.h>
 #include <linux/mutex.h>
 #include <linux/uaccess.h>
+#include <linux/regulator/consumer.h>
 
 #include "isp.h"
 #include "ispreg.h"
@@ -163,6 +164,9 @@ static void ccp2_if_enable(struct isp_ccp2_device *ccp2, u8 enable)
         struct isp_pipeline *pipe = to_isp_pipeline(&ccp2->subdev.entity);
         int i;
 
+        if (enable && ccp2->vdds_csib)
+                regulator_enable(ccp2->vdds_csib);
+
         /* Enable/Disable all the LCx channels */
         for (i = 0; i < CCP2_LCx_CHANS_NUM; i++)
                 isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_LCx_CTRL(i),
@@ -186,6 +190,9 @@ static void ccp2_if_enable(struct isp_ccp2_device *ccp2, u8 enable)
                                     ISPCCP2_LC01_IRQENABLE,
                                     ISPCCP2_LC01_IRQSTATUS_LC0_FS_IRQ);
         }
+
+        if (!enable && ccp2->vdds_csib)
+                regulator_disable(ccp2->vdds_csib);
 }
 
 /*
@@ -1137,6 +1144,9 @@ error:
  */
 void omap3isp_ccp2_cleanup(struct isp_device *isp)
 {
+        struct isp_ccp2_device *ccp2 = &isp->isp_ccp2;
+
+        regulator_put(ccp2->vdds_csib);
 }
 
 /*
@@ -1151,14 +1161,27 @@ int omap3isp_ccp2_init(struct isp_device *isp)
 
         init_waitqueue_head(&ccp2->wait);
 
-        /* On the OMAP36xx, the CCP2 uses the CSI PHY1 or PHY2, shared with
+        /*
+         * On the OMAP34xx the CSI1 receiver is operated in the CSIb IO
+         * complex, which is powered by vdds_csib power rail. Hence the
+         * request for the regulator.
+         *
+         * On the OMAP36xx, the CCP2 uses the CSI PHY1 or PHY2, shared with
          * the CSI2c or CSI2a receivers. The PHY then needs to be explicitly
          * configured.
          *
          * TODO: Don't hardcode the usage of PHY1 (shared with CSI2c).
          */
-        if (isp->revision == ISP_REVISION_15_0)
+        if (isp->revision == ISP_REVISION_2_0) {
+                ccp2->vdds_csib = regulator_get(isp->dev, "vdds_csib");
+                if (IS_ERR(ccp2->vdds_csib)) {
+                        dev_dbg(isp->dev,
+                                "Could not get regulator vdds_csib\n");
+                        ccp2->vdds_csib = NULL;
+                }
+        } else if (isp->revision == ISP_REVISION_15_0) {
                 ccp2->phy = &isp->isp_csiphy1;
+        }
 
         ret = ccp2_init_entities(ccp2);
         if (ret < 0)
diff --git a/drivers/media/video/omap3isp/ispccp2.h b/drivers/media/video/omap3isp/ispccp2.h
index 5505a86a9a7..6674e9de2cd 100644
--- a/drivers/media/video/omap3isp/ispccp2.h
+++ b/drivers/media/video/omap3isp/ispccp2.h
@@ -81,6 +81,7 @@ struct isp_ccp2_device {
         struct isp_interface_mem_config mem_cfg;
         struct isp_video video_in;
         struct isp_csiphy *phy;
+        struct regulator *vdds_csib;
         unsigned int error;
         enum isp_pipeline_stream_state state;
         wait_queue_head_t wait;
diff --git a/drivers/media/video/omap3isp/ispstat.c b/drivers/media/video/omap3isp/ispstat.c
index b44cb685236..808065948ac 100644
--- a/drivers/media/video/omap3isp/ispstat.c
+++ b/drivers/media/video/omap3isp/ispstat.c
@@ -1032,7 +1032,6 @@ static int isp_stat_init_entities(struct ispstat *stat, const char *name,
         snprintf(subdev->name, V4L2_SUBDEV_NAME_SIZE, "OMAP3 ISP %s", name);
         subdev->grp_id = 1 << 16;       /* group ID for isp subdevs */
         subdev->flags |= V4L2_SUBDEV_FL_HAS_EVENTS | V4L2_SUBDEV_FL_HAS_DEVNODE;
-        subdev->nevents = STAT_NEVENTS;
         v4l2_set_subdevdata(subdev, stat);
 
         stat->pad.flags = MEDIA_PAD_FL_SINK;
@@ -1050,7 +1049,7 @@ int omap3isp_stat_subscribe_event(struct v4l2_subdev *subdev,
         if (sub->type != stat->event_type)
                 return -EINVAL;
 
-        return v4l2_event_subscribe(fh, sub);
+        return v4l2_event_subscribe(fh, sub, STAT_NEVENTS);
 }
 
 int omap3isp_stat_unsubscribe_event(struct v4l2_subdev *subdev,
diff --git a/drivers/media/video/omap3isp/ispvideo.c b/drivers/media/video/omap3isp/ispvideo.c
index 9cd8f1aa567..fd965adfd59 100644
--- a/drivers/media/video/omap3isp/ispvideo.c
+++ b/drivers/media/video/omap3isp/ispvideo.c
@@ -695,7 +695,6 @@ isp_video_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
         strlcpy(cap->driver, ISP_VIDEO_DRIVER_NAME, sizeof(cap->driver));
         strlcpy(cap->card, video->video.name, sizeof(cap->card));
         strlcpy(cap->bus_info, "media", sizeof(cap->bus_info));
-        cap->version = ISP_VIDEO_DRIVER_VERSION;
 
         if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
                 cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
diff --git a/drivers/media/video/omap3isp/ispvideo.h b/drivers/media/video/omap3isp/ispvideo.h
index 911bea64e78..53160aa24e6 100644
--- a/drivers/media/video/omap3isp/ispvideo.h
+++ b/drivers/media/video/omap3isp/ispvideo.h
@@ -27,7 +27,6 @@
 #define OMAP3_ISP_VIDEO_H
 
 #include <linux/v4l2-mediabus.h>
-#include <linux/version.h>
 #include <media/media-entity.h>
 #include <media/v4l2-dev.h>
 #include <media/v4l2-fh.h>
@@ -35,7 +34,7 @@
 #include "ispqueue.h"
 
 #define ISP_VIDEO_DRIVER_NAME           "ispvideo"
-#define ISP_VIDEO_DRIVER_VERSION        KERNEL_VERSION(0, 0, 1)
+#define ISP_VIDEO_DRIVER_VERSION        "0.0.2"
 
 struct isp_device;
 struct isp_video;
diff --git a/drivers/media/video/ov2640.c b/drivers/media/video/ov2640.c
index 0cea0cf3667..9ce2fa037b9 100644
--- a/drivers/media/video/ov2640.c
+++ b/drivers/media/video/ov2640.c
@@ -1031,16 +1031,9 @@ static int ov2640_video_probe(struct soc_camera_device *icd,
         const char *devname;
         int ret;
 
-        /*
-         * we must have a parent by now. And it cannot be a wrong one.
-         * So this entire test is completely redundant.
-         */
-        if (!icd->dev.parent ||
-            to_soc_camera_host(icd->dev.parent)->nr != icd->iface) {
-                dev_err(&client->dev, "Parent missing or invalid!\n");
-                ret = -ENODEV;
-                goto err;
-        }
+        /* We must have a parent by now. And it cannot be a wrong one. */
+        BUG_ON(!icd->parent ||
+               to_soc_camera_host(icd->parent)->nr != icd->iface);
 
         /*
          * check and show product ID and manufacturer ID
diff --git a/drivers/media/video/ov5642.c b/drivers/media/video/ov5642.c
new file mode 100644
index 00000000000..349a4ad3ccc
--- /dev/null
+++ b/drivers/media/video/ov5642.c
@@ -0,0 +1,1012 @@
+/*
+ * Driver for OV5642 CMOS Image Sensor from Omnivision
+ *
+ * Copyright (C) 2011, Bastian Hecht <hechtb@gmail.com>
+ *
+ * Based on Sony IMX074 Camera Driver
+ * Copyright (C) 2010, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ *
+ * Based on Omnivision OV7670 Camera Driver
+ * Copyright (C) 2006-7 Jonathan Corbet <corbet@lwn.net>
+ *
+ * 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/delay.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/module.h>
+
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-subdev.h>
+
+/* OV5642 registers */
+#define REG_CHIP_ID_HIGH                0x300a
+#define REG_CHIP_ID_LOW                 0x300b
+
+#define REG_WINDOW_START_X_HIGH         0x3800
+#define REG_WINDOW_START_X_LOW          0x3801
+#define REG_WINDOW_START_Y_HIGH         0x3802
+#define REG_WINDOW_START_Y_LOW          0x3803
+#define REG_WINDOW_WIDTH_HIGH           0x3804
+#define REG_WINDOW_WIDTH_LOW            0x3805
+#define REG_WINDOW_HEIGHT_HIGH          0x3806
+#define REG_WINDOW_HEIGHT_LOW           0x3807
+#define REG_OUT_WIDTH_HIGH              0x3808
+#define REG_OUT_WIDTH_LOW               0x3809
+#define REG_OUT_HEIGHT_HIGH             0x380a
+#define REG_OUT_HEIGHT_LOW              0x380b
+#define REG_OUT_TOTAL_WIDTH_HIGH        0x380c
+#define REG_OUT_TOTAL_WIDTH_LOW         0x380d
+#define REG_OUT_TOTAL_HEIGHT_HIGH       0x380e
+#define REG_OUT_TOTAL_HEIGHT_LOW        0x380f
+
+/*
+ * define standard resolution.
+ * Works currently only for up to 720 lines
+ * eg. 320x240, 640x480, 800x600, 1280x720, 2048x720
+ */
+
+#define OV5642_WIDTH            1280
+#define OV5642_HEIGHT           720
+#define OV5642_TOTAL_WIDTH      3200
+#define OV5642_TOTAL_HEIGHT     2000
+#define OV5642_SENSOR_SIZE_X    2592
+#define OV5642_SENSOR_SIZE_Y    1944
+
+struct regval_list {
+        u16 reg_num;
+        u8 value;
+};
+
+static struct regval_list ov5642_default_regs_init[] = {
+        { 0x3103, 0x93 },
+        { 0x3008, 0x82 },
+        { 0x3017, 0x7f },
+        { 0x3018, 0xfc },
+        { 0x3810, 0xc2 },
+        { 0x3615, 0xf0 },
+        { 0x3000, 0x0  },
+        { 0x3001, 0x0  },
+        { 0x3002, 0x0  },
+        { 0x3003, 0x0  },
+        { 0x3004, 0xff },
+        { 0x3030, 0x2b },
+        { 0x3011, 0x8  },
+        { 0x3010, 0x10 },
+        { 0x3604, 0x60 },
+        { 0x3622, 0x60 },
+        { 0x3621, 0x9  },
+        { 0x3709, 0x0  },
+        { 0x4000, 0x21 },
+        { 0x401d, 0x22 },
+        { 0x3600, 0x54 },
+        { 0x3605, 0x4  },
+        { 0x3606, 0x3f },
+        { 0x3c01, 0x80 },
+        { 0x300d, 0x22 },
+        { 0x3623, 0x22 },
+        { 0x5000, 0x4f },
+        { 0x5020, 0x4  },
+        { 0x5181, 0x79 },
+        { 0x5182, 0x0  },
+        { 0x5185, 0x22 },
+        { 0x5197, 0x1  },
+        { 0x5500, 0xa  },
+        { 0x5504, 0x0  },
+        { 0x5505, 0x7f },
+        { 0x5080, 0x8  },
+        { 0x300e, 0x18 },
+        { 0x4610, 0x0  },
+        { 0x471d, 0x5  },
+        { 0x4708, 0x6  },
+        { 0x370c, 0xa0 },
+        { 0x5687, 0x94 },
+        { 0x501f, 0x0  },
+        { 0x5000, 0x4f },
+        { 0x5001, 0xcf },
+        { 0x4300, 0x30 },
+        { 0x4300, 0x30 },
+        { 0x460b, 0x35 },
+        { 0x471d, 0x0  },
+        { 0x3002, 0xc  },
+        { 0x3002, 0x0  },
+        { 0x4713, 0x3  },
+        { 0x471c, 0x50 },
+        { 0x4721, 0x2  },
+        { 0x4402, 0x90 },
+        { 0x460c, 0x22 },
+        { 0x3815, 0x44 },
+        { 0x3503, 0x7  },
+        { 0x3501, 0x73 },
+        { 0x3502, 0x80 },
+        { 0x350b, 0x0  },
+        { 0x3818, 0xc8 },
+        { 0x3824, 0x11 },
+        { 0x3a00, 0x78 },
+        { 0x3a1a, 0x4  },
+        { 0x3a13, 0x30 },
+        { 0x3a18, 0x0  },
+        { 0x3a19, 0x7c },
+        { 0x3a08, 0x12 },
+        { 0x3a09, 0xc0 },
+        { 0x3a0a, 0xf  },
+        { 0x3a0b, 0xa0 },
+        { 0x350c, 0x7  },
+        { 0x350d, 0xd0 },
+        { 0x3a0d, 0x8  },
+        { 0x3a0e, 0x6  },
+        { 0x3500, 0x0  },
+        { 0x3501, 0x0  },
+        { 0x3502, 0x0  },
+        { 0x350a, 0x0  },
+        { 0x350b, 0x0  },
+        { 0x3503, 0x0  },
+        { 0x3a0f, 0x3c },
+        { 0x3a10, 0x32 },
+        { 0x3a1b, 0x3c },
+        { 0x3a1e, 0x32 },
+        { 0x3a11, 0x80 },
+        { 0x3a1f, 0x20 },
+        { 0x3030, 0x2b },
+        { 0x3a02, 0x0  },
+        { 0x3a03, 0x7d },
+        { 0x3a04, 0x0  },
+        { 0x3a14, 0x0  },
+        { 0x3a15, 0x7d },
+        { 0x3a16, 0x0  },
+        { 0x3a00, 0x78 },
+        { 0x3a08, 0x9  },
+        { 0x3a09, 0x60 },
+        { 0x3a0a, 0x7  },
+        { 0x3a0b, 0xd0 },
+        { 0x3a0d, 0x10 },
+        { 0x3a0e, 0xd  },
+        { 0x4407, 0x4  },
+        { 0x5193, 0x70 },
+        { 0x589b, 0x0  },
+        { 0x589a, 0xc0 },
+        { 0x401e, 0x20 },
+        { 0x4001, 0x42 },
+        { 0x401c, 0x6  },
+        { 0x3825, 0xac },
+        { 0x3827, 0xc  },
+        { 0x528a, 0x1  },
+        { 0x528b, 0x4  },
+        { 0x528c, 0x8  },
+        { 0x528d, 0x10 },
+        { 0x528e, 0x20 },
+        { 0x528f, 0x28 },
+        { 0x5290, 0x30 },
+        { 0x5292, 0x0  },
+        { 0x5293, 0x1  },
+        { 0x5294, 0x0  },
+        { 0x5295, 0x4  },
+        { 0x5296, 0x0  },
+        { 0x5297, 0x8  },
+        { 0x5298, 0x0  },
+        { 0x5299, 0x10 },
+        { 0x529a, 0x0  },
+        { 0x529b, 0x20 },
+        { 0x529c, 0x0  },
+        { 0x529d, 0x28 },
+        { 0x529e, 0x0  },
+        { 0x529f, 0x30 },
+        { 0x5282, 0x0  },
+        { 0x5300, 0x0  },
+        { 0x5301, 0x20 },
+        { 0x5302, 0x0  },
+        { 0x5303, 0x7c },
+        { 0x530c, 0x0  },
+        { 0x530d, 0xc  },
+        { 0x530e, 0x20 },
+        { 0x530f, 0x80 },
+        { 0x5310, 0x20 },
+        { 0x5311, 0x80 },
+        { 0x5308, 0x20 },
+        { 0x5309, 0x40 },
+        { 0x5304, 0x0  },
+        { 0x5305, 0x30 },
+        { 0x5306, 0x0  },
+        { 0x5307, 0x80 },
+        { 0x5314, 0x8  },
+        { 0x5315, 0x20 },
+        { 0x5319, 0x30 },
+        { 0x5316, 0x10 },
+        { 0x5317, 0x0  },
+        { 0x5318, 0x2  },
+        { 0x5380, 0x1  },
+        { 0x5381, 0x0  },
+        { 0x5382, 0x0  },
+        { 0x5383, 0x4e },
+        { 0x5384, 0x0  },
+        { 0x5385, 0xf  },
+        { 0x5386, 0x0  },
+        { 0x5387, 0x0  },
+        { 0x5388, 0x1  },
+        { 0x5389, 0x15 },
+        { 0x538a, 0x0  },
+        { 0x538b, 0x31 },
+        { 0x538c, 0x0  },
+        { 0x538d, 0x0  },
+        { 0x538e, 0x0  },
+        { 0x538f, 0xf  },
+        { 0x5390, 0x0  },
+        { 0x5391, 0xab },
+        { 0x5392, 0x0  },
+        { 0x5393, 0xa2 },
+        { 0x5394, 0x8  },
+        { 0x5480, 0x14 },
+        { 0x5481, 0x21 },
+        { 0x5482, 0x36 },
+        { 0x5483, 0x57 },
+        { 0x5484, 0x65 },
+        { 0x5485, 0x71 },
+        { 0x5486, 0x7d },
+        { 0x5487, 0x87 },
+        { 0x5488, 0x91 },
+        { 0x5489, 0x9a },
+        { 0x548a, 0xaa },
+        { 0x548b, 0xb8 },
+        { 0x548c, 0xcd },
+        { 0x548d, 0xdd },
+        { 0x548e, 0xea },
+        { 0x548f, 0x1d },
+        { 0x5490, 0x5  },
+        { 0x5491, 0x0  },
+        { 0x5492, 0x4  },
+        { 0x5493, 0x20 },
+        { 0x5494, 0x3  },
+        { 0x5495, 0x60 },
+        { 0x5496, 0x2  },
+        { 0x5497, 0xb8 },
+        { 0x5498, 0x2  },
+        { 0x5499, 0x86 },
+        { 0x549a, 0x2  },
+        { 0x549b, 0x5b },
+        { 0x549c, 0x2  },
+        { 0x549d, 0x3b },
+        { 0x549e, 0x2  },
+        { 0x549f, 0x1c },
+        { 0x54a0, 0x2  },
+        { 0x54a1, 0x4  },
+        { 0x54a2, 0x1  },
+        { 0x54a3, 0xed },
+        { 0x54a4, 0x1  },
+        { 0x54a5, 0xc5 },
+        { 0x54a6, 0x1  },
+        { 0x54a7, 0xa5 },
+        { 0x54a8, 0x1  },
+        { 0x54a9, 0x6c },
+        { 0x54aa, 0x1  },
+        { 0x54ab, 0x41 },
+        { 0x54ac, 0x1  },
+        { 0x54ad, 0x20 },
+        { 0x54ae, 0x0  },
+        { 0x54af, 0x16 },
+        { 0x54b0, 0x1  },
+        { 0x54b1, 0x20 },
+        { 0x54b2, 0x0  },
+        { 0x54b3, 0x10 },
+        { 0x54b4, 0x0  },
+        { 0x54b5, 0xf0 },
+        { 0x54b6, 0x0  },
+        { 0x54b7, 0xdf },
+        { 0x5402, 0x3f },
+        { 0x5403, 0x0  },
+        { 0x3406, 0x0  },
+        { 0x5180, 0xff },
+        { 0x5181, 0x52 },
+        { 0x5182, 0x11 },
+        { 0x5183, 0x14 },
+        { 0x5184, 0x25 },
+        { 0x5185, 0x24 },
+        { 0x5186, 0x6  },
+        { 0x5187, 0x8  },
+        { 0x5188, 0x8  },
+        { 0x5189, 0x7c },
+        { 0x518a, 0x60 },
+        { 0x518b, 0xb2 },
+        { 0x518c, 0xb2 },
+        { 0x518d, 0x44 },
+        { 0x518e, 0x3d },
+        { 0x518f, 0x58 },
+        { 0x5190, 0x46 },
+        { 0x5191, 0xf8 },
+        { 0x5192, 0x4  },
+        { 0x5193, 0x70 },
+        { 0x5194, 0xf0 },
+        { 0x5195, 0xf0 },
+        { 0x5196, 0x3  },
+        { 0x5197, 0x1  },
+        { 0x5198, 0x4  },
+        { 0x5199, 0x12 },
+        { 0x519a, 0x4  },
+        { 0x519b, 0x0  },
+        { 0x519c, 0x6  },
+        { 0x519d, 0x82 },
+        { 0x519e, 0x0  },
+        { 0x5025, 0x80 },
+        { 0x3a0f, 0x38 },
+        { 0x3a10, 0x30 },
+        { 0x3a1b, 0x3a },
+        { 0x3a1e, 0x2e },
+        { 0x3a11, 0x60 },
+        { 0x3a1f, 0x10 },
+        { 0x5688, 0xa6 },
+        { 0x5689, 0x6a },
+        { 0x568a, 0xea },
+        { 0x568b, 0xae },
+        { 0x568c, 0xa6 },
+        { 0x568d, 0x6a },
+        { 0x568e, 0x62 },
+        { 0x568f, 0x26 },
+        { 0x5583, 0x40 },
+        { 0x5584, 0x40 },
+        { 0x5580, 0x2  },
+        { 0x5000, 0xcf },
+        { 0x5800, 0x27 },
+        { 0x5801, 0x19 },
+        { 0x5802, 0x12 },
+        { 0x5803, 0xf  },
+        { 0x5804, 0x10 },
+        { 0x5805, 0x15 },
+        { 0x5806, 0x1e },
+        { 0x5807, 0x2f },
+        { 0x5808, 0x15 },
+        { 0x5809, 0xd  },
+        { 0x580a, 0xa  },
+        { 0x580b, 0x9  },
+        { 0x580c, 0xa  },
+        { 0x580d, 0xc  },
+        { 0x580e, 0x12 },
+        { 0x580f, 0x19 },
+        { 0x5810, 0xb  },
+        { 0x5811, 0x7  },
+        { 0x5812, 0x4  },
+        { 0x5813, 0x3  },
+        { 0x5814, 0x3  },
+        { 0x5815, 0x6  },
+        { 0x5816, 0xa  },
+        { 0x5817, 0xf  },
+        { 0x5818, 0xa  },
+        { 0x5819, 0x5  },
+        { 0x581a, 0x1  },
+        { 0x581b, 0x0  },
+        { 0x581c, 0x0  },
+        { 0x581d, 0x3  },
+        { 0x581e, 0x8  },
+        { 0x581f, 0xc  },
+        { 0x5820, 0xa  },
+        { 0x5821, 0x5  },
+        { 0x5822, 0x1  },
+        { 0x5823, 0x0  },
+        { 0x5824, 0x0  },
+        { 0x5825, 0x3  },
+        { 0x5826, 0x8  },
+        { 0x5827, 0xc  },
+        { 0x5828, 0xe  },
+        { 0x5829, 0x8  },
+        { 0x582a, 0x6  },
+        { 0x582b, 0x4  },
+        { 0x582c, 0x5  },
+        { 0x582d, 0x7  },
+        { 0x582e, 0xb  },
+        { 0x582f, 0x12 },
+        { 0x5830, 0x18 },
+        { 0x5831, 0x10 },
+        { 0x5832, 0xc  },
+        { 0x5833, 0xa  },
+        { 0x5834, 0xb  },
+        { 0x5835, 0xe  },
+        { 0x5836, 0x15 },
+        { 0x5837, 0x19 },
+        { 0x5838, 0x32 },
+        { 0x5839, 0x1f },
+        { 0x583a, 0x18 },
+        { 0x583b, 0x16 },
+        { 0x583c, 0x17 },
+        { 0x583d, 0x1e },
+        { 0x583e, 0x26 },
+        { 0x583f, 0x53 },
+        { 0x5840, 0x10 },
+        { 0x5841, 0xf  },
+        { 0x5842, 0xd  },
+        { 0x5843, 0xc  },
+        { 0x5844, 0xe  },
+        { 0x5845, 0x9  },
+        { 0x5846, 0x11 },
+        { 0x5847, 0x10 },
+        { 0x5848, 0x10 },
+        { 0x5849, 0x10 },
+        { 0x584a, 0x10 },
+        { 0x584b, 0xe  },
+        { 0x584c, 0x10 },
+        { 0x584d, 0x10 },
+        { 0x584e, 0x11 },
+        { 0x584f, 0x10 },
+        { 0x5850, 0xf  },
+        { 0x5851, 0xc  },
+        { 0x5852, 0xf  },
+        { 0x5853, 0x10 },
+        { 0x5854, 0x10 },
+        { 0x5855, 0xf  },
+        { 0x5856, 0xe  },
+        { 0x5857, 0xb  },
+        { 0x5858, 0x10 },
+        { 0x5859, 0xd  },
+        { 0x585a, 0xd  },
+        { 0x585b, 0xc  },
+        { 0x585c, 0xc  },
+        { 0x585d, 0xc  },
+        { 0x585e, 0xb  },
+        { 0x585f, 0xc  },
+        { 0x5860, 0xc  },
+        { 0x5861, 0xc  },
+        { 0x5862, 0xd  },
+        { 0x5863, 0x8  },
+        { 0x5864, 0x11 },
+        { 0x5865, 0x18 },
+        { 0x5866, 0x18 },
+        { 0x5867, 0x19 },
+        { 0x5868, 0x17 },
+        { 0x5869, 0x19 },
+        { 0x586a, 0x16 },
+        { 0x586b, 0x13 },
+        { 0x586c, 0x13 },
+        { 0x586d, 0x12 },
+        { 0x586e, 0x13 },
+        { 0x586f, 0x16 },
+        { 0x5870, 0x14 },
+        { 0x5871, 0x12 },
+        { 0x5872, 0x10 },
+        { 0x5873, 0x11 },
+        { 0x5874, 0x11 },
+        { 0x5875, 0x16 },
+        { 0x5876, 0x14 },
+        { 0x5877, 0x11 },
+        { 0x5878, 0x10 },
+        { 0x5879, 0xf  },
+        { 0x587a, 0x10 },
+        { 0x587b, 0x14 },
+        { 0x587c, 0x13 },
+        { 0x587d, 0x12 },
+        { 0x587e, 0x11 },
+        { 0x587f, 0x11 },
+        { 0x5880, 0x12 },
+        { 0x5881, 0x15 },
+        { 0x5882, 0x14 },
+        { 0x5883, 0x15 },
+        { 0x5884, 0x15 },
+        { 0x5885, 0x15 },
+        { 0x5886, 0x13 },
+        { 0x5887, 0x17 },
+        { 0x3710, 0x10 },
+        { 0x3632, 0x51 },
+        { 0x3702, 0x10 },
+        { 0x3703, 0xb2 },
+        { 0x3704, 0x18 },
+        { 0x370b, 0x40 },
+        { 0x370d, 0x3  },
+        { 0x3631, 0x1  },
+        { 0x3632, 0x52 },
+        { 0x3606, 0x24 },
+        { 0x3620, 0x96 },
+        { 0x5785, 0x7  },
+        { 0x3a13, 0x30 },
+        { 0x3600, 0x52 },
+        { 0x3604, 0x48 },
+        { 0x3606, 0x1b },
+        { 0x370d, 0xb  },
+        { 0x370f, 0xc0 },
+        { 0x3709, 0x1  },
+        { 0x3823, 0x0  },
+        { 0x5007, 0x0  },
+        { 0x5009, 0x0  },
+        { 0x5011, 0x0  },
+        { 0x5013, 0x0  },
+        { 0x519e, 0x0  },
+        { 0x5086, 0x0  },
+        { 0x5087, 0x0  },
+        { 0x5088, 0x0  },
+        { 0x5089, 0x0  },
+        { 0x302b, 0x0  },
+        { 0x3503, 0x7  },
+        { 0x3011, 0x8  },
+        { 0x350c, 0x2  },
+        { 0x350d, 0xe4 },
+        { 0x3621, 0xc9 },
+        { 0x370a, 0x81 },
+        { 0xffff, 0xff },
+};
+
+static struct regval_list ov5642_default_regs_finalise[] = {
+        { 0x3810, 0xc2 },
+        { 0x3818, 0xc9 },
+        { 0x381c, 0x10 },
+        { 0x381d, 0xa0 },
+        { 0x381e, 0x5  },
+        { 0x381f, 0xb0 },
+        { 0x3820, 0x0  },
+        { 0x3821, 0x0  },
+        { 0x3824, 0x11 },
+        { 0x3a08, 0x1b },
+        { 0x3a09, 0xc0 },
+        { 0x3a0a, 0x17 },
+        { 0x3a0b, 0x20 },
+        { 0x3a0d, 0x2  },
+        { 0x3a0e, 0x1  },
+        { 0x401c, 0x4  },
+        { 0x5682, 0x5  },
+        { 0x5683, 0x0  },
+        { 0x5686, 0x2  },
+        { 0x5687, 0xcc },
+        { 0x5001, 0x4f },
+        { 0x589b, 0x6  },
+        { 0x589a, 0xc5 },
+        { 0x3503, 0x0  },
+        { 0x460c, 0x20 },
+        { 0x460b, 0x37 },
+        { 0x471c, 0xd0 },
+        { 0x471d, 0x5  },
+        { 0x3815, 0x1  },
+        { 0x3818, 0xc1 },
+        { 0x501f, 0x0  },
+        { 0x5002, 0xe0 },
+        { 0x4300, 0x32 }, /* UYVY */
+        { 0x3002, 0x1c },
+        { 0x4800, 0x14 },
+        { 0x4801, 0xf  },
+        { 0x3007, 0x3b },
+        { 0x300e, 0x4  },
+        { 0x4803, 0x50 },
+        { 0x3815, 0x1  },
+        { 0x4713, 0x2  },
+        { 0x4842, 0x1  },
+        { 0x300f, 0xe  },
+        { 0x3003, 0x3  },
+        { 0x3003, 0x1  },
+        { 0xffff, 0xff },
+};
+
+struct ov5642_datafmt {
+        enum v4l2_mbus_pixelcode        code;
+        enum v4l2_colorspace            colorspace;
+};
+
+struct ov5642 {
+        struct v4l2_subdev              subdev;
+        const struct ov5642_datafmt     *fmt;
+};
+
+static const struct ov5642_datafmt ov5642_colour_fmts[] = {
+        {V4L2_MBUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_JPEG},
+};
+
+static struct ov5642 *to_ov5642(const struct i2c_client *client)
+{
+        return container_of(i2c_get_clientdata(client), struct ov5642, subdev);
+}
+
+/* Find a data format by a pixel code in an array */
+static const struct ov5642_datafmt
+                        *ov5642_find_datafmt(enum v4l2_mbus_pixelcode code)
+{
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(ov5642_colour_fmts); i++)
+                if (ov5642_colour_fmts[i].code == code)
+                        return ov5642_colour_fmts + i;
+
+        return NULL;
+}
+
+static int reg_read(struct i2c_client *client, u16 reg, u8 *val)
+{
+        int ret;
+        /* We have 16-bit i2c addresses - care for endianess */
+        unsigned char data[2] = { reg >> 8, reg & 0xff };
+
+        ret = i2c_master_send(client, data, 2);
+        if (ret < 2) {
+                dev_err(&client->dev, "%s: i2c read error, reg: %x\n",
+                        __func__, reg);
+                return ret < 0 ? ret : -EIO;
+        }
+
+        ret = i2c_master_recv(client, val, 1);
+        if (ret < 1) {
+                dev_err(&client->dev, "%s: i2c read error, reg: %x\n",
+                                __func__, reg);
+                return ret < 0 ? ret : -EIO;
+        }
+        return 0;
+}
+
+static int reg_write(struct i2c_client *client, u16 reg, u8 val)
+{
+        int ret;
+        unsigned char data[3] = { reg >> 8, reg & 0xff, val };
+
+        ret = i2c_master_send(client, data, 3);
+        if (ret < 3) {
+                dev_err(&client->dev, "%s: i2c write error, reg: %x\n",
+                        __func__, reg);
+                return ret < 0 ? ret : -EIO;
+        }
+
+        return 0;
+}
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov5642_get_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        int ret;
+        u8 val;
+
+        if (reg->reg & ~0xffff)
+                return -EINVAL;
+
+        reg->size = 1;
+
+        ret = reg_read(client, reg->reg, &val);
+        if (!ret)
+                reg->val = (__u64)val;
+
+        return ret;
+}
+
+static int ov5642_set_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+        if (reg->reg & ~0xffff || reg->val & ~0xff)
+                return -EINVAL;
+
+        return reg_write(client, reg->reg, reg->val);
+}
+#endif
+
+static int ov5642_write_array(struct i2c_client *client,
+                                struct regval_list *vals)
+{
+        while (vals->reg_num != 0xffff || vals->value != 0xff) {
+                int ret = reg_write(client, vals->reg_num, vals->value);
+                if (ret < 0)
+                        return ret;
+                vals++;
+        }
+        dev_dbg(&client->dev, "Register list loaded\n");
+        return 0;
+}
+
+static int ov5642_set_resolution(struct i2c_client *client)
+{
+        int ret;
+        u8 start_x_high = ((OV5642_SENSOR_SIZE_X - OV5642_WIDTH) / 2) >> 8;
+        u8 start_x_low  = ((OV5642_SENSOR_SIZE_X - OV5642_WIDTH) / 2) & 0xff;
+        u8 start_y_high = ((OV5642_SENSOR_SIZE_Y - OV5642_HEIGHT) / 2) >> 8;
+        u8 start_y_low  = ((OV5642_SENSOR_SIZE_Y - OV5642_HEIGHT) / 2) & 0xff;
+
+        u8 width_high   = OV5642_WIDTH  >> 8;
+        u8 width_low    = OV5642_WIDTH  & 0xff;
+        u8 height_high  = OV5642_HEIGHT >> 8;
+        u8 height_low   = OV5642_HEIGHT & 0xff;
+
+        u8 total_width_high  = OV5642_TOTAL_WIDTH  >> 8;
+        u8 total_width_low   = OV5642_TOTAL_WIDTH  & 0xff;
+        u8 total_height_high = OV5642_TOTAL_HEIGHT >> 8;
+        u8 total_height_low  = OV5642_TOTAL_HEIGHT & 0xff;
+
+        ret = reg_write(client, REG_WINDOW_START_X_HIGH, start_x_high);
+        if (!ret)
+                ret = reg_write(client, REG_WINDOW_START_X_LOW, start_x_low);
+        if (!ret)
+                ret = reg_write(client, REG_WINDOW_START_Y_HIGH, start_y_high);
+        if (!ret)
+                ret = reg_write(client, REG_WINDOW_START_Y_LOW, start_y_low);
+
+        if (!ret)
+                ret = reg_write(client, REG_WINDOW_WIDTH_HIGH, width_high);
+        if (!ret)
+                ret = reg_write(client, REG_WINDOW_WIDTH_LOW , width_low);
+        if (!ret)
+                ret = reg_write(client, REG_WINDOW_HEIGHT_HIGH, height_high);
+        if (!ret)
+                ret = reg_write(client, REG_WINDOW_HEIGHT_LOW,  height_low);
+
+        if (!ret)
+                ret = reg_write(client, REG_OUT_WIDTH_HIGH, width_high);
+        if (!ret)
+                ret = reg_write(client, REG_OUT_WIDTH_LOW , width_low);
+        if (!ret)
+                ret = reg_write(client, REG_OUT_HEIGHT_HIGH, height_high);
+        if (!ret)
+                ret = reg_write(client, REG_OUT_HEIGHT_LOW,  height_low);
+
+        if (!ret)
+                ret = reg_write(client, REG_OUT_TOTAL_WIDTH_HIGH, total_width_high);
+        if (!ret)
+                ret = reg_write(client, REG_OUT_TOTAL_WIDTH_LOW, total_width_low);
+        if (!ret)
+                ret = reg_write(client, REG_OUT_TOTAL_HEIGHT_HIGH, total_height_high);
+        if (!ret)
+                ret = reg_write(client, REG_OUT_TOTAL_HEIGHT_LOW,  total_height_low);
+
+        return ret;
+}
+
+static int ov5642_try_fmt(struct v4l2_subdev *sd,
+                          struct v4l2_mbus_framefmt *mf)
+{
+        const struct ov5642_datafmt *fmt = ov5642_find_datafmt(mf->code);
+
+        dev_dbg(sd->v4l2_dev->dev, "%s(%u) width: %u heigth: %u\n",
+                        __func__, mf->code, mf->width, mf->height);
+
+        if (!fmt) {
+                mf->code        = ov5642_colour_fmts[0].code;
+                mf->colorspace  = ov5642_colour_fmts[0].colorspace;
+        }
+
+        mf->width       = OV5642_WIDTH;
+        mf->height      = OV5642_HEIGHT;
+        mf->field       = V4L2_FIELD_NONE;
+
+        return 0;
+}
+
+static int ov5642_s_fmt(struct v4l2_subdev *sd,
+                        struct v4l2_mbus_framefmt *mf)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct ov5642 *priv = to_ov5642(client);
+
+        dev_dbg(sd->v4l2_dev->dev, "%s(%u)\n", __func__, mf->code);
+
+        /* MIPI CSI could have changed the format, double-check */
+        if (!ov5642_find_datafmt(mf->code))
+                return -EINVAL;
+
+        ov5642_try_fmt(sd, mf);
+
+        priv->fmt = ov5642_find_datafmt(mf->code);
+
+        ov5642_write_array(client, ov5642_default_regs_init);
+        ov5642_set_resolution(client);
+        ov5642_write_array(client, ov5642_default_regs_finalise);
+
+        return 0;
+}
+
+static int ov5642_g_fmt(struct v4l2_subdev *sd,
+                        struct v4l2_mbus_framefmt *mf)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct ov5642 *priv = to_ov5642(client);
+
+        const struct ov5642_datafmt *fmt = priv->fmt;
+
+        mf->code        = fmt->code;
+        mf->colorspace  = fmt->colorspace;
+        mf->width       = OV5642_WIDTH;
+        mf->height      = OV5642_HEIGHT;
+        mf->field       = V4L2_FIELD_NONE;
+
+        return 0;
+}
+
+static int ov5642_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+                           enum v4l2_mbus_pixelcode *code)
+{
+        if (index >= ARRAY_SIZE(ov5642_colour_fmts))
+                return -EINVAL;
+
+        *code = ov5642_colour_fmts[index].code;
+        return 0;
+}
+
+static int ov5642_g_chip_ident(struct v4l2_subdev *sd,
+                               struct v4l2_dbg_chip_ident *id)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+        if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
+                return -EINVAL;
+
+        if (id->match.addr != client->addr)
+                return -ENODEV;
+
+        id->ident       = V4L2_IDENT_OV5642;
+        id->revision    = 0;
+
+        return 0;
+}
+
+static int ov5642_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+        struct v4l2_rect *rect = &a->c;
+
+        a->type         = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        rect->top       = 0;
+        rect->left      = 0;
+        rect->width     = OV5642_WIDTH;
+        rect->height    = OV5642_HEIGHT;
+
+        return 0;
+}
+
+static int ov5642_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+        a->bounds.left                  = 0;
+        a->bounds.top                   = 0;
+        a->bounds.width                 = OV5642_WIDTH;
+        a->bounds.height                = OV5642_HEIGHT;
+        a->defrect                      = a->bounds;
+        a->type                         = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        a->pixelaspect.numerator        = 1;
+        a->pixelaspect.denominator      = 1;
+
+        return 0;
+}
+
+static struct v4l2_subdev_video_ops ov5642_subdev_video_ops = {
+        .s_mbus_fmt     = ov5642_s_fmt,
+        .g_mbus_fmt     = ov5642_g_fmt,
+        .try_mbus_fmt   = ov5642_try_fmt,
+        .enum_mbus_fmt  = ov5642_enum_fmt,
+        .g_crop         = ov5642_g_crop,
+        .cropcap        = ov5642_cropcap,
+};
+
+static struct v4l2_subdev_core_ops ov5642_subdev_core_ops = {
+        .g_chip_ident   = ov5642_g_chip_ident,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+        .g_register     = ov5642_get_register,
+        .s_register     = ov5642_set_register,
+#endif
+};
+
+static struct v4l2_subdev_ops ov5642_subdev_ops = {
+        .core   = &ov5642_subdev_core_ops,
+        .video  = &ov5642_subdev_video_ops,
+};
+
+/*
+ * We have to provide soc-camera operations, but we don't have anything to say
+ * there. The MIPI CSI2 driver will provide .query_bus_param and .set_bus_param
+ */
+static unsigned long soc_ov5642_query_bus_param(struct soc_camera_device *icd)
+{
+        return 0;
+}
+
+static int soc_ov5642_set_bus_param(struct soc_camera_device *icd,
+                                 unsigned long flags)
+{
+        return -EINVAL;
+}
+
+static struct soc_camera_ops soc_ov5642_ops = {
+        .query_bus_param        = soc_ov5642_query_bus_param,
+        .set_bus_param          = soc_ov5642_set_bus_param,
+};
+
+static int ov5642_video_probe(struct soc_camera_device *icd,
+                              struct i2c_client *client)
+{
+        int ret;
+        u8 id_high, id_low;
+        u16 id;
+
+        /* Read sensor Model ID */
+        ret = reg_read(client, REG_CHIP_ID_HIGH, &id_high);
+        if (ret < 0)
+                return ret;
+
+        id = id_high << 8;
+
+        ret = reg_read(client, REG_CHIP_ID_LOW, &id_low);
+        if (ret < 0)
+                return ret;
+
+        id |= id_low;
+
+        dev_info(&client->dev, "Chip ID 0x%04x detected\n", id);
+
+        if (id != 0x5642)
+                return -ENODEV;
+
+        return 0;
+}
+
+static int ov5642_probe(struct i2c_client *client,
+                        const struct i2c_device_id *did)
+{
+        struct ov5642 *priv;
+        struct soc_camera_device *icd = client->dev.platform_data;
+        struct soc_camera_link *icl;
+        int ret;
+
+        if (!icd) {
+                dev_err(&client->dev, "OV5642: missing soc-camera data!\n");
+                return -EINVAL;
+        }
+
+        icl = to_soc_camera_link(icd);
+        if (!icl) {
+                dev_err(&client->dev, "OV5642: missing platform data!\n");
+                return -EINVAL;
+        }
+
+        priv = kzalloc(sizeof(struct ov5642), GFP_KERNEL);
+        if (!priv)
+                return -ENOMEM;
+
+        v4l2_i2c_subdev_init(&priv->subdev, client, &ov5642_subdev_ops);
+
+        icd->ops        = &soc_ov5642_ops;
+        priv->fmt       = &ov5642_colour_fmts[0];
+
+        ret = ov5642_video_probe(icd, client);
+        if (ret < 0)
+                goto error;
+
+        return 0;
+
+error:
+        icd->ops = NULL;
+        kfree(priv);
+        return ret;
+}
+
+static int ov5642_remove(struct i2c_client *client)
+{
+        struct ov5642 *priv = to_ov5642(client);
+        struct soc_camera_device *icd = client->dev.platform_data;
+        struct soc_camera_link *icl = to_soc_camera_link(icd);
+
+        icd->ops = NULL;
+        if (icl->free_bus)
+                icl->free_bus(icl);
+        kfree(priv);
+
+        return 0;
+}
+
+static const struct i2c_device_id ov5642_id[] = {
+        { "ov5642", 0 },
+        { }
+};
+MODULE_DEVICE_TABLE(i2c, ov5642_id);
+
+static struct i2c_driver ov5642_i2c_driver = {
+        .driver = {
+                .name = "ov5642",
+        },
+        .probe          = ov5642_probe,
+        .remove         = ov5642_remove,
+        .id_table       = ov5642_id,
+};
+
+static int __init ov5642_mod_init(void)
+{
+        return i2c_add_driver(&ov5642_i2c_driver);
+}
+
+static void __exit ov5642_mod_exit(void)
+{
+        i2c_del_driver(&ov5642_i2c_driver);
+}
+
+module_init(ov5642_mod_init);
+module_exit(ov5642_mod_exit);
+
+MODULE_DESCRIPTION("Omnivision OV5642 Camera driver");
+MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/video/ov7670.c b/drivers/media/video/ov7670.c
index d4e7c11553c..8aa05853128 100644
--- a/drivers/media/video/ov7670.c
+++ b/drivers/media/video/ov7670.c
@@ -19,8 +19,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-chip-ident.h>
 #include <media/v4l2-mediabus.h>
-
-#include "ov7670.h"
+#include <media/ov7670.h>
 
 MODULE_AUTHOR("Jonathan Corbet <corbet@lwn.net>");
 MODULE_DESCRIPTION("A low-level driver for OmniVision ov7670 sensors");
diff --git a/drivers/media/video/ov7670.h b/drivers/media/video/ov7670.h
deleted file mode 100644
index b133bc12303..00000000000
--- a/drivers/media/video/ov7670.h
+++ /dev/null
@@ -1,20 +0,0 @@
-/*
- * A V4L2 driver for OmniVision OV7670 cameras.
- *
- * Copyright 2010 One Laptop Per Child
- *
- * This file may be distributed under the terms of the GNU General
- * Public License, version 2.
- */
-
-#ifndef __OV7670_H
-#define __OV7670_H
-
-struct ov7670_config {
-        int min_width;                  /* Filter out smaller sizes */
-        int min_height;                 /* Filter out smaller sizes */
-        int clock_speed;                /* External clock speed (MHz) */
-        bool use_smbus;                 /* Use smbus I/O instead of I2C */
-};
-
-#endif
diff --git a/drivers/media/video/ov772x.c b/drivers/media/video/ov772x.c
index 48895ef863f..397870f076c 100644
--- a/drivers/media/video/ov772x.c
+++ b/drivers/media/video/ov772x.c
@@ -1032,13 +1032,9 @@ static int ov772x_video_probe(struct soc_camera_device *icd,
         u8                  pid, ver;
         const char         *devname;
 
-        /*
-         * We must have a parent by now. And it cannot be a wrong one.
-         * So this entire test is completely redundant.
-         */
-        if (!icd->dev.parent ||
-            to_soc_camera_host(icd->dev.parent)->nr != icd->iface)
-                return -ENODEV;
+        /* We must have a parent by now. And it cannot be a wrong one. */
+        BUG_ON(!icd->parent ||
+               to_soc_camera_host(icd->parent)->nr != icd->iface);
 
         /*
          * check and show product ID and manufacturer ID
diff --git a/drivers/media/video/ov9640.c b/drivers/media/video/ov9640.c
index 5173ac449dd..3681a6ff081 100644
--- a/drivers/media/video/ov9640.c
+++ b/drivers/media/video/ov9640.c
@@ -657,16 +657,9 @@ static int ov9640_video_probe(struct soc_camera_device *icd,
         const char      *devname;
         int             ret = 0;
 
-        /*
-         * We must have a parent by now. And it cannot be a wrong one.
-         * So this entire test is completely redundant.
-         */
-        if (!icd->dev.parent ||
-            to_soc_camera_host(icd->dev.parent)->nr != icd->iface) {
-                dev_err(&client->dev, "Parent missing or invalid!\n");
-                ret = -ENODEV;
-                goto err;
-        }
+        /* We must have a parent by now. And it cannot be a wrong one. */
+        BUG_ON(!icd->parent ||
+               to_soc_camera_host(icd->parent)->nr != icd->iface);
 
         /*
          * check and show product ID and manufacturer ID
diff --git a/drivers/media/video/ov9740.c b/drivers/media/video/ov9740.c
index 4d4ee4faca6..edd1ffcca30 100644
--- a/drivers/media/video/ov9740.c
+++ b/drivers/media/video/ov9740.c
@@ -44,12 +44,12 @@
 #define OV9740_Y_ADDR_START_LO          0x0347
 #define OV9740_X_ADDR_END_HI            0x0348
 #define OV9740_X_ADDR_END_LO            0x0349
-#define OV9740_Y_ADDR_END_HI            0x034A
-#define OV9740_Y_ADDR_END_LO            0x034B
-#define OV9740_X_OUTPUT_SIZE_HI         0x034C
-#define OV9740_X_OUTPUT_SIZE_LO         0x034D
-#define OV9740_Y_OUTPUT_SIZE_HI         0x034E
-#define OV9740_Y_OUTPUT_SIZE_LO         0x034F
+#define OV9740_Y_ADDR_END_HI            0x034a
+#define OV9740_Y_ADDR_END_LO            0x034b
+#define OV9740_X_OUTPUT_SIZE_HI         0x034c
+#define OV9740_X_OUTPUT_SIZE_LO         0x034d
+#define OV9740_Y_OUTPUT_SIZE_HI         0x034e
+#define OV9740_Y_OUTPUT_SIZE_LO         0x034f
 
 /* IO Control Registers */
 #define OV9740_IO_CREL00                0x3002
@@ -68,6 +68,7 @@
 #define OV9740_ANALOG_CTRL04            0x3604
 #define OV9740_ANALOG_CTRL10            0x3610
 #define OV9740_ANALOG_CTRL12            0x3612
+#define OV9740_ANALOG_CTRL15            0x3615
 #define OV9740_ANALOG_CTRL20            0x3620
 #define OV9740_ANALOG_CTRL21            0x3621
 #define OV9740_ANALOG_CTRL22            0x3622
@@ -89,28 +90,28 @@
 #define OV9740_TIMING_CTRL35            0x3835
 
 /* Banding Filter */
-#define OV9740_AEC_MAXEXPO_60_H         0x3A02
-#define OV9740_AEC_MAXEXPO_60_L         0x3A03
-#define OV9740_AEC_B50_STEP_HI          0x3A08
-#define OV9740_AEC_B50_STEP_LO          0x3A09
-#define OV9740_AEC_B60_STEP_HI          0x3A0A
-#define OV9740_AEC_B60_STEP_LO          0x3A0B
-#define OV9740_AEC_CTRL0D               0x3A0D
-#define OV9740_AEC_CTRL0E               0x3A0E
-#define OV9740_AEC_MAXEXPO_50_H         0x3A14
-#define OV9740_AEC_MAXEXPO_50_L         0x3A15
+#define OV9740_AEC_MAXEXPO_60_H         0x3a02
+#define OV9740_AEC_MAXEXPO_60_L         0x3a03
+#define OV9740_AEC_B50_STEP_HI          0x3a08
+#define OV9740_AEC_B50_STEP_LO          0x3a09
+#define OV9740_AEC_B60_STEP_HI          0x3a0a
+#define OV9740_AEC_B60_STEP_LO          0x3a0b
+#define OV9740_AEC_CTRL0D               0x3a0d
+#define OV9740_AEC_CTRL0E               0x3a0e
+#define OV9740_AEC_MAXEXPO_50_H         0x3a14
+#define OV9740_AEC_MAXEXPO_50_L         0x3a15
 
 /* AEC/AGC Control */
 #define OV9740_AEC_ENABLE               0x3503
-#define OV9740_GAIN_CEILING_01          0x3A18
-#define OV9740_GAIN_CEILING_02          0x3A19
-#define OV9740_AEC_HI_THRESHOLD         0x3A11
-#define OV9740_AEC_3A1A                 0x3A1A
-#define OV9740_AEC_CTRL1B_WPT2          0x3A1B
-#define OV9740_AEC_CTRL0F_WPT           0x3A0F
-#define OV9740_AEC_CTRL10_BPT           0x3A10
-#define OV9740_AEC_CTRL1E_BPT2          0x3A1E
-#define OV9740_AEC_LO_THRESHOLD         0x3A1F
+#define OV9740_GAIN_CEILING_01          0x3a18
+#define OV9740_GAIN_CEILING_02          0x3a19
+#define OV9740_AEC_HI_THRESHOLD         0x3a11
+#define OV9740_AEC_3A1A                 0x3a1a
+#define OV9740_AEC_CTRL1B_WPT2          0x3a1b
+#define OV9740_AEC_CTRL0F_WPT           0x3a0f
+#define OV9740_AEC_CTRL10_BPT           0x3a10
+#define OV9740_AEC_CTRL1E_BPT2          0x3a1e
+#define OV9740_AEC_LO_THRESHOLD         0x3a1f
 
 /* BLC Control */
 #define OV9740_BLC_AUTO_ENABLE          0x4002
@@ -132,7 +133,7 @@
 #define OV9740_VT_SYS_CLK_DIV           0x0303
 #define OV9740_VT_PIX_CLK_DIV           0x0301
 #define OV9740_PLL_CTRL3010             0x3010
-#define OV9740_VFIFO_CTRL00             0x460E
+#define OV9740_VFIFO_CTRL00             0x460e
 
 /* ISP Control */
 #define OV9740_ISP_CTRL00               0x5000
@@ -141,9 +142,9 @@
 #define OV9740_ISP_CTRL05               0x5005
 #define OV9740_ISP_CTRL12               0x5012
 #define OV9740_ISP_CTRL19               0x5019
-#define OV9740_ISP_CTRL1A               0x501A
-#define OV9740_ISP_CTRL1E               0x501E
-#define OV9740_ISP_CTRL1F               0x501F
+#define OV9740_ISP_CTRL1A               0x501a
+#define OV9740_ISP_CTRL1E               0x501e
+#define OV9740_ISP_CTRL1F               0x501f
 #define OV9740_ISP_CTRL20               0x5020
 #define OV9740_ISP_CTRL21               0x5021
 
@@ -158,12 +159,12 @@
 #define OV9740_AWB_ADV_CTRL04           0x5187
 #define OV9740_AWB_ADV_CTRL05           0x5188
 #define OV9740_AWB_ADV_CTRL06           0x5189
-#define OV9740_AWB_ADV_CTRL07           0x518A
-#define OV9740_AWB_ADV_CTRL08           0x518B
-#define OV9740_AWB_ADV_CTRL09           0x518C
-#define OV9740_AWB_ADV_CTRL10           0x518D
-#define OV9740_AWB_ADV_CTRL11           0x518E
-#define OV9740_AWB_CTRL0F               0x518F
+#define OV9740_AWB_ADV_CTRL07           0x518a
+#define OV9740_AWB_ADV_CTRL08           0x518b
+#define OV9740_AWB_ADV_CTRL09           0x518c
+#define OV9740_AWB_ADV_CTRL10           0x518d
+#define OV9740_AWB_ADV_CTRL11           0x518e
+#define OV9740_AWB_CTRL0F               0x518f
 #define OV9740_AWB_CTRL10               0x5190
 #define OV9740_AWB_CTRL11               0x5191
 #define OV9740_AWB_CTRL12               0x5192
@@ -180,27 +181,8 @@
 #define OV9740_MIPI_CTRL_3012           0x3012
 #define OV9740_SC_CMMM_MIPI_CTR         0x3014
 
-/* supported resolutions */
-enum {
-        OV9740_VGA,
-        OV9740_720P,
-};
-
-struct ov9740_resolution {
-        unsigned int width;
-        unsigned int height;
-};
-
-static struct ov9740_resolution ov9740_resolutions[] = {
-        [OV9740_VGA] = {
-                .width  = 640,
-                .height = 480,
-        },
-        [OV9740_720P] = {
-                .width  = 1280,
-                .height = 720,
-        },
-};
+#define OV9740_MAX_WIDTH                1280
+#define OV9740_MAX_HEIGHT               720
 
 /* Misc. structures */
 struct ov9740_reg {
@@ -219,9 +201,16 @@ struct ov9740_priv {
 
         bool                            flag_vflip;
         bool                            flag_hflip;
+
+        /* For suspend/resume. */
+        struct v4l2_mbus_framefmt       current_mf;
+        bool                            current_enable;
 };
 
 static const struct ov9740_reg ov9740_defaults[] = {
+        /* Software Reset */
+        { OV9740_SOFTWARE_RESET,        0x01 },
+
         /* Banding Filter */
         { OV9740_AEC_B50_STEP_HI,       0x00 },
         { OV9740_AEC_B50_STEP_LO,       0xe8 },
@@ -241,36 +230,36 @@ static const struct ov9740_reg ov9740_defaults[] = {
         /* Un-documented OV9740 registers */
         { 0x5800, 0x29 }, { 0x5801, 0x25 }, { 0x5802, 0x20 }, { 0x5803, 0x21 },
         { 0x5804, 0x26 }, { 0x5805, 0x2e }, { 0x5806, 0x11 }, { 0x5807, 0x0c },
-        { 0x5808, 0x09 }, { 0x5809, 0x0a }, { 0x580A, 0x0e }, { 0x580B, 0x16 },
-        { 0x580C, 0x06 }, { 0x580D, 0x02 }, { 0x580E, 0x00 }, { 0x580F, 0x00 },
+        { 0x5808, 0x09 }, { 0x5809, 0x0a }, { 0x580a, 0x0e }, { 0x580b, 0x16 },
+        { 0x580c, 0x06 }, { 0x580d, 0x02 }, { 0x580e, 0x00 }, { 0x580f, 0x00 },
         { 0x5810, 0x04 }, { 0x5811, 0x0a }, { 0x5812, 0x05 }, { 0x5813, 0x02 },
         { 0x5814, 0x00 }, { 0x5815, 0x00 }, { 0x5816, 0x03 }, { 0x5817, 0x09 },
-        { 0x5818, 0x0f }, { 0x5819, 0x0a }, { 0x581A, 0x07 }, { 0x581B, 0x08 },
-        { 0x581C, 0x0b }, { 0x581D, 0x14 }, { 0x581E, 0x28 }, { 0x581F, 0x23 },
+        { 0x5818, 0x0f }, { 0x5819, 0x0a }, { 0x581a, 0x07 }, { 0x581b, 0x08 },
+        { 0x581c, 0x0b }, { 0x581d, 0x14 }, { 0x581e, 0x28 }, { 0x581f, 0x23 },
         { 0x5820, 0x1d }, { 0x5821, 0x1e }, { 0x5822, 0x24 }, { 0x5823, 0x2a },
         { 0x5824, 0x4f }, { 0x5825, 0x6f }, { 0x5826, 0x5f }, { 0x5827, 0x7f },
-        { 0x5828, 0x9f }, { 0x5829, 0x5f }, { 0x582A, 0x8f }, { 0x582B, 0x9e },
-        { 0x582C, 0x8f }, { 0x582D, 0x9f }, { 0x582E, 0x4f }, { 0x582F, 0x87 },
+        { 0x5828, 0x9f }, { 0x5829, 0x5f }, { 0x582a, 0x8f }, { 0x582b, 0x9e },
+        { 0x582c, 0x8f }, { 0x582d, 0x9f }, { 0x582e, 0x4f }, { 0x582f, 0x87 },
         { 0x5830, 0x86 }, { 0x5831, 0x97 }, { 0x5832, 0xae }, { 0x5833, 0x3f },
         { 0x5834, 0x8e }, { 0x5835, 0x7c }, { 0x5836, 0x7e }, { 0x5837, 0xaf },
-        { 0x5838, 0x8f }, { 0x5839, 0x8f }, { 0x583A, 0x9f }, { 0x583B, 0x7f },
-        { 0x583C, 0x5f },
+        { 0x5838, 0x8f }, { 0x5839, 0x8f }, { 0x583a, 0x9f }, { 0x583b, 0x7f },
+        { 0x583c, 0x5f },
 
         /* Y Gamma */
         { 0x5480, 0x07 }, { 0x5481, 0x18 }, { 0x5482, 0x2c }, { 0x5483, 0x4e },
         { 0x5484, 0x5e }, { 0x5485, 0x6b }, { 0x5486, 0x77 }, { 0x5487, 0x82 },
-        { 0x5488, 0x8c }, { 0x5489, 0x95 }, { 0x548A, 0xa4 }, { 0x548B, 0xb1 },
-        { 0x548C, 0xc6 }, { 0x548D, 0xd8 }, { 0x548E, 0xe9 },
+        { 0x5488, 0x8c }, { 0x5489, 0x95 }, { 0x548a, 0xa4 }, { 0x548b, 0xb1 },
+        { 0x548c, 0xc6 }, { 0x548d, 0xd8 }, { 0x548e, 0xe9 },
 
         /* UV Gamma */
         { 0x5490, 0x0f }, { 0x5491, 0xff }, { 0x5492, 0x0d }, { 0x5493, 0x05 },
         { 0x5494, 0x07 }, { 0x5495, 0x1a }, { 0x5496, 0x04 }, { 0x5497, 0x01 },
-        { 0x5498, 0x03 }, { 0x5499, 0x53 }, { 0x549A, 0x02 }, { 0x549B, 0xeb },
-        { 0x549C, 0x02 }, { 0x549D, 0xa0 }, { 0x549E, 0x02 }, { 0x549F, 0x67 },
-        { 0x54A0, 0x02 }, { 0x54A1, 0x3b }, { 0x54A2, 0x02 }, { 0x54A3, 0x18 },
-        { 0x54A4, 0x01 }, { 0x54A5, 0xe7 }, { 0x54A6, 0x01 }, { 0x54A7, 0xc3 },
-        { 0x54A8, 0x01 }, { 0x54A9, 0x94 }, { 0x54AA, 0x01 }, { 0x54AB, 0x72 },
-        { 0x54AC, 0x01 }, { 0x54AD, 0x57 },
+        { 0x5498, 0x03 }, { 0x5499, 0x53 }, { 0x549a, 0x02 }, { 0x549b, 0xeb },
+        { 0x549c, 0x02 }, { 0x549d, 0xa0 }, { 0x549e, 0x02 }, { 0x549f, 0x67 },
+        { 0x54a0, 0x02 }, { 0x54a1, 0x3b }, { 0x54a2, 0x02 }, { 0x54a3, 0x18 },
+        { 0x54a4, 0x01 }, { 0x54a5, 0xe7 }, { 0x54a6, 0x01 }, { 0x54a7, 0xc3 },
+        { 0x54a8, 0x01 }, { 0x54a9, 0x94 }, { 0x54aa, 0x01 }, { 0x54ab, 0x72 },
+        { 0x54ac, 0x01 }, { 0x54ad, 0x57 },
 
         /* AWB */
         { OV9740_AWB_CTRL00,            0xf0 },
@@ -296,18 +285,18 @@ static const struct ov9740_reg ov9740_defaults[] = {
         { OV9740_AWB_CTRL14,            0x00 },
 
         /* CIP */
-        { 0x530D, 0x12 },
+        { 0x530d, 0x12 },
 
         /* CMX */
         { 0x5380, 0x01 }, { 0x5381, 0x00 }, { 0x5382, 0x00 }, { 0x5383, 0x17 },
         { 0x5384, 0x00 }, { 0x5385, 0x01 }, { 0x5386, 0x00 }, { 0x5387, 0x00 },
-        { 0x5388, 0x00 }, { 0x5389, 0xe0 }, { 0x538A, 0x00 }, { 0x538B, 0x20 },
-        { 0x538C, 0x00 }, { 0x538D, 0x00 }, { 0x538E, 0x00 }, { 0x538F, 0x16 },
+        { 0x5388, 0x00 }, { 0x5389, 0xe0 }, { 0x538a, 0x00 }, { 0x538b, 0x20 },
+        { 0x538c, 0x00 }, { 0x538d, 0x00 }, { 0x538e, 0x00 }, { 0x538f, 0x16 },
         { 0x5390, 0x00 }, { 0x5391, 0x9c }, { 0x5392, 0x00 }, { 0x5393, 0xa0 },
         { 0x5394, 0x18 },
 
         /* 50/60 Detection */
-        { 0x3C0A, 0x9c }, { 0x3C0B, 0x3f },
+        { 0x3c0a, 0x9c }, { 0x3c0b, 0x3f },
 
         /* Output Select */
         { OV9740_IO_OUTPUT_SEL01,       0x00 },
@@ -333,6 +322,7 @@ static const struct ov9740_reg ov9740_defaults[] = {
         { OV9740_ANALOG_CTRL10,         0xa1 },
         { OV9740_ANALOG_CTRL12,         0x24 },
         { OV9740_ANALOG_CTRL22,         0x9f },
+        { OV9740_ANALOG_CTRL15,         0xf0 },
 
         /* Sensor Control */
         { OV9740_SENSOR_CTRL03,         0x42 },
@@ -385,7 +375,7 @@ static const struct ov9740_reg ov9740_defaults[] = {
         { OV9740_LN_LENGTH_PCK_LO,      0x62 },
 
         /* MIPI Control */
-        { OV9740_MIPI_CTRL00,           0x44 },
+        { OV9740_MIPI_CTRL00,           0x44 }, /* 0x64 for discontinuous clk */
         { OV9740_MIPI_3837,             0x01 },
         { OV9740_MIPI_CTRL01,           0x0f },
         { OV9740_MIPI_CTRL03,           0x05 },
@@ -393,54 +383,9 @@ static const struct ov9740_reg ov9740_defaults[] = {
         { OV9740_VFIFO_RD_CTRL,         0x16 },
         { OV9740_MIPI_CTRL_3012,        0x70 },
         { OV9740_SC_CMMM_MIPI_CTR,      0x01 },
-};
-
-static const struct ov9740_reg ov9740_regs_vga[] = {
-        { OV9740_X_ADDR_START_HI,       0x00 },
-        { OV9740_X_ADDR_START_LO,       0xa0 },
-        { OV9740_Y_ADDR_START_HI,       0x00 },
-        { OV9740_Y_ADDR_START_LO,       0x00 },
-        { OV9740_X_ADDR_END_HI,         0x04 },
-        { OV9740_X_ADDR_END_LO,         0x63 },
-        { OV9740_Y_ADDR_END_HI,         0x02 },
-        { OV9740_Y_ADDR_END_LO,         0xd3 },
-        { OV9740_X_OUTPUT_SIZE_HI,      0x02 },
-        { OV9740_X_OUTPUT_SIZE_LO,      0x80 },
-        { OV9740_Y_OUTPUT_SIZE_HI,      0x01 },
-        { OV9740_Y_OUTPUT_SIZE_LO,      0xe0 },
-        { OV9740_ISP_CTRL1E,            0x03 },
-        { OV9740_ISP_CTRL1F,            0xc0 },
-        { OV9740_ISP_CTRL20,            0x02 },
-        { OV9740_ISP_CTRL21,            0xd0 },
-        { OV9740_VFIFO_READ_START_HI,   0x01 },
-        { OV9740_VFIFO_READ_START_LO,   0x40 },
-        { OV9740_ISP_CTRL00,            0xff },
-        { OV9740_ISP_CTRL01,            0xff },
-        { OV9740_ISP_CTRL03,            0xff },
-};
 
-static const struct ov9740_reg ov9740_regs_720p[] = {
-        { OV9740_X_ADDR_START_HI,       0x00 },
-        { OV9740_X_ADDR_START_LO,       0x00 },
-        { OV9740_Y_ADDR_START_HI,       0x00 },
-        { OV9740_Y_ADDR_START_LO,       0x00 },
-        { OV9740_X_ADDR_END_HI,         0x05 },
-        { OV9740_X_ADDR_END_LO,         0x03 },
-        { OV9740_Y_ADDR_END_HI,         0x02 },
-        { OV9740_Y_ADDR_END_LO,         0xd3 },
-        { OV9740_X_OUTPUT_SIZE_HI,      0x05 },
-        { OV9740_X_OUTPUT_SIZE_LO,      0x00 },
-        { OV9740_Y_OUTPUT_SIZE_HI,      0x02 },
-        { OV9740_Y_OUTPUT_SIZE_LO,      0xd0 },
-        { OV9740_ISP_CTRL1E,            0x05 },
-        { OV9740_ISP_CTRL1F,            0x00 },
-        { OV9740_ISP_CTRL20,            0x02 },
-        { OV9740_ISP_CTRL21,            0xd0 },
-        { OV9740_VFIFO_READ_START_HI,   0x02 },
-        { OV9740_VFIFO_READ_START_LO,   0x30 },
-        { OV9740_ISP_CTRL00,            0xff },
-        { OV9740_ISP_CTRL01,            0xef },
-        { OV9740_ISP_CTRL03,            0xff },
+        /* YUYV order */
+        { OV9740_ISP_CTRL19,            0x02 },
 };
 
 static enum v4l2_mbus_pixelcode ov9740_codes[] = {
@@ -537,7 +482,8 @@ static int ov9740_reg_rmw(struct i2c_client *client, u16 reg, u8 set, u8 unset)
         ret = ov9740_reg_read(client, reg, &val);
         if (ret < 0) {
                 dev_err(&client->dev,
-                        "[Read]-Modify-Write of register %02x failed!\n", reg);
+                        "[Read]-Modify-Write of register 0x%04x failed!\n",
+                        reg);
                 return ret;
         }
 
@@ -547,7 +493,8 @@ static int ov9740_reg_rmw(struct i2c_client *client, u16 reg, u8 set, u8 unset)
         ret = ov9740_reg_write(client, reg, val);
         if (ret < 0) {
                 dev_err(&client->dev,
-                        "Read-Modify-[Write] of register %02x failed!\n", reg);
+                        "Read-Modify-[Write] of register 0x%04x failed!\n",
+                        reg);
                 return ret;
         }
 
@@ -608,6 +555,8 @@ static int ov9740_s_stream(struct v4l2_subdev *sd, int enable)
                                                0x00);
         }
 
+        priv->current_enable = enable;
+
         return ret;
 }
 
@@ -630,126 +579,127 @@ static unsigned long ov9740_query_bus_param(struct soc_camera_device *icd)
         return soc_camera_apply_sensor_flags(icl, flags);
 }
 
-/* Get status of additional camera capabilities */
-static int ov9740_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-        struct ov9740_priv *priv = to_ov9740(sd);
-
-        switch (ctrl->id) {
-        case V4L2_CID_VFLIP:
-                ctrl->value = priv->flag_vflip;
-                break;
-        case V4L2_CID_HFLIP:
-                ctrl->value = priv->flag_hflip;
-                break;
-        default:
-                return -EINVAL;
-        }
-
-        return 0;
-}
-
-/* Set status of additional camera capabilities */
-static int ov9740_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-        struct ov9740_priv *priv = to_ov9740(sd);
-
-        switch (ctrl->id) {
-        case V4L2_CID_VFLIP:
-                priv->flag_vflip = ctrl->value;
-                break;
-        case V4L2_CID_HFLIP:
-                priv->flag_hflip = ctrl->value;
-                break;
-        default:
-                return -EINVAL;
-        }
-
-        return 0;
-}
-
-/* Get chip identification */
-static int ov9740_g_chip_ident(struct v4l2_subdev *sd,
-                               struct v4l2_dbg_chip_ident *id)
+/* select nearest higher resolution for capture */
+static void ov9740_res_roundup(u32 *width, u32 *height)
 {
-        struct ov9740_priv *priv = to_ov9740(sd);
+        /* Width must be a multiple of 4 pixels. */
+        *width = ALIGN(*width, 4);
 
-        id->ident = priv->ident;
-        id->revision = priv->revision;
+        /* Max resolution is 1280x720 (720p). */
+        if (*width > OV9740_MAX_WIDTH)
+                *width = OV9740_MAX_WIDTH;
 
-        return 0;
+        if (*height > OV9740_MAX_HEIGHT)
+                *height = OV9740_MAX_HEIGHT;
 }
 
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-static int ov9740_get_register(struct v4l2_subdev *sd,
-                               struct v4l2_dbg_register *reg)
+/* Setup registers according to resolution and color encoding */
+static int ov9740_set_res(struct i2c_client *client, u32 width, u32 height)
 {
-        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        u32 x_start;
+        u32 y_start;
+        u32 x_end;
+        u32 y_end;
+        bool scaling = 0;
+        u32 scale_input_x;
+        u32 scale_input_y;
         int ret;
-        u8 val;
-
-        if (reg->reg & ~0xffff)
-                return -EINVAL;
 
-        reg->size = 2;
-
-        ret = ov9740_reg_read(client, reg->reg, &val);
-        if (ret)
-                return ret;
-
-        reg->val = (__u64)val;
+        if ((width != OV9740_MAX_WIDTH) || (height != OV9740_MAX_HEIGHT))
+                scaling = 1;
 
-        return ret;
-}
-
-static int ov9740_set_register(struct v4l2_subdev *sd,
-                               struct v4l2_dbg_register *reg)
-{
-        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        /*
+         * Try to use as much of the sensor area as possible when supporting
+         * smaller resolutions.  Depending on the aspect ratio of the
+         * chosen resolution, we can either use the full width of the sensor,
+         * or the full height of the sensor (or both if the aspect ratio is
+         * the same as 1280x720.
+         */
+        if ((OV9740_MAX_WIDTH * height) > (OV9740_MAX_HEIGHT * width)) {
+                scale_input_x = (OV9740_MAX_HEIGHT * width) / height;
+                scale_input_y = OV9740_MAX_HEIGHT;
+        } else {
+                scale_input_x = OV9740_MAX_WIDTH;
+                scale_input_y = (OV9740_MAX_WIDTH * height) / width;
+        }
 
-        if (reg->reg & ~0xffff || reg->val & ~0xff)
-                return -EINVAL;
+        /* These describe the area of the sensor to use. */
+        x_start = (OV9740_MAX_WIDTH - scale_input_x) / 2;
+        y_start = (OV9740_MAX_HEIGHT - scale_input_y) / 2;
+        x_end = x_start + scale_input_x - 1;
+        y_end = y_start + scale_input_y - 1;
 
-        return ov9740_reg_write(client, reg->reg, reg->val);
-}
-#endif
+        ret = ov9740_reg_write(client, OV9740_X_ADDR_START_HI, x_start >> 8);
+        if (ret)
+                goto done;
+        ret = ov9740_reg_write(client, OV9740_X_ADDR_START_LO, x_start & 0xff);
+        if (ret)
+                goto done;
+        ret = ov9740_reg_write(client, OV9740_Y_ADDR_START_HI, y_start >> 8);
+        if (ret)
+                goto done;
+        ret = ov9740_reg_write(client, OV9740_Y_ADDR_START_LO, y_start & 0xff);
+        if (ret)
+                goto done;
 
-/* select nearest higher resolution for capture */
-static void ov9740_res_roundup(u32 *width, u32 *height)
-{
-        int i;
+        ret = ov9740_reg_write(client, OV9740_X_ADDR_END_HI, x_end >> 8);
+        if (ret)
+                goto done;
+        ret = ov9740_reg_write(client, OV9740_X_ADDR_END_LO, x_end & 0xff);
+        if (ret)
+                goto done;
+        ret = ov9740_reg_write(client, OV9740_Y_ADDR_END_HI, y_end >> 8);
+        if (ret)
+                goto done;
+        ret = ov9740_reg_write(client, OV9740_Y_ADDR_END_LO, y_end & 0xff);
+        if (ret)
+                goto done;
 
-        for (i = 0; i < ARRAY_SIZE(ov9740_resolutions); i++)
-                if ((ov9740_resolutions[i].width >= *width) &&
-                    (ov9740_resolutions[i].height >= *height)) {
-                        *width = ov9740_resolutions[i].width;
-                        *height = ov9740_resolutions[i].height;
-                        return;
-                }
+        ret = ov9740_reg_write(client, OV9740_X_OUTPUT_SIZE_HI, width >> 8);
+        if (ret)
+                goto done;
+        ret = ov9740_reg_write(client, OV9740_X_OUTPUT_SIZE_LO, width & 0xff);
+        if (ret)
+                goto done;
+        ret = ov9740_reg_write(client, OV9740_Y_OUTPUT_SIZE_HI, height >> 8);
+        if (ret)
+                goto done;
+        ret = ov9740_reg_write(client, OV9740_Y_OUTPUT_SIZE_LO, height & 0xff);
+        if (ret)
+                goto done;
 
-        *width = ov9740_resolutions[OV9740_720P].width;
-        *height = ov9740_resolutions[OV9740_720P].height;
-}
+        ret = ov9740_reg_write(client, OV9740_ISP_CTRL1E, scale_input_x >> 8);
+        if (ret)
+                goto done;
+        ret = ov9740_reg_write(client, OV9740_ISP_CTRL1F, scale_input_x & 0xff);
+        if (ret)
+                goto done;
+        ret = ov9740_reg_write(client, OV9740_ISP_CTRL20, scale_input_y >> 8);
+        if (ret)
+                goto done;
+        ret = ov9740_reg_write(client, OV9740_ISP_CTRL21, scale_input_y & 0xff);
+        if (ret)
+                goto done;
 
-/* Setup registers according to resolution and color encoding */
-static int ov9740_set_res(struct i2c_client *client, u32 width)
-{
-        int ret;
+        ret = ov9740_reg_write(client, OV9740_VFIFO_READ_START_HI,
+                               (scale_input_x - width) >> 8);
+        if (ret)
+                goto done;
+        ret = ov9740_reg_write(client, OV9740_VFIFO_READ_START_LO,
+                               (scale_input_x - width) & 0xff);
+        if (ret)
+                goto done;
 
-        /* select register configuration for given resolution */
-        if (width == ov9740_resolutions[OV9740_VGA].width) {
-                dev_dbg(&client->dev, "Setting image size to 640x480\n");
-                ret = ov9740_reg_write_array(client, ov9740_regs_vga,
-                                             ARRAY_SIZE(ov9740_regs_vga));
-        } else if (width == ov9740_resolutions[OV9740_720P].width) {
-                dev_dbg(&client->dev, "Setting image size to 1280x720\n");
-                ret = ov9740_reg_write_array(client, ov9740_regs_720p,
-                                             ARRAY_SIZE(ov9740_regs_720p));
-        } else {
-                dev_err(&client->dev, "Failed to select resolution!\n");
-                return -EINVAL;
-        }
+        ret = ov9740_reg_write(client, OV9740_ISP_CTRL00, 0xff);
+        if (ret)
+                goto done;
+        ret = ov9740_reg_write(client, OV9740_ISP_CTRL01, 0xef |
+                                                          (scaling << 4));
+        if (ret)
+                goto done;
+        ret = ov9740_reg_write(client, OV9740_ISP_CTRL03, 0xff);
 
+done:
         return ret;
 }
 
@@ -758,6 +708,7 @@ static int ov9740_s_fmt(struct v4l2_subdev *sd,
                         struct v4l2_mbus_framefmt *mf)
 {
         struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct ov9740_priv *priv = to_ov9740(sd);
         enum v4l2_colorspace cspace;
         enum v4l2_mbus_pixelcode code = mf->code;
         int ret;
@@ -777,13 +728,15 @@ static int ov9740_s_fmt(struct v4l2_subdev *sd,
         if (ret < 0)
                 return ret;
 
-        ret = ov9740_set_res(client, mf->width);
+        ret = ov9740_set_res(client, mf->width, mf->height);
         if (ret < 0)
                 return ret;
 
         mf->code        = code;
         mf->colorspace  = cspace;
 
+        memcpy(&priv->current_mf, mf, sizeof(struct v4l2_mbus_framefmt));
+
         return ret;
 }
 
@@ -814,8 +767,8 @@ static int ov9740_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
 {
         a->bounds.left          = 0;
         a->bounds.top           = 0;
-        a->bounds.width         = ov9740_resolutions[OV9740_720P].width;
-        a->bounds.height        = ov9740_resolutions[OV9740_720P].height;
+        a->bounds.width         = OV9740_MAX_WIDTH;
+        a->bounds.height        = OV9740_MAX_HEIGHT;
         a->defrect              = a->bounds;
         a->type                 = V4L2_BUF_TYPE_VIDEO_CAPTURE;
         a->pixelaspect.numerator        = 1;
@@ -828,13 +781,115 @@ static int ov9740_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
 {
         a->c.left               = 0;
         a->c.top                = 0;
-        a->c.width              = ov9740_resolutions[OV9740_720P].width;
-        a->c.height             = ov9740_resolutions[OV9740_720P].height;
+        a->c.width              = OV9740_MAX_WIDTH;
+        a->c.height             = OV9740_MAX_HEIGHT;
         a->type                 = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
         return 0;
 }
 
+/* Get status of additional camera capabilities */
+static int ov9740_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+        struct ov9740_priv *priv = to_ov9740(sd);
+
+        switch (ctrl->id) {
+        case V4L2_CID_VFLIP:
+                ctrl->value = priv->flag_vflip;
+                break;
+        case V4L2_CID_HFLIP:
+                ctrl->value = priv->flag_hflip;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+/* Set status of additional camera capabilities */
+static int ov9740_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+        struct ov9740_priv *priv = to_ov9740(sd);
+
+        switch (ctrl->id) {
+        case V4L2_CID_VFLIP:
+                priv->flag_vflip = ctrl->value;
+                break;
+        case V4L2_CID_HFLIP:
+                priv->flag_hflip = ctrl->value;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+/* Get chip identification */
+static int ov9740_g_chip_ident(struct v4l2_subdev *sd,
+                               struct v4l2_dbg_chip_ident *id)
+{
+        struct ov9740_priv *priv = to_ov9740(sd);
+
+        id->ident = priv->ident;
+        id->revision = priv->revision;
+
+        return 0;
+}
+
+static int ov9740_s_power(struct v4l2_subdev *sd, int on)
+{
+        struct ov9740_priv *priv = to_ov9740(sd);
+
+        if (!priv->current_enable)
+                return 0;
+
+        if (on) {
+                ov9740_s_fmt(sd, &priv->current_mf);
+                ov9740_s_stream(sd, priv->current_enable);
+        } else {
+                ov9740_s_stream(sd, 0);
+                priv->current_enable = true;
+        }
+
+        return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov9740_get_register(struct v4l2_subdev *sd,
+                               struct v4l2_dbg_register *reg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        int ret;
+        u8 val;
+
+        if (reg->reg & ~0xffff)
+                return -EINVAL;
+
+        reg->size = 2;
+
+        ret = ov9740_reg_read(client, reg->reg, &val);
+        if (ret)
+                return ret;
+
+        reg->val = (__u64)val;
+
+        return ret;
+}
+
+static int ov9740_set_register(struct v4l2_subdev *sd,
+                               struct v4l2_dbg_register *reg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+        if (reg->reg & ~0xffff || reg->val & ~0xff)
+                return -EINVAL;
+
+        return ov9740_reg_write(client, reg->reg, reg->val);
+}
+#endif
+
 static int ov9740_video_probe(struct soc_camera_device *icd,
                               struct i2c_client *client)
 {
@@ -843,16 +898,9 @@ static int ov9740_video_probe(struct soc_camera_device *icd,
         u8 modelhi, modello;
         int ret;
 
-        /*
-         * We must have a parent by now. And it cannot be a wrong one.
-         * So this entire test is completely redundant.
-         */
-        if (!icd->dev.parent ||
-            to_soc_camera_host(icd->dev.parent)->nr != icd->iface) {
-                dev_err(&client->dev, "Parent missing or invalid!\n");
-                ret = -ENODEV;
-                goto err;
-        }
+        /* We must have a parent by now. And it cannot be a wrong one. */
+        BUG_ON(!icd->parent ||
+               to_soc_camera_host(icd->parent)->nr != icd->iface);
 
         /*
          * check and show product ID and manufacturer ID
@@ -901,24 +949,24 @@ static struct soc_camera_ops ov9740_ops = {
         .num_controls           = ARRAY_SIZE(ov9740_controls),
 };
 
+static struct v4l2_subdev_video_ops ov9740_video_ops = {
+        .s_stream               = ov9740_s_stream,
+        .s_mbus_fmt             = ov9740_s_fmt,
+        .try_mbus_fmt           = ov9740_try_fmt,
+        .enum_mbus_fmt          = ov9740_enum_fmt,
+        .cropcap                = ov9740_cropcap,
+        .g_crop                 = ov9740_g_crop,
+};
+
 static struct v4l2_subdev_core_ops ov9740_core_ops = {
         .g_ctrl                 = ov9740_g_ctrl,
         .s_ctrl                 = ov9740_s_ctrl,
         .g_chip_ident           = ov9740_g_chip_ident,
+        .s_power                = ov9740_s_power,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register             = ov9740_get_register,
         .s_register             = ov9740_set_register,
 #endif
-
-};
-
-static struct v4l2_subdev_video_ops ov9740_video_ops = {
-        .s_stream               = ov9740_s_stream,
-        .s_mbus_fmt             = ov9740_s_fmt,
-        .try_mbus_fmt           = ov9740_try_fmt,
-        .enum_mbus_fmt          = ov9740_enum_fmt,
-        .cropcap                = ov9740_cropcap,
-        .g_crop                 = ov9740_g_crop,
 };
 
 static struct v4l2_subdev_ops ov9740_subdev_ops = {
diff --git a/drivers/media/video/pms.c b/drivers/media/video/pms.c
index 7551907f8c2..e753b5e4d2c 100644
--- a/drivers/media/video/pms.c
+++ b/drivers/media/video/pms.c
@@ -28,7 +28,6 @@
 #include <linux/mm.h>
 #include <linux/ioport.h>
 #include <linux/init.h>
-#include <linux/version.h>
 #include <linux/mutex.h>
 #include <linux/uaccess.h>
 #include <asm/io.h>
@@ -39,7 +38,7 @@
 #include <media/v4l2-device.h>
 
 MODULE_LICENSE("GPL");
-
+MODULE_VERSION("0.0.4");
 
 #define MOTOROLA        1
 #define PHILIPS2        2               /* SAA7191 */
@@ -678,7 +677,6 @@ static int pms_querycap(struct file *file, void  *priv,
         strlcpy(vcap->driver, dev->v4l2_dev.name, sizeof(vcap->driver));
         strlcpy(vcap->card, "Mediavision PMS", sizeof(vcap->card));
         strlcpy(vcap->bus_info, "ISA", sizeof(vcap->bus_info));
-        vcap->version = KERNEL_VERSION(0, 0, 3);
         vcap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
         return 0;
 }
diff --git a/drivers/media/video/pvrusb2/pvrusb2-main.c b/drivers/media/video/pvrusb2/pvrusb2-main.c
index 2254194aad5..c1d9bb61cd7 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-main.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-main.c
@@ -168,6 +168,7 @@ module_exit(pvr_exit);
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
+MODULE_VERSION("0.9.1");
 
 
 /*
diff --git a/drivers/media/video/pvrusb2/pvrusb2-v4l2.c b/drivers/media/video/pvrusb2/pvrusb2-v4l2.c
index 38761142a4d..e27f8ab7696 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-v4l2.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-v4l2.c
@@ -91,7 +91,7 @@ static struct v4l2_capability pvr_capability ={
         .driver         = "pvrusb2",
         .card           = "Hauppauge WinTV pvr-usb2",
         .bus_info       = "usb",
-        .version        = KERNEL_VERSION(0, 9, 0),
+        .version        = LINUX_VERSION_CODE,
         .capabilities   = (V4L2_CAP_VIDEO_CAPTURE |
                            V4L2_CAP_TUNER | V4L2_CAP_AUDIO | V4L2_CAP_RADIO |
                            V4L2_CAP_READWRITE),
@@ -369,11 +369,6 @@ static long pvr2_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
                 break;
         }
 
-        case VIDIOC_S_AUDIO:
-        {
-                ret = -EINVAL;
-                break;
-        }
         case VIDIOC_G_TUNER:
         {
                 struct v4l2_tuner *vt = (struct v4l2_tuner *)arg;
@@ -850,7 +845,7 @@ static long pvr2_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 #endif
 
         default :
-                ret = -EINVAL;
+                ret = -ENOTTY;
                 break;
         }
 
diff --git a/drivers/media/video/pwc/Kconfig b/drivers/media/video/pwc/Kconfig
index 8da42e4f1ba..d63d0a85003 100644
--- a/drivers/media/video/pwc/Kconfig
+++ b/drivers/media/video/pwc/Kconfig
@@ -1,6 +1,7 @@
 config USB_PWC
         tristate "USB Philips Cameras"
         depends on VIDEO_V4L2
+        select VIDEOBUF2_VMALLOC
         ---help---
           Say Y or M here if you want to use one of these Philips & OEM
           webcams:
diff --git a/drivers/media/video/pwc/pwc-ctrl.c b/drivers/media/video/pwc/pwc-ctrl.c
index 760b4de13ad..3977addf3ba 100644
--- a/drivers/media/video/pwc/pwc-ctrl.c
+++ b/drivers/media/video/pwc/pwc-ctrl.c
@@ -3,6 +3,7 @@
    video modes.
    (C) 1999-2003 Nemosoft Unv.
    (C) 2004-2006 Luc Saillard (luc@saillard.org)
+   (C) 2011 Hans de Goede <hdegoede@redhat.com>
 
    NOTE: this version of pwc is an unofficial (modified) release of pwc & pcwx
    driver and thus may have bugs that are not present in the original version.
@@ -43,61 +44,12 @@
 #include <asm/errno.h>
 
 #include "pwc.h"
-#include "pwc-uncompress.h"
 #include "pwc-kiara.h"
 #include "pwc-timon.h"
 #include "pwc-dec1.h"
 #include "pwc-dec23.h"
 
-/* Request types: video */
-#define SET_LUM_CTL                     0x01
-#define GET_LUM_CTL                     0x02
-#define SET_CHROM_CTL                   0x03
-#define GET_CHROM_CTL                   0x04
-#define SET_STATUS_CTL                  0x05
-#define GET_STATUS_CTL                  0x06
-#define SET_EP_STREAM_CTL               0x07
-#define GET_EP_STREAM_CTL               0x08
-#define GET_XX_CTL                      0x09
-#define SET_XX_CTL                      0x0A
-#define GET_XY_CTL                      0x0B
-#define SET_XY_CTL                      0x0C
-#define SET_MPT_CTL                     0x0D
-#define GET_MPT_CTL                     0x0E
-
-/* Selectors for the Luminance controls [GS]ET_LUM_CTL */
-#define AGC_MODE_FORMATTER                      0x2000
-#define PRESET_AGC_FORMATTER                    0x2100
-#define SHUTTER_MODE_FORMATTER                  0x2200
-#define PRESET_SHUTTER_FORMATTER                0x2300
-#define PRESET_CONTOUR_FORMATTER                0x2400
-#define AUTO_CONTOUR_FORMATTER                  0x2500
-#define BACK_LIGHT_COMPENSATION_FORMATTER       0x2600
-#define CONTRAST_FORMATTER                      0x2700
-#define DYNAMIC_NOISE_CONTROL_FORMATTER         0x2800
-#define FLICKERLESS_MODE_FORMATTER              0x2900
-#define AE_CONTROL_SPEED                        0x2A00
-#define BRIGHTNESS_FORMATTER                    0x2B00
-#define GAMMA_FORMATTER                         0x2C00
-
-/* Selectors for the Chrominance controls [GS]ET_CHROM_CTL */
-#define WB_MODE_FORMATTER                       0x1000
-#define AWB_CONTROL_SPEED_FORMATTER             0x1100
-#define AWB_CONTROL_DELAY_FORMATTER             0x1200
-#define PRESET_MANUAL_RED_GAIN_FORMATTER        0x1300
-#define PRESET_MANUAL_BLUE_GAIN_FORMATTER       0x1400
-#define COLOUR_MODE_FORMATTER                   0x1500
-#define SATURATION_MODE_FORMATTER1              0x1600
-#define SATURATION_MODE_FORMATTER2              0x1700
-
-/* Selectors for the Status controls [GS]ET_STATUS_CTL */
-#define SAVE_USER_DEFAULTS_FORMATTER            0x0200
-#define RESTORE_USER_DEFAULTS_FORMATTER         0x0300
-#define RESTORE_FACTORY_DEFAULTS_FORMATTER      0x0400
-#define READ_AGC_FORMATTER                      0x0500
-#define READ_SHUTTER_FORMATTER                  0x0600
-#define READ_RED_GAIN_FORMATTER                 0x0700
-#define READ_BLUE_GAIN_FORMATTER                0x0800
+/* Selectors for status controls used only in this file */
 #define GET_STATUS_B00                          0x0B00
 #define SENSOR_TYPE_FORMATTER1                  0x0C00
 #define GET_STATUS_3000                         0x3000
@@ -116,11 +68,6 @@
 /* Formatters for the Video Endpoint controls [GS]ET_EP_STREAM_CTL */
 #define VIDEO_OUTPUT_CONTROL_FORMATTER          0x0100
 
-/* Formatters for the motorized pan & tilt [GS]ET_MPT_CTL */
-#define PT_RELATIVE_CONTROL_FORMATTER           0x01
-#define PT_RESET_CONTROL_FORMATTER              0x02
-#define PT_STATUS_FORMATTER                     0x03
-
 static const char *size2name[PSZ_MAX] =
 {
         "subQCIF",
@@ -160,7 +107,7 @@ static void pwc_set_image_buffer_size(struct pwc_device *pdev);
 /****************************************************************************/
 
 static int _send_control_msg(struct pwc_device *pdev,
-        u8 request, u16 value, int index, void *buf, int buflen, int timeout)
+        u8 request, u16 value, int index, void *buf, int buflen)
 {
         int rc;
         void *kbuf = NULL;
@@ -177,7 +124,7 @@ static int _send_control_msg(struct pwc_device *pdev,
                 USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
                 value,
                 index,
-                kbuf, buflen, timeout);
+                kbuf, buflen, USB_CTRL_SET_TIMEOUT);
 
         kfree(kbuf);
         return rc;
@@ -197,9 +144,13 @@ static int recv_control_msg(struct pwc_device *pdev,
                 USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
                 value,
                 pdev->vcinterface,
-                kbuf, buflen, 500);
+                kbuf, buflen, USB_CTRL_GET_TIMEOUT);
         memcpy(buf, kbuf, buflen);
         kfree(kbuf);
+
+        if (rc < 0)
+                PWC_ERROR("recv_control_msg error %d req %02x val %04x\n",
+                          rc, request, value);
         return rc;
 }
 
@@ -210,18 +161,16 @@ static inline int send_video_command(struct pwc_device *pdev,
                 SET_EP_STREAM_CTL,
                 VIDEO_OUTPUT_CONTROL_FORMATTER,
                 index,
-                buf, buflen, 1000);
+                buf, buflen);
 }
 
-static inline int send_control_msg(struct pwc_device *pdev,
+int send_control_msg(struct pwc_device *pdev,
         u8 request, u16 value, void *buf, int buflen)
 {
         return _send_control_msg(pdev,
-                request, value, pdev->vcinterface, buf, buflen, 500);
+                request, value, pdev->vcinterface, buf, buflen);
 }
 
-
-
 static int set_video_mode_Nala(struct pwc_device *pdev, int size, int frames)
 {
         unsigned char buf[3];
@@ -261,8 +210,11 @@ static int set_video_mode_Nala(struct pwc_device *pdev, int size, int frames)
                 PWC_DEBUG_MODULE("Failed to send video command... %d\n", ret);
                 return ret;
         }
-        if (pEntry->compressed && pdev->pixfmt == V4L2_PIX_FMT_YUV420)
-                pwc_dec1_init(pdev->type, pdev->release, buf, pdev->decompress_data);
+        if (pEntry->compressed && pdev->pixfmt == V4L2_PIX_FMT_YUV420) {
+                ret = pwc_dec1_init(pdev, pdev->type, pdev->release, buf);
+                if (ret < 0)
+                        return ret;
+        }
 
         pdev->cmd_len = 3;
         memcpy(pdev->cmd_buf, buf, 3);
@@ -321,8 +273,11 @@ static int set_video_mode_Timon(struct pwc_device *pdev, int size, int frames, i
         if (ret < 0)
                 return ret;
 
-        if (pChoose->bandlength > 0 && pdev->pixfmt == V4L2_PIX_FMT_YUV420)
-                pwc_dec23_init(pdev, pdev->type, buf);
+        if (pChoose->bandlength > 0 && pdev->pixfmt == V4L2_PIX_FMT_YUV420) {
+                ret = pwc_dec23_init(pdev, pdev->type, buf);
+                if (ret < 0)
+                        return ret;
+        }
 
         pdev->cmd_len = 13;
         memcpy(pdev->cmd_buf, buf, 13);
@@ -394,8 +349,11 @@ static int set_video_mode_Kiara(struct pwc_device *pdev, int size, int frames, i
         if (ret < 0)
                 return ret;
 
-        if (pChoose->bandlength > 0 && pdev->pixfmt == V4L2_PIX_FMT_YUV420)
-                pwc_dec23_init(pdev, pdev->type, buf);
+        if (pChoose->bandlength > 0 && pdev->pixfmt == V4L2_PIX_FMT_YUV420) {
+                ret = pwc_dec23_init(pdev, pdev->type, buf);
+                if (ret < 0)
+                        return ret;
+        }
 
         pdev->cmd_len = 12;
         memcpy(pdev->cmd_buf, buf, 12);
@@ -452,6 +410,7 @@ int pwc_set_video_mode(struct pwc_device *pdev, int width, int height, int frame
         }
         pdev->view.x = width;
         pdev->view.y = height;
+        pdev->vcompression = compression;
         pdev->frame_total_size = pdev->frame_size + pdev->frame_header_size + pdev->frame_trailer_size;
         pwc_set_image_buffer_size(pdev);
         PWC_DEBUG_SIZE("Set viewport to %dx%d, image size is %dx%d.\n", width, height, pwc_image_sizes[size].x, pwc_image_sizes[size].y);
@@ -511,13 +470,9 @@ unsigned int pwc_get_fps(struct pwc_device *pdev, unsigned int index, unsigned i
         return ret;
 }
 
-#define BLACK_Y 0
-#define BLACK_U 128
-#define BLACK_V 128
-
 static void pwc_set_image_buffer_size(struct pwc_device *pdev)
 {
-        int i, factor = 0;
+        int factor = 0;
 
         /* for V4L2_PIX_FMT_YUV420 */
         switch (pdev->pixfmt) {
@@ -541,442 +496,108 @@ static void pwc_set_image_buffer_size(struct pwc_device *pdev)
          */
         pdev->offset.x = ((pdev->view.x - pdev->image.x) / 2) & 0xFFFC;
         pdev->offset.y = ((pdev->view.y - pdev->image.y) / 2) & 0xFFFE;
-
-        /* Fill buffers with black colors */
-        for (i = 0; i < pwc_mbufs; i++) {
-                unsigned char *p = pdev->image_data + pdev->images[i].offset;
-                memset(p, BLACK_Y, pdev->view.x * pdev->view.y);
-                p += pdev->view.x * pdev->view.y;
-                memset(p, BLACK_U, pdev->view.x * pdev->view.y/4);
-                p += pdev->view.x * pdev->view.y/4;
-                memset(p, BLACK_V, pdev->view.x * pdev->view.y/4);
-        }
 }
 
-
-
-/* BRIGHTNESS */
-
-int pwc_get_brightness(struct pwc_device *pdev)
+int pwc_get_u8_ctrl(struct pwc_device *pdev, u8 request, u16 value, int *data)
 {
-        char buf;
         int ret;
+        u8 buf;
 
-        ret = recv_control_msg(pdev,
-                GET_LUM_CTL, BRIGHTNESS_FORMATTER, &buf, sizeof(buf));
+        ret = recv_control_msg(pdev, request, value, &buf, sizeof(buf));
         if (ret < 0)
                 return ret;
-        return buf;
-}
 
-int pwc_set_brightness(struct pwc_device *pdev, int value)
-{
-        char buf;
-
-        if (value < 0)
-                value = 0;
-        if (value > 0xffff)
-                value = 0xffff;
-        buf = (value >> 9) & 0x7f;
-        return send_control_msg(pdev,
-                SET_LUM_CTL, BRIGHTNESS_FORMATTER, &buf, sizeof(buf));
+        *data = buf;
+        return 0;
 }
 
-/* CONTRAST */
-
-int pwc_get_contrast(struct pwc_device *pdev)
+int pwc_set_u8_ctrl(struct pwc_device *pdev, u8 request, u16 value, u8 data)
 {
-        char buf;
         int ret;
 
-        ret = recv_control_msg(pdev,
-                GET_LUM_CTL, CONTRAST_FORMATTER, &buf, sizeof(buf));
+        ret = send_control_msg(pdev, request, value, &data, sizeof(data));
         if (ret < 0)
                 return ret;
-        return buf;
-}
 
-int pwc_set_contrast(struct pwc_device *pdev, int value)
-{
-        char buf;
-
-        if (value < 0)
-                value = 0;
-        if (value > 0xffff)
-                value = 0xffff;
-        buf = (value >> 10) & 0x3f;
-        return send_control_msg(pdev,
-                SET_LUM_CTL, CONTRAST_FORMATTER, &buf, sizeof(buf));
+        return 0;
 }
 
-/* GAMMA */
-
-int pwc_get_gamma(struct pwc_device *pdev)
+int pwc_get_s8_ctrl(struct pwc_device *pdev, u8 request, u16 value, int *data)
 {
-        char buf;
         int ret;
+        s8 buf;
 
-        ret = recv_control_msg(pdev,
-                GET_LUM_CTL, GAMMA_FORMATTER, &buf, sizeof(buf));
+        ret = recv_control_msg(pdev, request, value, &buf, sizeof(buf));
         if (ret < 0)
                 return ret;
-        return buf;
-}
-
-int pwc_set_gamma(struct pwc_device *pdev, int value)
-{
-        char buf;
 
-        if (value < 0)
-                value = 0;
-        if (value > 0xffff)
-                value = 0xffff;
-        buf = (value >> 11) & 0x1f;
-        return send_control_msg(pdev,
-                SET_LUM_CTL, GAMMA_FORMATTER, &buf, sizeof(buf));
-}
-
-
-/* SATURATION */
-
-/* return a value between [-100 , 100] */
-int pwc_get_saturation(struct pwc_device *pdev, int *value)
-{
-        char buf;
-        int ret, saturation_register;
-
-        if (pdev->type < 675)
-                return -EINVAL;
-        if (pdev->type < 730)
-                saturation_register = SATURATION_MODE_FORMATTER2;
-        else
-                saturation_register = SATURATION_MODE_FORMATTER1;
-        ret = recv_control_msg(pdev,
-                GET_CHROM_CTL, saturation_register, &buf, sizeof(buf));
-        if (ret < 0)
-                return ret;
-        *value = (signed)buf;
+        *data = buf;
         return 0;
 }
 
-/* @param value saturation color between [-100 , 100] */
-int pwc_set_saturation(struct pwc_device *pdev, int value)
+int pwc_get_u16_ctrl(struct pwc_device *pdev, u8 request, u16 value, int *data)
 {
-        char buf;
-        int saturation_register;
-
-        if (pdev->type < 675)
-                return -EINVAL;
-        if (value < -100)
-                value = -100;
-        if (value > 100)
-                value = 100;
-        if (pdev->type < 730)
-                saturation_register = SATURATION_MODE_FORMATTER2;
-        else
-                saturation_register = SATURATION_MODE_FORMATTER1;
-        return send_control_msg(pdev,
-                SET_CHROM_CTL, saturation_register, &buf, sizeof(buf));
-}
-
-/* AGC */
-
-int pwc_set_agc(struct pwc_device *pdev, int mode, int value)
-{
-        char buf;
         int ret;
+        u8 buf[2];
 
-        if (mode)
-                buf = 0x0; /* auto */
-        else
-                buf = 0xff; /* fixed */
-
-        ret = send_control_msg(pdev,
-                SET_LUM_CTL, AGC_MODE_FORMATTER, &buf, sizeof(buf));
-
-        if (!mode && ret >= 0) {
-                if (value < 0)
-                        value = 0;
-                if (value > 0xffff)
-                        value = 0xffff;
-                buf = (value >> 10) & 0x3F;
-                ret = send_control_msg(pdev,
-                        SET_LUM_CTL, PRESET_AGC_FORMATTER, &buf, sizeof(buf));
-        }
+        ret = recv_control_msg(pdev, request, value, buf, sizeof(buf));
         if (ret < 0)
                 return ret;
+
+        *data = (buf[1] << 8) | buf[0];
         return 0;
 }
 
-int pwc_get_agc(struct pwc_device *pdev, int *value)
+int pwc_set_u16_ctrl(struct pwc_device *pdev, u8 request, u16 value, u16 data)
 {
-        unsigned char buf;
         int ret;
+        u8 buf[2];
 
-        ret = recv_control_msg(pdev,
-                GET_LUM_CTL, AGC_MODE_FORMATTER, &buf, sizeof(buf));
+        buf[0] = data & 0xff;
+        buf[1] = data >> 8;
+        ret = send_control_msg(pdev, request, value, buf, sizeof(buf));
         if (ret < 0)
                 return ret;
 
-        if (buf != 0) { /* fixed */
-                ret = recv_control_msg(pdev,
-                        GET_LUM_CTL, PRESET_AGC_FORMATTER, &buf, sizeof(buf));
-                if (ret < 0)
-                        return ret;
-                if (buf > 0x3F)
-                        buf = 0x3F;
-                *value = (buf << 10);
-        }
-        else { /* auto */
-                ret = recv_control_msg(pdev,
-                        GET_STATUS_CTL, READ_AGC_FORMATTER, &buf, sizeof(buf));
-                if (ret < 0)
-                        return ret;
-                /* Gah... this value ranges from 0x00 ... 0x9F */
-                if (buf > 0x9F)
-                        buf = 0x9F;
-                *value = -(48 + buf * 409);
-        }
-
         return 0;
 }
 
-int pwc_set_shutter_speed(struct pwc_device *pdev, int mode, int value)
-{
-        char buf[2];
-        int speed, ret;
-
-
-        if (mode)
-                buf[0] = 0x0;   /* auto */
-        else
-                buf[0] = 0xff; /* fixed */
-
-        ret = send_control_msg(pdev,
-                SET_LUM_CTL, SHUTTER_MODE_FORMATTER, &buf, 1);
-
-        if (!mode && ret >= 0) {
-                if (value < 0)
-                        value = 0;
-                if (value > 0xffff)
-                        value = 0xffff;
-
-                if (DEVICE_USE_CODEC2(pdev->type)) {
-                        /* speed ranges from 0x0 to 0x290 (656) */
-                        speed = (value / 100);
-                        buf[1] = speed >> 8;
-                        buf[0] = speed & 0xff;
-                } else if (DEVICE_USE_CODEC3(pdev->type)) {
-                        /* speed seems to range from 0x0 to 0xff */
-                        buf[1] = 0;
-                        buf[0] = value >> 8;
-                }
-
-                ret = send_control_msg(pdev,
-                        SET_LUM_CTL, PRESET_SHUTTER_FORMATTER,
-                        &buf, sizeof(buf));
-        }
-        return ret;
-}
-
-/* This function is not exported to v4l1, so output values between 0 -> 256 */
-int pwc_get_shutter_speed(struct pwc_device *pdev, int *value)
+int pwc_button_ctrl(struct pwc_device *pdev, u16 value)
 {
-        unsigned char buf[2];
         int ret;
 
-        ret = recv_control_msg(pdev,
-                GET_STATUS_CTL, READ_SHUTTER_FORMATTER, &buf, sizeof(buf));
+        ret = send_control_msg(pdev, SET_STATUS_CTL, value, NULL, 0);
         if (ret < 0)
                 return ret;
-        *value = buf[0] + (buf[1] << 8);
-        if (DEVICE_USE_CODEC2(pdev->type)) {
-                /* speed ranges from 0x0 to 0x290 (656) */
-                *value *= 256/656;
-        } else if (DEVICE_USE_CODEC3(pdev->type)) {
-                /* speed seems to range from 0x0 to 0xff */
-        }
+
         return 0;
 }
 
-
 /* POWER */
-
-int pwc_camera_power(struct pwc_device *pdev, int power)
+void pwc_camera_power(struct pwc_device *pdev, int power)
 {
         char buf;
+        int r;
+
+        if (!pdev->power_save)
+                return;
 
         if (pdev->type < 675 || (pdev->type < 730 && pdev->release < 6))
-                return 0;       /* Not supported by Nala or Timon < release 6 */
+                return; /* Not supported by Nala or Timon < release 6 */
 
         if (power)
                 buf = 0x00; /* active */
         else
                 buf = 0xFF; /* power save */
-        return send_control_msg(pdev,
+        r = send_control_msg(pdev,
                 SET_STATUS_CTL, SET_POWER_SAVE_MODE_FORMATTER,
                 &buf, sizeof(buf));
-}
-
-
-
-/* private calls */
-
-int pwc_restore_user(struct pwc_device *pdev)
-{
-        return send_control_msg(pdev,
-                SET_STATUS_CTL, RESTORE_USER_DEFAULTS_FORMATTER, NULL, 0);
-}
-
-int pwc_save_user(struct pwc_device *pdev)
-{
-        return send_control_msg(pdev,
-                SET_STATUS_CTL, SAVE_USER_DEFAULTS_FORMATTER, NULL, 0);
-}
-
-int pwc_restore_factory(struct pwc_device *pdev)
-{
-        return send_control_msg(pdev,
-                SET_STATUS_CTL, RESTORE_FACTORY_DEFAULTS_FORMATTER, NULL, 0);
-}
-
- /* ************************************************* */
- /* Patch by Alvarado: (not in the original version   */
-
- /*
-  * the camera recognizes modes from 0 to 4:
-  *
-  * 00: indoor (incandescant lighting)
-  * 01: outdoor (sunlight)
-  * 02: fluorescent lighting
-  * 03: manual
-  * 04: auto
-  */
-int pwc_set_awb(struct pwc_device *pdev, int mode)
-{
-        char buf;
-        int ret;
-
-        if (mode < 0)
-            mode = 0;
-
-        if (mode > 4)
-            mode = 4;
-
-        buf = mode & 0x07; /* just the lowest three bits */
-
-        ret = send_control_msg(pdev,
-                SET_CHROM_CTL, WB_MODE_FORMATTER, &buf, sizeof(buf));
-
-        if (ret < 0)
-                return ret;
-        return 0;
-}
-
-int pwc_get_awb(struct pwc_device *pdev)
-{
-        unsigned char buf;
-        int ret;
-
-        ret = recv_control_msg(pdev,
-                GET_CHROM_CTL, WB_MODE_FORMATTER, &buf, sizeof(buf));
-
-        if (ret < 0)
-                return ret;
-        return buf;
-}
-
-int pwc_set_red_gain(struct pwc_device *pdev, int value)
-{
-        unsigned char buf;
-
-        if (value < 0)
-                value = 0;
-        if (value > 0xffff)
-                value = 0xffff;
-        /* only the msb is considered */
-        buf = value >> 8;
-        return send_control_msg(pdev,
-                SET_CHROM_CTL, PRESET_MANUAL_RED_GAIN_FORMATTER,
-                &buf, sizeof(buf));
-}
-
-int pwc_get_red_gain(struct pwc_device *pdev, int *value)
-{
-        unsigned char buf;
-        int ret;
-
-        ret = recv_control_msg(pdev,
-                GET_CHROM_CTL, PRESET_MANUAL_RED_GAIN_FORMATTER,
-                &buf, sizeof(buf));
-        if (ret < 0)
-            return ret;
-        *value = buf << 8;
-        return 0;
-}
-
-
-int pwc_set_blue_gain(struct pwc_device *pdev, int value)
-{
-        unsigned char buf;
-
-        if (value < 0)
-                value = 0;
-        if (value > 0xffff)
-                value = 0xffff;
-        /* only the msb is considered */
-        buf = value >> 8;
-        return send_control_msg(pdev,
-                SET_CHROM_CTL, PRESET_MANUAL_BLUE_GAIN_FORMATTER,
-                &buf, sizeof(buf));
-}
-
-int pwc_get_blue_gain(struct pwc_device *pdev, int *value)
-{
-        unsigned char buf;
-        int ret;
-
-        ret = recv_control_msg(pdev,
-                GET_CHROM_CTL, PRESET_MANUAL_BLUE_GAIN_FORMATTER,
-                &buf, sizeof(buf));
-        if (ret < 0)
-            return ret;
-        *value = buf << 8;
-        return 0;
-}
-
 
-/* The following two functions are different, since they only read the
-   internal red/blue gains, which may be different from the manual
-   gains set or read above.
- */
-static int pwc_read_red_gain(struct pwc_device *pdev, int *value)
-{
-        unsigned char buf;
-        int ret;
-
-        ret = recv_control_msg(pdev,
-                GET_STATUS_CTL, READ_RED_GAIN_FORMATTER, &buf, sizeof(buf));
-        if (ret < 0)
-                return ret;
-        *value = buf << 8;
-        return 0;
+        if (r < 0)
+                PWC_ERROR("Failed to power %s camera (%d)\n",
+                          power ? "on" : "off", r);
 }
 
-static int pwc_read_blue_gain(struct pwc_device *pdev, int *value)
-{
-        unsigned char buf;
-        int ret;
-
-        ret = recv_control_msg(pdev,
-                GET_STATUS_CTL, READ_BLUE_GAIN_FORMATTER, &buf, sizeof(buf));
-        if (ret < 0)
-                return ret;
-        *value = buf << 8;
-        return 0;
-}
-
-
 static int pwc_set_wb_speed(struct pwc_device *pdev, int speed)
 {
         unsigned char buf;
@@ -1028,6 +649,7 @@ static int pwc_get_wb_delay(struct pwc_device *pdev, int *value)
 int pwc_set_leds(struct pwc_device *pdev, int on_value, int off_value)
 {
         unsigned char buf[2];
+        int r;
 
         if (pdev->type < 730)
                 return 0;
@@ -1045,8 +667,12 @@ int pwc_set_leds(struct pwc_device *pdev, int on_value, int off_value)
         buf[0] = on_value;
         buf[1] = off_value;
 
-        return send_control_msg(pdev,
+        r = send_control_msg(pdev,
                 SET_STATUS_CTL, LED_FORMATTER, &buf, sizeof(buf));
+        if (r < 0)
+                PWC_ERROR("Failed to set LED on/off time (%d)\n", r);
+
+        return r;
 }
 
 static int pwc_get_leds(struct pwc_device *pdev, int *on_value, int *off_value)
@@ -1069,164 +695,6 @@ static int pwc_get_leds(struct pwc_device *pdev, int *on_value, int *off_value)
         return 0;
 }
 
-int pwc_set_contour(struct pwc_device *pdev, int contour)
-{
-        unsigned char buf;
-        int ret;
-
-        if (contour < 0)
-                buf = 0xff; /* auto contour on */
-        else
-                buf = 0x0; /* auto contour off */
-        ret = send_control_msg(pdev,
-                SET_LUM_CTL, AUTO_CONTOUR_FORMATTER, &buf, sizeof(buf));
-        if (ret < 0)
-                return ret;
-
-        if (contour < 0)
-                return 0;
-        if (contour > 0xffff)
-                contour = 0xffff;
-
-        buf = (contour >> 10); /* contour preset is [0..3f] */
-        ret = send_control_msg(pdev,
-                SET_LUM_CTL, PRESET_CONTOUR_FORMATTER, &buf, sizeof(buf));
-        if (ret < 0)
-                return ret;
-        return 0;
-}
-
-int pwc_get_contour(struct pwc_device *pdev, int *contour)
-{
-        unsigned char buf;
-        int ret;
-
-        ret = recv_control_msg(pdev,
-                GET_LUM_CTL, AUTO_CONTOUR_FORMATTER, &buf, sizeof(buf));
-        if (ret < 0)
-                return ret;
-
-        if (buf == 0) {
-                /* auto mode off, query current preset value */
-                ret = recv_control_msg(pdev,
-                        GET_LUM_CTL, PRESET_CONTOUR_FORMATTER,
-                        &buf, sizeof(buf));
-                if (ret < 0)
-                        return ret;
-                *contour = buf << 10;
-        }
-        else
-                *contour = -1;
-        return 0;
-}
-
-
-int pwc_set_backlight(struct pwc_device *pdev, int backlight)
-{
-        unsigned char buf;
-
-        if (backlight)
-                buf = 0xff;
-        else
-                buf = 0x0;
-        return send_control_msg(pdev,
-                SET_LUM_CTL, BACK_LIGHT_COMPENSATION_FORMATTER,
-                &buf, sizeof(buf));
-}
-
-int pwc_get_backlight(struct pwc_device *pdev, int *backlight)
-{
-        int ret;
-        unsigned char buf;
-
-        ret = recv_control_msg(pdev,
-                GET_LUM_CTL, BACK_LIGHT_COMPENSATION_FORMATTER,
-                &buf, sizeof(buf));
-        if (ret < 0)
-                return ret;
-        *backlight = !!buf;
-        return 0;
-}
-
-int pwc_set_colour_mode(struct pwc_device *pdev, int colour)
-{
-        unsigned char buf;
-
-        if (colour)
-                buf = 0xff;
-        else
-                buf = 0x0;
-        return send_control_msg(pdev,
-                SET_CHROM_CTL, COLOUR_MODE_FORMATTER, &buf, sizeof(buf));
-}
-
-int pwc_get_colour_mode(struct pwc_device *pdev, int *colour)
-{
-        int ret;
-        unsigned char buf;
-
-        ret = recv_control_msg(pdev,
-                GET_CHROM_CTL, COLOUR_MODE_FORMATTER, &buf, sizeof(buf));
-        if (ret < 0)
-                return ret;
-        *colour = !!buf;
-        return 0;
-}
-
-
-int pwc_set_flicker(struct pwc_device *pdev, int flicker)
-{
-        unsigned char buf;
-
-        if (flicker)
-                buf = 0xff;
-        else
-                buf = 0x0;
-        return send_control_msg(pdev,
-                SET_LUM_CTL, FLICKERLESS_MODE_FORMATTER, &buf, sizeof(buf));
-}
-
-int pwc_get_flicker(struct pwc_device *pdev, int *flicker)
-{
-        int ret;
-        unsigned char buf;
-
-        ret = recv_control_msg(pdev,
-                GET_LUM_CTL, FLICKERLESS_MODE_FORMATTER, &buf, sizeof(buf));
-        if (ret < 0)
-                return ret;
-        *flicker = !!buf;
-        return 0;
-}
-
-int pwc_set_dynamic_noise(struct pwc_device *pdev, int noise)
-{
-        unsigned char buf;
-
-        if (noise < 0)
-                noise = 0;
-        if (noise > 3)
-                noise = 3;
-        buf = noise;
-        return send_control_msg(pdev,
-                SET_LUM_CTL, DYNAMIC_NOISE_CONTROL_FORMATTER,
-                &buf, sizeof(buf));
-}
-
-int pwc_get_dynamic_noise(struct pwc_device *pdev, int *noise)
-{
-        int ret;
-        unsigned char buf;
-
-        ret = recv_control_msg(pdev,
-                GET_LUM_CTL, DYNAMIC_NOISE_CONTROL_FORMATTER,
-                &buf, sizeof(buf));
-        if (ret < 0)
-                return ret;
-        *noise = buf;
-        return 0;
-}
-
 static int _pwc_mpt_reset(struct pwc_device *pdev, int flags)
 {
         unsigned char buf;
@@ -1309,7 +777,7 @@ static int pwc_mpt_get_status(struct pwc_device *pdev, struct pwc_mpt_status *st
         return 0;
 }
 
-
+#ifdef CONFIG_USB_PWC_DEBUG
 int pwc_get_cmos_sensor(struct pwc_device *pdev, int *sensor)
 {
         unsigned char buf;
@@ -1332,7 +800,7 @@ int pwc_get_cmos_sensor(struct pwc_device *pdev, int *sensor)
                 *sensor = buf;
         return 0;
 }
-
+#endif
 
  /* End of Add-Ons                                    */
  /* ************************************************* */
@@ -1356,37 +824,41 @@ int pwc_get_cmos_sensor(struct pwc_device *pdev, int *sensor)
 /* copy local variable to arg */
 #define ARG_OUT(ARG_name) /* nothing */
 
+/*
+ * Our ctrls use native values, but the old custom pwc ioctl interface expects
+ * values from 0 - 65535, define 2 helper functions to scale things. */
+static int pwc_ioctl_g_ctrl(struct v4l2_ctrl *ctrl)
+{
+        return v4l2_ctrl_g_ctrl(ctrl) * 65535 / ctrl->maximum;
+}
+
+static int pwc_ioctl_s_ctrl(struct v4l2_ctrl *ctrl, int val)
+{
+        return v4l2_ctrl_s_ctrl(ctrl, val * ctrl->maximum / 65535);
+}
+
 long pwc_ioctl(struct pwc_device *pdev, unsigned int cmd, void *arg)
 {
         long ret = 0;
 
         switch(cmd) {
         case VIDIOCPWCRUSER:
-        {
-                if (pwc_restore_user(pdev))
-                        ret = -EINVAL;
+                ret = pwc_button_ctrl(pdev, RESTORE_USER_DEFAULTS_FORMATTER);
                 break;
-        }
 
         case VIDIOCPWCSUSER:
-        {
-                if (pwc_save_user(pdev))
-                        ret = -EINVAL;
+                ret = pwc_button_ctrl(pdev, SAVE_USER_DEFAULTS_FORMATTER);
                 break;
-        }
 
         case VIDIOCPWCFACTORY:
-        {
-                if (pwc_restore_factory(pdev))
-                        ret = -EINVAL;
+                ret = pwc_button_ctrl(pdev, RESTORE_FACTORY_DEFAULTS_FORMATTER);
                 break;
-        }
 
         case VIDIOCPWCSCQUAL:
         {
                 ARG_DEF(int, qual)
 
-                if (pdev->iso_init) {
+                if (vb2_is_streaming(&pdev->vb_queue)) {
                         ret = -EBUSY;
                         break;
                 }
@@ -1396,8 +868,6 @@ long pwc_ioctl(struct pwc_device *pdev, unsigned int cmd, void *arg)
                         ret = -EINVAL;
                 else
                         ret = pwc_set_video_mode(pdev, pdev->view.x, pdev->view.y, pdev->vframes, ARGR(qual), pdev->vsnapshot);
-                if (ret >= 0)
-                        pdev->vcompression = ARGR(qual);
                 break;
         }
 
@@ -1432,71 +902,59 @@ long pwc_ioctl(struct pwc_device *pdev, unsigned int cmd, void *arg)
         case VIDIOCPWCSAGC:
         {
                 ARG_DEF(int, agc)
-
                 ARG_IN(agc)
-                if (pwc_set_agc(pdev, ARGR(agc) < 0 ? 1 : 0, ARGR(agc)))
-                        ret = -EINVAL;
+                ret = v4l2_ctrl_s_ctrl(pdev->autogain, ARGR(agc) < 0);
+                if (ret == 0 && ARGR(agc) >= 0)
+                        ret = pwc_ioctl_s_ctrl(pdev->gain, ARGR(agc));
                 break;
         }
 
         case VIDIOCPWCGAGC:
         {
                 ARG_DEF(int, agc)
-
-                if (pwc_get_agc(pdev, ARGA(agc)))
-                        ret = -EINVAL;
+                if (v4l2_ctrl_g_ctrl(pdev->autogain))
+                        ARGR(agc) = -1;
+                else
+                        ARGR(agc) = pwc_ioctl_g_ctrl(pdev->gain);
                 ARG_OUT(agc)
                 break;
         }
 
         case VIDIOCPWCSSHUTTER:
         {
-                ARG_DEF(int, shutter_speed)
-
-                ARG_IN(shutter_speed)
-                ret = pwc_set_shutter_speed(pdev, ARGR(shutter_speed) < 0 ? 1 : 0, ARGR(shutter_speed));
+                ARG_DEF(int, shutter)
+                ARG_IN(shutter)
+                ret = v4l2_ctrl_s_ctrl(pdev->exposure_auto,
+                                       /* Menu idx 0 = auto, idx 1 = manual */
+                                       ARGR(shutter) >= 0);
+                if (ret == 0 && ARGR(shutter) >= 0)
+                        ret = pwc_ioctl_s_ctrl(pdev->exposure, ARGR(shutter));
                 break;
         }
 
         case VIDIOCPWCSAWB:
         {
                 ARG_DEF(struct pwc_whitebalance, wb)
-
                 ARG_IN(wb)
-                ret = pwc_set_awb(pdev, ARGR(wb).mode);
-                if (ret >= 0 && ARGR(wb).mode == PWC_WB_MANUAL) {
-                        pwc_set_red_gain(pdev, ARGR(wb).manual_red);
-                        pwc_set_blue_gain(pdev, ARGR(wb).manual_blue);
-                }
+                ret = v4l2_ctrl_s_ctrl(pdev->auto_white_balance,
+                                       ARGR(wb).mode);
+                if (ret == 0 && ARGR(wb).mode == PWC_WB_MANUAL)
+                        ret = pwc_ioctl_s_ctrl(pdev->red_balance,
+                                               ARGR(wb).manual_red);
+                if (ret == 0 && ARGR(wb).mode == PWC_WB_MANUAL)
+                        ret = pwc_ioctl_s_ctrl(pdev->blue_balance,
+                                               ARGR(wb).manual_blue);
                 break;
         }
 
         case VIDIOCPWCGAWB:
         {
                 ARG_DEF(struct pwc_whitebalance, wb)
-
-                memset(ARGA(wb), 0, sizeof(struct pwc_whitebalance));
-                ARGR(wb).mode = pwc_get_awb(pdev);
-                if (ARGR(wb).mode < 0)
-                        ret = -EINVAL;
-                else {
-                        if (ARGR(wb).mode == PWC_WB_MANUAL) {
-                                ret = pwc_get_red_gain(pdev, &ARGR(wb).manual_red);
-                                if (ret < 0)
-                                        break;
-                                ret = pwc_get_blue_gain(pdev, &ARGR(wb).manual_blue);
-                                if (ret < 0)
-                                        break;
-                        }
-                        if (ARGR(wb).mode == PWC_WB_AUTO) {
-                                ret = pwc_read_red_gain(pdev, &ARGR(wb).read_red);
-                                if (ret < 0)
-                                        break;
-                                ret = pwc_read_blue_gain(pdev, &ARGR(wb).read_blue);
-                                if (ret < 0)
-                                        break;
-                        }
-                }
+                ARGR(wb).mode = v4l2_ctrl_g_ctrl(pdev->auto_white_balance);
+                ARGR(wb).manual_red = ARGR(wb).read_red =
+                        pwc_ioctl_g_ctrl(pdev->red_balance);
+                ARGR(wb).manual_blue = ARGR(wb).read_blue =
+                        pwc_ioctl_g_ctrl(pdev->blue_balance);
                 ARG_OUT(wb)
                 break;
         }
@@ -1550,17 +1008,20 @@ long pwc_ioctl(struct pwc_device *pdev, unsigned int cmd, void *arg)
         case VIDIOCPWCSCONTOUR:
         {
                 ARG_DEF(int, contour)
-
                 ARG_IN(contour)
-                ret = pwc_set_contour(pdev, ARGR(contour));
+                ret = v4l2_ctrl_s_ctrl(pdev->autocontour, ARGR(contour) < 0);
+                if (ret == 0 && ARGR(contour) >= 0)
+                        ret = pwc_ioctl_s_ctrl(pdev->contour, ARGR(contour));
                 break;
         }
 
         case VIDIOCPWCGCONTOUR:
         {
                 ARG_DEF(int, contour)
-
-                ret = pwc_get_contour(pdev, ARGA(contour));
+                if (v4l2_ctrl_g_ctrl(pdev->autocontour))
+                        ARGR(contour) = -1;
+                else
+                        ARGR(contour) = pwc_ioctl_g_ctrl(pdev->contour);
                 ARG_OUT(contour)
                 break;
         }
@@ -1568,17 +1029,15 @@ long pwc_ioctl(struct pwc_device *pdev, unsigned int cmd, void *arg)
         case VIDIOCPWCSBACKLIGHT:
         {
                 ARG_DEF(int, backlight)
-
                 ARG_IN(backlight)
-                ret = pwc_set_backlight(pdev, ARGR(backlight));
+                ret = v4l2_ctrl_s_ctrl(pdev->backlight, ARGR(backlight));
                 break;
         }
 
         case VIDIOCPWCGBACKLIGHT:
         {
                 ARG_DEF(int, backlight)
-
-                ret = pwc_get_backlight(pdev, ARGA(backlight));
+                ARGR(backlight) = v4l2_ctrl_g_ctrl(pdev->backlight);
                 ARG_OUT(backlight)
                 break;
         }
@@ -1586,17 +1045,15 @@ long pwc_ioctl(struct pwc_device *pdev, unsigned int cmd, void *arg)
         case VIDIOCPWCSFLICKER:
         {
                 ARG_DEF(int, flicker)
-
                 ARG_IN(flicker)
-                ret = pwc_set_flicker(pdev, ARGR(flicker));
+                ret = v4l2_ctrl_s_ctrl(pdev->flicker, ARGR(flicker));
                 break;
         }
 
         case VIDIOCPWCGFLICKER:
         {
                 ARG_DEF(int, flicker)
-
-                ret = pwc_get_flicker(pdev, ARGA(flicker));
+                ARGR(flicker) = v4l2_ctrl_g_ctrl(pdev->flicker);
                 ARG_OUT(flicker)
                 break;
         }
@@ -1604,17 +1061,15 @@ long pwc_ioctl(struct pwc_device *pdev, unsigned int cmd, void *arg)
         case VIDIOCPWCSDYNNOISE:
         {
                 ARG_DEF(int, dynnoise)
-
                 ARG_IN(dynnoise)
-                ret = pwc_set_dynamic_noise(pdev, ARGR(dynnoise));
+                ret = v4l2_ctrl_s_ctrl(pdev->noise_reduction, ARGR(dynnoise));
                 break;
         }
 
         case VIDIOCPWCGDYNNOISE:
         {
                 ARG_DEF(int, dynnoise)
-
-                ret = pwc_get_dynamic_noise(pdev, ARGA(dynnoise));
+                ARGR(dynnoise) = v4l2_ctrl_g_ctrl(pdev->noise_reduction);
                 ARG_OUT(dynnoise);
                 break;
         }
diff --git a/drivers/media/video/pwc/pwc-dec1.c b/drivers/media/video/pwc/pwc-dec1.c
index c29593f589e..be0e02cb487 100644
--- a/drivers/media/video/pwc/pwc-dec1.c
+++ b/drivers/media/video/pwc/pwc-dec1.c
@@ -22,29 +22,19 @@
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */
-
-
-
 #include "pwc-dec1.h"
 
-
-void pwc_dec1_init(int type, int release, void *buffer, void *table)
+int pwc_dec1_init(struct pwc_device *pwc, int type, int release, void *buffer)
 {
+        struct pwc_dec1_private *pdec;
 
-}
-
-void pwc_dec1_exit(void)
-{
+        if (pwc->decompress_data == NULL) {
+                pdec = kmalloc(sizeof(struct pwc_dec1_private), GFP_KERNEL);
+                if (pdec == NULL)
+                        return -ENOMEM;
+                pwc->decompress_data = pdec;
+        }
+        pdec = pwc->decompress_data;
 
-
-
-}
-
-int pwc_dec1_alloc(struct pwc_device *pwc)
-{
-        pwc->decompress_data = kmalloc(sizeof(struct pwc_dec1_private), GFP_KERNEL);
-        if (pwc->decompress_data == NULL)
-                return -ENOMEM;
         return 0;
 }
-
diff --git a/drivers/media/video/pwc/pwc-dec1.h b/drivers/media/video/pwc/pwc-dec1.h
index 8b62ddcc5c7..a57d8601080 100644
--- a/drivers/media/video/pwc/pwc-dec1.h
+++ b/drivers/media/video/pwc/pwc-dec1.h
@@ -22,8 +22,6 @@
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */
 
-
-
 #ifndef PWC_DEC1_H
 #define PWC_DEC1_H
 
@@ -32,12 +30,8 @@
 struct pwc_dec1_private
 {
         int version;
-
 };
 
-int  pwc_dec1_alloc(struct pwc_device *pwc);
-void pwc_dec1_init(int type, int release, void *buffer, void *private_data);
-void pwc_dec1_exit(void);
+int pwc_dec1_init(struct pwc_device *pwc, int type, int release, void *buffer);
 
 #endif
-
diff --git a/drivers/media/video/pwc/pwc-dec23.c b/drivers/media/video/pwc/pwc-dec23.c
index 0c801b8f3ec..06a4e877ba4 100644
--- a/drivers/media/video/pwc/pwc-dec23.c
+++ b/drivers/media/video/pwc/pwc-dec23.c
@@ -916,27 +916,5 @@ void pwc_dec23_decompress(const struct pwc_device *pwc,
                         pout_planar_v += pwc->view.x;
 
                 }
-
         }
-
 }
-
-void pwc_dec23_exit(void)
-{
-        /* Do nothing */
-
-}
-
-/**
- * Allocate a private structure used by lookup table.
- * You must call kfree() to free the memory allocated.
- */
-int pwc_dec23_alloc(struct pwc_device *pwc)
-{
-        pwc->decompress_data = kmalloc(sizeof(struct pwc_dec23_private), GFP_KERNEL);
-        if (pwc->decompress_data == NULL)
-                return -ENOMEM;
-        return 0;
-}
-
-/* vim: set cino= formatoptions=croql cindent shiftwidth=8 tabstop=8: */
diff --git a/drivers/media/video/pwc/pwc-dec23.h b/drivers/media/video/pwc/pwc-dec23.h
index 1c55298ad15..a0ac4f3dff8 100644
--- a/drivers/media/video/pwc/pwc-dec23.h
+++ b/drivers/media/video/pwc/pwc-dec23.h
@@ -49,19 +49,9 @@ struct pwc_dec23_private
 
 };
 
-
-int pwc_dec23_alloc(struct pwc_device *pwc);
 int pwc_dec23_init(struct pwc_device *pwc, int type, unsigned char *cmd);
-void pwc_dec23_exit(void);
 void pwc_dec23_decompress(const struct pwc_device *pwc,
                           const void *src,
                           void *dst,
                           int flags);
-
-
-
 #endif
-
-
-/* vim: set cino= formatoptions=croql cindent shiftwidth=8 tabstop=8: */
-
diff --git a/drivers/media/video/pwc/pwc-if.c b/drivers/media/video/pwc/pwc-if.c
index b0bde5a87c8..51ca3589b1b 100644
--- a/drivers/media/video/pwc/pwc-if.c
+++ b/drivers/media/video/pwc/pwc-if.c
@@ -2,6 +2,7 @@
    USB and Video4Linux interface part.
    (C) 1999-2004 Nemosoft Unv.
    (C) 2004-2006 Luc Saillard (luc@saillard.org)
+   (C) 2011 Hans de Goede <hdegoede@redhat.com>
 
    NOTE: this version of pwc is an unofficial (modified) release of pwc & pcwx
    driver and thus may have bugs that are not present in the original version.
@@ -74,7 +75,6 @@
 #include "pwc-timon.h"
 #include "pwc-dec23.h"
 #include "pwc-dec1.h"
-#include "pwc-uncompress.h"
 
 /* Function prototypes and driver templates */
 
@@ -116,6 +116,7 @@ MODULE_DEVICE_TABLE(usb, pwc_device_table);
 
 static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id *id);
 static void usb_pwc_disconnect(struct usb_interface *intf);
+static void pwc_isoc_cleanup(struct pwc_device *pdev);
 
 static struct usb_driver pwc_driver = {
         .name =                 "Philips webcam",       /* name */
@@ -127,14 +128,11 @@ static struct usb_driver pwc_driver = {
 #define MAX_DEV_HINTS   20
 #define MAX_ISOC_ERRORS 20
 
-static int default_size = PSZ_QCIF;
 static int default_fps = 10;
-static int default_fbufs = 3;   /* Default number of frame buffers */
-        int pwc_mbufs = 2;      /* Default number of mmap() buffers */
 #ifdef CONFIG_USB_PWC_DEBUG
         int pwc_trace = PWC_DEBUG_LEVEL;
 #endif
-static int power_save;
+static int power_save = -1;
 static int led_on = 100, led_off; /* defaults to LED that is on while in use */
 static int pwc_preferred_compression = 1; /* 0..3 = uncompressed..high */
 static struct {
@@ -173,389 +171,20 @@ static struct video_device pwc_template = {
 /***************************************************************************/
 /* Private functions */
 
-/* Here we want the physical address of the memory.
- * This is used when initializing the contents of the area.
- */
-
-
-
-static void *pwc_rvmalloc(unsigned long size)
-{
-        void * mem;
-        unsigned long adr;
-
-        mem=vmalloc_32(size);
-        if (!mem)
-                return NULL;
-
-        memset(mem, 0, size); /* Clear the ram out, no junk to the user */
-        adr=(unsigned long) mem;
-        while (size > 0)
-         {
-           SetPageReserved(vmalloc_to_page((void *)adr));
-           adr  += PAGE_SIZE;
-           size -= PAGE_SIZE;
-         }
-        return mem;
-}
-
-static void pwc_rvfree(void * mem, unsigned long size)
-{
-        unsigned long adr;
-
-        if (!mem)
-                return;
-
-        adr=(unsigned long) mem;
-        while ((long) size > 0)
-         {
-           ClearPageReserved(vmalloc_to_page((void *)adr));
-           adr  += PAGE_SIZE;
-           size -= PAGE_SIZE;
-         }
-        vfree(mem);
-}
-
-
-
-
-static int pwc_allocate_buffers(struct pwc_device *pdev)
-{
-        int i, err;
-        void *kbuf;
-
-        PWC_DEBUG_MEMORY(">> pwc_allocate_buffers(pdev = 0x%p)\n", pdev);
-
-        if (pdev == NULL)
-                return -ENXIO;
-
-        /* Allocate Isochronuous pipe buffers */
-        for (i = 0; i < MAX_ISO_BUFS; i++) {
-                if (pdev->sbuf[i].data == NULL) {
-                        kbuf = kzalloc(ISO_BUFFER_SIZE, GFP_KERNEL);
-                        if (kbuf == NULL) {
-                                PWC_ERROR("Failed to allocate iso buffer %d.\n", i);
-                                return -ENOMEM;
-                        }
-                        PWC_DEBUG_MEMORY("Allocated iso buffer at %p.\n", kbuf);
-                        pdev->sbuf[i].data = kbuf;
-                }
-        }
-
-        /* Allocate frame buffer structure */
-        if (pdev->fbuf == NULL) {
-                kbuf = kzalloc(default_fbufs * sizeof(struct pwc_frame_buf), GFP_KERNEL);
-                if (kbuf == NULL) {
-                        PWC_ERROR("Failed to allocate frame buffer structure.\n");
-                        return -ENOMEM;
-                }
-                PWC_DEBUG_MEMORY("Allocated frame buffer structure at %p.\n", kbuf);
-                pdev->fbuf = kbuf;
-        }
-
-        /* create frame buffers, and make circular ring */
-        for (i = 0; i < default_fbufs; i++) {
-                if (pdev->fbuf[i].data == NULL) {
-                        kbuf = vzalloc(PWC_FRAME_SIZE); /* need vmalloc since frame buffer > 128K */
-                        if (kbuf == NULL) {
-                                PWC_ERROR("Failed to allocate frame buffer %d.\n", i);
-                                return -ENOMEM;
-                        }
-                        PWC_DEBUG_MEMORY("Allocated frame buffer %d at %p.\n", i, kbuf);
-                        pdev->fbuf[i].data = kbuf;
-                }
-        }
-
-        /* Allocate decompressor table space */
-        if (DEVICE_USE_CODEC1(pdev->type))
-                err = pwc_dec1_alloc(pdev);
-        else
-                err = pwc_dec23_alloc(pdev);
-
-        if (err) {
-                PWC_ERROR("Failed to allocate decompress table.\n");
-                return err;
-        }
-
-        /* Allocate image buffer; double buffer for mmap() */
-        kbuf = pwc_rvmalloc(pwc_mbufs * pdev->len_per_image);
-        if (kbuf == NULL) {
-                PWC_ERROR("Failed to allocate image buffer(s). needed (%d)\n",
-                                pwc_mbufs * pdev->len_per_image);
-                return -ENOMEM;
-        }
-        PWC_DEBUG_MEMORY("Allocated image buffer at %p.\n", kbuf);
-        pdev->image_data = kbuf;
-        for (i = 0; i < pwc_mbufs; i++) {
-                pdev->images[i].offset = i * pdev->len_per_image;
-                pdev->images[i].vma_use_count = 0;
-        }
-        for (; i < MAX_IMAGES; i++) {
-                pdev->images[i].offset = 0;
-        }
-
-        kbuf = NULL;
-
-        PWC_DEBUG_MEMORY("<< pwc_allocate_buffers()\n");
-        return 0;
-}
-
-static void pwc_free_buffers(struct pwc_device *pdev)
-{
-        int i;
-
-        PWC_DEBUG_MEMORY("Entering free_buffers(%p).\n", pdev);
-
-        if (pdev == NULL)
-                return;
-        /* Release Iso-pipe buffers */
-        for (i = 0; i < MAX_ISO_BUFS; i++)
-                if (pdev->sbuf[i].data != NULL) {
-                        PWC_DEBUG_MEMORY("Freeing ISO buffer at %p.\n", pdev->sbuf[i].data);
-                        kfree(pdev->sbuf[i].data);
-                        pdev->sbuf[i].data = NULL;
-                }
-
-        /* The same for frame buffers */
-        if (pdev->fbuf != NULL) {
-                for (i = 0; i < default_fbufs; i++) {
-                        if (pdev->fbuf[i].data != NULL) {
-                                PWC_DEBUG_MEMORY("Freeing frame buffer %d at %p.\n", i, pdev->fbuf[i].data);
-                                vfree(pdev->fbuf[i].data);
-                                pdev->fbuf[i].data = NULL;
-                        }
-                }
-                kfree(pdev->fbuf);
-                pdev->fbuf = NULL;
-        }
-
-        /* Intermediate decompression buffer & tables */
-        if (pdev->decompress_data != NULL) {
-                PWC_DEBUG_MEMORY("Freeing decompression buffer at %p.\n", pdev->decompress_data);
-                kfree(pdev->decompress_data);
-                pdev->decompress_data = NULL;
-        }
-
-        /* Release image buffers */
-        if (pdev->image_data != NULL) {
-                PWC_DEBUG_MEMORY("Freeing image buffer at %p.\n", pdev->image_data);
-                pwc_rvfree(pdev->image_data, pwc_mbufs * pdev->len_per_image);
-        }
-        pdev->image_data = NULL;
-
-        PWC_DEBUG_MEMORY("Leaving free_buffers().\n");
-}
-
-/* The frame & image buffer mess.
-
-   Yes, this is a mess. Well, it used to be simple, but alas...  In this
-   module, 3 buffers schemes are used to get the data from the USB bus to
-   the user program. The first scheme involves the ISO buffers (called thus
-   since they transport ISO data from the USB controller), and not really
-   interesting. Suffices to say the data from this buffer is quickly
-   gathered in an interrupt handler (pwc_isoc_handler) and placed into the
-   frame buffer.
-
-   The frame buffer is the second scheme, and is the central element here.
-   It collects the data from a single frame from the camera (hence, the
-   name). Frames are delimited by the USB camera with a short USB packet,
-   so that's easy to detect. The frame buffers form a list that is filled
-   by the camera+USB controller and drained by the user process through
-   either read() or mmap().
-
-   The image buffer is the third scheme, in which frames are decompressed
-   and converted into planar format. For mmap() there is more than
-   one image buffer available.
-
-   The frame buffers provide the image buffering. In case the user process
-   is a bit slow, this introduces lag and some undesired side-effects.
-   The problem arises when the frame buffer is full. I used to drop the last
-   frame, which makes the data in the queue stale very quickly. But dropping
-   the frame at the head of the queue proved to be a litte bit more difficult.
-   I tried a circular linked scheme, but this introduced more problems than
-   it solved.
-
-   Because filling and draining are completely asynchronous processes, this
-   requires some fiddling with pointers and mutexes.
-
-   Eventually, I came up with a system with 2 lists: an 'empty' frame list
-   and a 'full' frame list:
-     * Initially, all frame buffers but one are on the 'empty' list; the one
-       remaining buffer is our initial fill frame.
-     * If a frame is needed for filling, we try to take it from the 'empty'
-       list, unless that list is empty, in which case we take the buffer at
-       the head of the 'full' list.
-     * When our fill buffer has been filled, it is appended to the 'full'
-       list.
-     * If a frame is needed by read() or mmap(), it is taken from the head of
-       the 'full' list, handled, and then appended to the 'empty' list. If no
-       buffer is present on the 'full' list, we wait.
-   The advantage is that the buffer that is currently being decompressed/
-   converted, is on neither list, and thus not in our way (any other scheme
-   I tried had the problem of old data lingering in the queue).
-
-   Whatever strategy you choose, it always remains a tradeoff: with more
-   frame buffers the chances of a missed frame are reduced. On the other
-   hand, on slower machines it introduces lag because the queue will
-   always be full.
- */
-
-/**
-  \brief Find next frame buffer to fill. Take from empty or full list, whichever comes first.
- */
-static int pwc_next_fill_frame(struct pwc_device *pdev)
-{
-        int ret;
-        unsigned long flags;
-
-        ret = 0;
-        spin_lock_irqsave(&pdev->ptrlock, flags);
-        if (pdev->fill_frame != NULL) {
-                /* append to 'full' list */
-                if (pdev->full_frames == NULL) {
-                        pdev->full_frames = pdev->fill_frame;
-                        pdev->full_frames_tail = pdev->full_frames;
-                }
-                else {
-                        pdev->full_frames_tail->next = pdev->fill_frame;
-                        pdev->full_frames_tail = pdev->fill_frame;
-                }
-        }
-        if (pdev->empty_frames != NULL) {
-                /* We have empty frames available. That's easy */
-                pdev->fill_frame = pdev->empty_frames;
-                pdev->empty_frames = pdev->empty_frames->next;
-        }
-        else {
-                /* Hmm. Take it from the full list */
-                /* sanity check */
-                if (pdev->full_frames == NULL) {
-                        PWC_ERROR("Neither empty or full frames available!\n");
-                        spin_unlock_irqrestore(&pdev->ptrlock, flags);
-                        return -EINVAL;
-                }
-                pdev->fill_frame = pdev->full_frames;
-                pdev->full_frames = pdev->full_frames->next;
-                ret = 1;
-        }
-        pdev->fill_frame->next = NULL;
-        spin_unlock_irqrestore(&pdev->ptrlock, flags);
-        return ret;
-}
-
-
-/**
-  \brief Reset all buffers, pointers and lists, except for the image_used[] buffer.
-
-  If the image_used[] buffer is cleared too, mmap()/VIDIOCSYNC will run into trouble.
- */
-static void pwc_reset_buffers(struct pwc_device *pdev)
-{
-        int i;
-        unsigned long flags;
-
-        PWC_DEBUG_MEMORY(">> %s __enter__\n", __func__);
-
-        spin_lock_irqsave(&pdev->ptrlock, flags);
-        pdev->full_frames = NULL;
-        pdev->full_frames_tail = NULL;
-        for (i = 0; i < default_fbufs; i++) {
-                pdev->fbuf[i].filled = 0;
-                if (i > 0)
-                        pdev->fbuf[i].next = &pdev->fbuf[i - 1];
-                else
-                        pdev->fbuf->next = NULL;
-        }
-        pdev->empty_frames = &pdev->fbuf[default_fbufs - 1];
-        pdev->empty_frames_tail = pdev->fbuf;
-        pdev->read_frame = NULL;
-        pdev->fill_frame = pdev->empty_frames;
-        pdev->empty_frames = pdev->empty_frames->next;
-
-        pdev->image_read_pos = 0;
-        pdev->fill_image = 0;
-        spin_unlock_irqrestore(&pdev->ptrlock, flags);
-
-        PWC_DEBUG_MEMORY("<< %s __leaving__\n", __func__);
-}
-
-
-/**
-  \brief Do all the handling for getting one frame: get pointer, decompress, advance pointers.
- */
-int pwc_handle_frame(struct pwc_device *pdev)
+struct pwc_frame_buf *pwc_get_next_fill_buf(struct pwc_device *pdev)
 {
-        int ret = 0;
-        unsigned long flags;
-
-        spin_lock_irqsave(&pdev->ptrlock, flags);
-        /* First grab our read_frame; this is removed from all lists, so
-           we can release the lock after this without problems */
-        if (pdev->read_frame != NULL) {
-                /* This can't theoretically happen */
-                PWC_ERROR("Huh? Read frame still in use?\n");
-                spin_unlock_irqrestore(&pdev->ptrlock, flags);
-                return ret;
-        }
-
-
-        if (pdev->full_frames == NULL) {
-                PWC_ERROR("Woops. No frames ready.\n");
-        }
-        else {
-                pdev->read_frame = pdev->full_frames;
-                pdev->full_frames = pdev->full_frames->next;
-                pdev->read_frame->next = NULL;
-        }
-
-        if (pdev->read_frame != NULL) {
-                /* Decompression is a lengthy process, so it's outside of the lock.
-                   This gives the isoc_handler the opportunity to fill more frames
-                   in the mean time.
-                */
-                spin_unlock_irqrestore(&pdev->ptrlock, flags);
-                ret = pwc_decompress(pdev);
-                spin_lock_irqsave(&pdev->ptrlock, flags);
-
-                /* We're done with read_buffer, tack it to the end of the empty buffer list */
-                if (pdev->empty_frames == NULL) {
-                        pdev->empty_frames = pdev->read_frame;
-                        pdev->empty_frames_tail = pdev->empty_frames;
-                }
-                else {
-                        pdev->empty_frames_tail->next = pdev->read_frame;
-                        pdev->empty_frames_tail = pdev->read_frame;
-                }
-                pdev->read_frame = NULL;
-        }
-        spin_unlock_irqrestore(&pdev->ptrlock, flags);
-        return ret;
-}
-
-/**
-  \brief Advance pointers of image buffer (after each user request)
-*/
-void pwc_next_image(struct pwc_device *pdev)
-{
-        pdev->image_used[pdev->fill_image] = 0;
-        pdev->fill_image = (pdev->fill_image + 1) % pwc_mbufs;
-}
-
-/**
- * Print debug information when a frame is discarded because all of our buffer
- * is full
- */
-static void pwc_frame_dumped(struct pwc_device *pdev)
-{
-        pdev->vframes_dumped++;
-        if (pdev->vframe_count < FRAME_LOWMARK)
-                return;
-
-        if (pdev->vframes_dumped < 20)
-                PWC_DEBUG_FLOW("Dumping frame %d\n", pdev->vframe_count);
-        else if (pdev->vframes_dumped == 20)
-                PWC_DEBUG_FLOW("Dumping frame %d (last message)\n",
-                                pdev->vframe_count);
+        unsigned long flags = 0;
+        struct pwc_frame_buf *buf = NULL;
+
+        spin_lock_irqsave(&pdev->queued_bufs_lock, flags);
+        if (list_empty(&pdev->queued_bufs))
+                goto leave;
+
+        buf = list_entry(pdev->queued_bufs.next, struct pwc_frame_buf, list);
+        list_del(&buf->list);
+leave:
+        spin_unlock_irqrestore(&pdev->queued_bufs_lock, flags);
+        return buf;
 }
 
 static void pwc_snapshot_button(struct pwc_device *pdev, int down)
@@ -575,9 +204,9 @@ static void pwc_snapshot_button(struct pwc_device *pdev, int down)
 #endif
 }
 
-static int pwc_rcv_short_packet(struct pwc_device *pdev, const struct pwc_frame_buf *fbuf)
+static void pwc_frame_complete(struct pwc_device *pdev)
 {
-        int awake = 0;
+        struct pwc_frame_buf *fbuf = pdev->fill_buf;
 
         /* The ToUCam Fun CMOS sensor causes the firmware to send 2 or 3 bogus
            frames on the USB wire after an exposure change. This conditition is
@@ -589,7 +218,6 @@ static int pwc_rcv_short_packet(struct pwc_device *pdev, const struct pwc_frame_
                 if (ptr[1] == 1 && ptr[0] & 0x10) {
                         PWC_TRACE("Hyundai CMOS sensor bug. Dropping frame.\n");
                         pdev->drop_frames += 2;
-                        pdev->vframes_error++;
                 }
                 if ((ptr[0] ^ pdev->vmirror) & 0x01) {
                         pwc_snapshot_button(pdev, ptr[0] & 0x01);
@@ -612,8 +240,7 @@ static int pwc_rcv_short_packet(struct pwc_device *pdev, const struct pwc_frame_
                    */
                 if (fbuf->filled == 4)
                         pdev->drop_frames++;
-        }
-        else if (pdev->type == 740 || pdev->type == 720) {
+        } else if (pdev->type == 740 || pdev->type == 720) {
                 unsigned char *ptr = (unsigned char *)fbuf->data;
                 if ((ptr[0] ^ pdev->vmirror) & 0x01) {
                         pwc_snapshot_button(pdev, ptr[0] & 0x01);
@@ -621,33 +248,23 @@ static int pwc_rcv_short_packet(struct pwc_device *pdev, const struct pwc_frame_
                 pdev->vmirror = ptr[0] & 0x03;
         }
 
-        /* In case we were instructed to drop the frame, do so silently.
-           The buffer pointers are not updated either (but the counters are reset below).
-           */
-        if (pdev->drop_frames > 0)
+        /* In case we were instructed to drop the frame, do so silently. */
+        if (pdev->drop_frames > 0) {
                 pdev->drop_frames--;
-        else {
+        } else {
                 /* Check for underflow first */
                 if (fbuf->filled < pdev->frame_total_size) {
                         PWC_DEBUG_FLOW("Frame buffer underflow (%d bytes);"
                                        " discarded.\n", fbuf->filled);
-                        pdev->vframes_error++;
-                }
-                else {
-                        /* Send only once per EOF */
-                        awake = 1; /* delay wake_ups */
-
-                        /* Find our next frame to fill. This will always succeed, since we
-                         * nick a frame from either empty or full list, but if we had to
-                         * take it from the full list, it means a frame got dropped.
-                         */
-                        if (pwc_next_fill_frame(pdev))
-                                pwc_frame_dumped(pdev);
-
+                } else {
+                        fbuf->vb.v4l2_buf.field = V4L2_FIELD_NONE;
+                        fbuf->vb.v4l2_buf.sequence = pdev->vframe_count;
+                        vb2_buffer_done(&fbuf->vb, VB2_BUF_STATE_DONE);
+                        pdev->fill_buf = NULL;
+                        pdev->vsync = 0;
                 }
         } /* !drop_frames */
         pdev->vframe_count++;
-        return awake;
 }
 
 /* This gets called for the Isochronous pipe (video). This is done in
@@ -655,24 +272,20 @@ static int pwc_rcv_short_packet(struct pwc_device *pdev, const struct pwc_frame_
  */
 static void pwc_isoc_handler(struct urb *urb)
 {
-        struct pwc_device *pdev;
+        struct pwc_device *pdev = (struct pwc_device *)urb->context;
         int i, fst, flen;
-        int awake;
-        struct pwc_frame_buf *fbuf;
-        unsigned char *fillptr = NULL, *iso_buf = NULL;
+        unsigned char *iso_buf = NULL;
 
-        awake = 0;
-        pdev = (struct pwc_device *)urb->context;
-        if (pdev == NULL) {
-                PWC_ERROR("isoc_handler() called with NULL device?!\n");
-                return;
-        }
-
-        if (urb->status == -ENOENT || urb->status == -ECONNRESET) {
+        if (urb->status == -ENOENT || urb->status == -ECONNRESET ||
+            urb->status == -ESHUTDOWN) {
                 PWC_DEBUG_OPEN("URB (%p) unlinked %ssynchronuously.\n", urb, urb->status == -ENOENT ? "" : "a");
                 return;
         }
-        if (urb->status != -EINPROGRESS && urb->status != 0) {
+
+        if (pdev->fill_buf == NULL)
+                pdev->fill_buf = pwc_get_next_fill_buf(pdev);
+
+        if (urb->status != 0) {
                 const char *errmsg;
 
                 errmsg = "Unknown";
@@ -684,29 +297,21 @@ static void pwc_isoc_handler(struct urb *urb)
                         case -EILSEQ:           errmsg = "CRC/Timeout (could be anything)"; break;
                         case -ETIME:            errmsg = "Device does not respond"; break;
                 }
-                PWC_DEBUG_FLOW("pwc_isoc_handler() called with status %d [%s].\n", urb->status, errmsg);
-                /* Give up after a number of contiguous errors on the USB bus.
-                   Appearantly something is wrong so we simulate an unplug event.
-                 */
+                PWC_ERROR("pwc_isoc_handler() called with status %d [%s].\n",
+                          urb->status, errmsg);
+                /* Give up after a number of contiguous errors */
                 if (++pdev->visoc_errors > MAX_ISOC_ERRORS)
                 {
-                        PWC_INFO("Too many ISOC errors, bailing out.\n");
-                        pdev->error_status = EIO;
-                        awake = 1;
-                        wake_up_interruptible(&pdev->frameq);
+                        PWC_ERROR("Too many ISOC errors, bailing out.\n");
+                        if (pdev->fill_buf) {
+                                vb2_buffer_done(&pdev->fill_buf->vb,
+                                                VB2_BUF_STATE_ERROR);
+                                pdev->fill_buf = NULL;
+                        }
                 }
-                goto handler_end; // ugly, but practical
-        }
-
-        fbuf = pdev->fill_frame;
-        if (fbuf == NULL) {
-                PWC_ERROR("pwc_isoc_handler without valid fill frame.\n");
-                awake = 1;
+                pdev->vsync = 0; /* Drop the current frame */
                 goto handler_end;
         }
-        else {
-                fillptr = fbuf->data + fbuf->filled;
-        }
 
         /* Reset ISOC error counter. We did get here, after all. */
         pdev->visoc_errors = 0;
@@ -720,89 +325,73 @@ static void pwc_isoc_handler(struct urb *urb)
                 fst  = urb->iso_frame_desc[i].status;
                 flen = urb->iso_frame_desc[i].actual_length;
                 iso_buf = urb->transfer_buffer + urb->iso_frame_desc[i].offset;
-                if (fst == 0) {
-                        if (flen > 0) { /* if valid data... */
-                                if (pdev->vsync > 0) { /* ...and we are not sync-hunting... */
-                                        pdev->vsync = 2;
-
-                                        /* ...copy data to frame buffer, if possible */
-                                        if (flen + fbuf->filled > pdev->frame_total_size) {
-                                                PWC_DEBUG_FLOW("Frame buffer overflow (flen = %d, frame_total_size = %d).\n", flen, pdev->frame_total_size);
-                                                pdev->vsync = 0; /* Hmm, let's wait for an EOF (end-of-frame) */
-                                                pdev->vframes_error++;
-                                        }
-                                        else {
-                                                memmove(fillptr, iso_buf, flen);
-                                                fillptr += flen;
-                                        }
-                                }
+                if (fst != 0) {
+                        PWC_ERROR("Iso frame %d has error %d\n", i, fst);
+                        continue;
+                }
+                if (flen > 0 && pdev->vsync) {
+                        struct pwc_frame_buf *fbuf = pdev->fill_buf;
+
+                        if (pdev->vsync == 1) {
+                                do_gettimeofday(&fbuf->vb.v4l2_buf.timestamp);
+                                pdev->vsync = 2;
+                        }
+
+                        if (flen + fbuf->filled > pdev->frame_total_size) {
+                                PWC_ERROR("Frame overflow (%d > %d)\n",
+                                          flen + fbuf->filled,
+                                          pdev->frame_total_size);
+                                pdev->vsync = 0; /* Let's wait for an EOF */
+                        } else {
+                                memcpy(fbuf->data + fbuf->filled, iso_buf,
+                                       flen);
                                 fbuf->filled += flen;
-                        } /* ..flen > 0 */
-
-                        if (flen < pdev->vlast_packet_size) {
-                                /* Shorter packet... We probably have the end of an image-frame;
-                                   wake up read() process and let select()/poll() do something.
-                                   Decompression is done in user time over there.
-                                   */
-                                if (pdev->vsync == 2) {
-                                        if (pwc_rcv_short_packet(pdev, fbuf)) {
-                                                awake = 1;
-                                                fbuf = pdev->fill_frame;
-                                        }
-                                }
-                                fbuf->filled = 0;
-                                fillptr = fbuf->data;
+                        }
+                }
+                if (flen < pdev->vlast_packet_size) {
+                        /* Shorter packet... end of frame */
+                        if (pdev->vsync == 2)
+                                pwc_frame_complete(pdev);
+                        if (pdev->fill_buf == NULL)
+                                pdev->fill_buf = pwc_get_next_fill_buf(pdev);
+                        if (pdev->fill_buf) {
+                                pdev->fill_buf->filled = 0;
                                 pdev->vsync = 1;
                         }
-
-                        pdev->vlast_packet_size = flen;
-                } /* ..status == 0 */
-                else {
-                        /* This is normally not interesting to the user, unless
-                         * you are really debugging something, default = 0 */
-                        static int iso_error;
-                        iso_error++;
-                        if (iso_error < 20)
-                                PWC_DEBUG_FLOW("Iso frame %d of USB has error %d\n", i, fst);
                 }
+                pdev->vlast_packet_size = flen;
         }
 
 handler_end:
-        if (awake)
-                wake_up_interruptible(&pdev->frameq);
-
-        urb->dev = pdev->udev;
         i = usb_submit_urb(urb, GFP_ATOMIC);
         if (i != 0)
                 PWC_ERROR("Error (%d) re-submitting urb in pwc_isoc_handler.\n", i);
 }
 
-
-int pwc_isoc_init(struct pwc_device *pdev)
+static int pwc_isoc_init(struct pwc_device *pdev)
 {
         struct usb_device *udev;
         struct urb *urb;
         int i, j, ret;
-
         struct usb_interface *intf;
         struct usb_host_interface *idesc = NULL;
 
-        if (pdev == NULL)
-                return -EFAULT;
         if (pdev->iso_init)
                 return 0;
+
         pdev->vsync = 0;
+        pdev->vlast_packet_size = 0;
+        pdev->fill_buf = NULL;
+        pdev->vframe_count = 0;
+        pdev->visoc_errors = 0;
         udev = pdev->udev;
 
         /* Get the current alternate interface, adjust packet size */
-        if (!udev->actconfig)
-                return -EFAULT;
         intf = usb_ifnum_to_if(udev, 0);
         if (intf)
                 idesc = usb_altnum_to_altsetting(intf, pdev->valternate);
-
         if (!idesc)
-                return -EFAULT;
+                return -EIO;
 
         /* Search video endpoint */
         pdev->vmax_packet_size = -1;
@@ -825,34 +414,32 @@ int pwc_isoc_init(struct pwc_device *pdev)
         if (ret < 0)
                 return ret;
 
+        /* Allocate and init Isochronuous urbs */
         for (i = 0; i < MAX_ISO_BUFS; i++) {
                 urb = usb_alloc_urb(ISO_FRAMES_PER_DESC, GFP_KERNEL);
                 if (urb == NULL) {
                         PWC_ERROR("Failed to allocate urb %d\n", i);
-                        ret = -ENOMEM;
-                        break;
+                        pdev->iso_init = 1;
+                        pwc_isoc_cleanup(pdev);
+                        return -ENOMEM;
                 }
-                pdev->sbuf[i].urb = urb;
+                pdev->urbs[i] = urb;
                 PWC_DEBUG_MEMORY("Allocated URB at 0x%p\n", urb);
-        }
-        if (ret) {
-                /* De-allocate in reverse order */
-                while (i--) {
-                        usb_free_urb(pdev->sbuf[i].urb);
-                        pdev->sbuf[i].urb = NULL;
-                }
-                return ret;
-        }
-
-        /* init URB structure */
-        for (i = 0; i < MAX_ISO_BUFS; i++) {
-                urb = pdev->sbuf[i].urb;
 
                 urb->interval = 1; // devik
                 urb->dev = udev;
                 urb->pipe = usb_rcvisocpipe(udev, pdev->vendpoint);
-                urb->transfer_flags = URB_ISO_ASAP;
-                urb->transfer_buffer = pdev->sbuf[i].data;
+                urb->transfer_flags = URB_ISO_ASAP | URB_NO_TRANSFER_DMA_MAP;
+                urb->transfer_buffer = usb_alloc_coherent(udev,
+                                                          ISO_BUFFER_SIZE,
+                                                          GFP_KERNEL,
+                                                          &urb->transfer_dma);
+                if (urb->transfer_buffer == NULL) {
+                        PWC_ERROR("Failed to allocate urb buffer %d\n", i);
+                        pdev->iso_init = 1;
+                        pwc_isoc_cleanup(pdev);
+                        return -ENOMEM;
+                }
                 urb->transfer_buffer_length = ISO_BUFFER_SIZE;
                 urb->complete = pwc_isoc_handler;
                 urb->context = pdev;
@@ -866,14 +453,14 @@ int pwc_isoc_init(struct pwc_device *pdev)
 
         /* link */
         for (i = 0; i < MAX_ISO_BUFS; i++) {
-                ret = usb_submit_urb(pdev->sbuf[i].urb, GFP_KERNEL);
+                ret = usb_submit_urb(pdev->urbs[i], GFP_KERNEL);
                 if (ret) {
                         PWC_ERROR("isoc_init() submit_urb %d failed with error %d\n", i, ret);
                         pdev->iso_init = 1;
                         pwc_isoc_cleanup(pdev);
                         return ret;
                 }
-                PWC_DEBUG_MEMORY("URB 0x%p submitted.\n", pdev->sbuf[i].urb);
+                PWC_DEBUG_MEMORY("URB 0x%p submitted.\n", pdev->urbs[i]);
         }
 
         /* All is done... */
@@ -888,12 +475,9 @@ static void pwc_iso_stop(struct pwc_device *pdev)
 
         /* Unlinking ISOC buffers one by one */
         for (i = 0; i < MAX_ISO_BUFS; i++) {
-                struct urb *urb;
-
-                urb = pdev->sbuf[i].urb;
-                if (urb) {
-                        PWC_DEBUG_MEMORY("Unlinking URB %p\n", urb);
-                        usb_kill_urb(urb);
+                if (pdev->urbs[i]) {
+                        PWC_DEBUG_MEMORY("Unlinking URB %p\n", pdev->urbs[i]);
+                        usb_kill_urb(pdev->urbs[i]);
                 }
         }
 }
@@ -904,40 +488,51 @@ static void pwc_iso_free(struct pwc_device *pdev)
 
         /* Freeing ISOC buffers one by one */
         for (i = 0; i < MAX_ISO_BUFS; i++) {
-                struct urb *urb;
-
-                urb = pdev->sbuf[i].urb;
-                if (urb) {
+                if (pdev->urbs[i]) {
                         PWC_DEBUG_MEMORY("Freeing URB\n");
-                        usb_free_urb(urb);
-                        pdev->sbuf[i].urb = NULL;
+                        if (pdev->urbs[i]->transfer_buffer) {
+                                usb_free_coherent(pdev->udev,
+                                        pdev->urbs[i]->transfer_buffer_length,
+                                        pdev->urbs[i]->transfer_buffer,
+                                        pdev->urbs[i]->transfer_dma);
+                        }
+                        usb_free_urb(pdev->urbs[i]);
+                        pdev->urbs[i] = NULL;
                 }
         }
 }
 
-void pwc_isoc_cleanup(struct pwc_device *pdev)
+static void pwc_isoc_cleanup(struct pwc_device *pdev)
 {
         PWC_DEBUG_OPEN(">> pwc_isoc_cleanup()\n");
-        if (pdev == NULL)
-                return;
+
         if (pdev->iso_init == 0)
                 return;
 
         pwc_iso_stop(pdev);
         pwc_iso_free(pdev);
-
-        /* Stop camera, but only if we are sure the camera is still there (unplug
-           is signalled by EPIPE)
-         */
-        if (pdev->error_status != EPIPE) {
-                PWC_DEBUG_OPEN("Setting alternate interface 0.\n");
-                usb_set_interface(pdev->udev, 0, 0);
-        }
+        usb_set_interface(pdev->udev, 0, 0);
 
         pdev->iso_init = 0;
         PWC_DEBUG_OPEN("<< pwc_isoc_cleanup()\n");
 }
 
+/*
+ * Release all queued buffers, no need to take queued_bufs_lock, since all
+ * iso urbs have been killed when we're called so pwc_isoc_handler won't run.
+ */
+static void pwc_cleanup_queued_bufs(struct pwc_device *pdev)
+{
+        while (!list_empty(&pdev->queued_bufs)) {
+                struct pwc_frame_buf *buf;
+
+                buf = list_entry(pdev->queued_bufs.next, struct pwc_frame_buf,
+                                 list);
+                list_del(&buf->list);
+                vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+        }
+}
+
 /*********
  * sysfs
  *********/
@@ -1051,98 +646,15 @@ static const char *pwc_sensor_type_to_string(unsigned int sensor_type)
 
 static int pwc_video_open(struct file *file)
 {
-        int i, ret;
         struct video_device *vdev = video_devdata(file);
         struct pwc_device *pdev;
 
         PWC_DEBUG_OPEN(">> video_open called(vdev = 0x%p).\n", vdev);
 
         pdev = video_get_drvdata(vdev);
-        BUG_ON(!pdev);
-        if (pdev->vopen) {
-                PWC_DEBUG_OPEN("I'm busy, someone is using the device.\n");
-                return -EBUSY;
-        }
-
-        pwc_construct(pdev); /* set min/max sizes correct */
-        if (!pdev->usb_init) {
-                PWC_DEBUG_OPEN("Doing first time initialization.\n");
-                pdev->usb_init = 1;
-
-                /* Query sensor type */
-                ret = pwc_get_cmos_sensor(pdev, &i);
-                if (ret >= 0)
-                {
-                        PWC_DEBUG_OPEN("This %s camera is equipped with a %s (%d).\n",
-                                        pdev->vdev.name,
-                                        pwc_sensor_type_to_string(i), i);
-                }
-        }
-
-        /* Turn on camera */
-        if (power_save) {
-                i = pwc_camera_power(pdev, 1);
-                if (i < 0)
-                        PWC_DEBUG_OPEN("Failed to restore power to the camera! (%d)\n", i);
-        }
-        /* Set LED on/off time */
-        if (pwc_set_leds(pdev, led_on, led_off) < 0)
-                PWC_DEBUG_OPEN("Failed to set LED on/off time.\n");
-
-
-        /* So far, so good. Allocate memory. */
-        i = pwc_allocate_buffers(pdev);
-        if (i < 0) {
-                PWC_DEBUG_OPEN("Failed to allocate buffers memory.\n");
-                pwc_free_buffers(pdev);
-                return i;
-        }
-
-        /* Reset buffers & parameters */
-        pwc_reset_buffers(pdev);
-        for (i = 0; i < pwc_mbufs; i++)
-                pdev->image_used[i] = 0;
-        pdev->vframe_count = 0;
-        pdev->vframes_dumped = 0;
-        pdev->vframes_error = 0;
-        pdev->visoc_errors = 0;
-        pdev->error_status = 0;
-        pwc_construct(pdev); /* set min/max sizes correct */
-
-        /* Set some defaults */
-        pdev->vsnapshot = 0;
-
-        /* Set video size, first try the last used video size
-           (or the default one); if that fails try QCIF/10 or QSIF/10;
-           it that fails too, give up.
-         */
-        i = pwc_set_video_mode(pdev, pwc_image_sizes[pdev->vsize].x, pwc_image_sizes[pdev->vsize].y, pdev->vframes, pdev->vcompression, 0);
-        if (i)  {
-                unsigned int default_resolution;
-                PWC_DEBUG_OPEN("First attempt at set_video_mode failed.\n");
-                if (pdev->type>= 730)
-                        default_resolution = PSZ_QSIF;
-                else
-                        default_resolution = PSZ_QCIF;
-
-                i = pwc_set_video_mode(pdev,
-                                       pwc_image_sizes[default_resolution].x,
-                                       pwc_image_sizes[default_resolution].y,
-                                       10,
-                                       pdev->vcompression,
-                                       0);
-        }
-        if (i) {
-                PWC_DEBUG_OPEN("Second attempt at set_video_mode failed.\n");
-                pwc_free_buffers(pdev);
-                return i;
-        }
-
-        /* Initialize the webcam to sane value */
-        pwc_set_brightness(pdev, 0x7fff);
-        pwc_set_agc(pdev, 1, 0);
+        if (!pdev->udev)
+                return -ENODEV;
 
-        pdev->vopen++;
         file->private_data = vdev;
         PWC_DEBUG_OPEN("<< video_open() returns 0.\n");
         return 0;
@@ -1158,239 +670,211 @@ static void pwc_video_release(struct video_device *vfd)
                 if (device_hint[hint].pdev == pdev)
                         device_hint[hint].pdev = NULL;
 
+        /* Free intermediate decompression buffer & tables */
+        if (pdev->decompress_data != NULL) {
+                PWC_DEBUG_MEMORY("Freeing decompression buffer at %p.\n",
+                                 pdev->decompress_data);
+                kfree(pdev->decompress_data);
+                pdev->decompress_data = NULL;
+        }
+
+        v4l2_ctrl_handler_free(&pdev->ctrl_handler);
+
         kfree(pdev);
 }
 
-/* Note that all cleanup is done in the reverse order as in _open */
 static int pwc_video_close(struct file *file)
 {
         struct video_device *vdev = file->private_data;
         struct pwc_device *pdev;
-        int i;
 
         PWC_DEBUG_OPEN(">> video_close called(vdev = 0x%p).\n", vdev);
 
         pdev = video_get_drvdata(vdev);
-        if (pdev->vopen == 0)
-                PWC_DEBUG_MODULE("video_close() called on closed device?\n");
-
-        /* Dump statistics, but only if a reasonable amount of frames were
-           processed (to prevent endless log-entries in case of snap-shot
-           programs)
-         */
-        if (pdev->vframe_count > 20)
-                PWC_DEBUG_MODULE("Closing video device: %d frames received, dumped %d frames, %d frames with errors.\n", pdev->vframe_count, pdev->vframes_dumped, pdev->vframes_error);
-
-        if (DEVICE_USE_CODEC1(pdev->type))
-            pwc_dec1_exit();
-        else
-            pwc_dec23_exit();
-
-        pwc_isoc_cleanup(pdev);
-        pwc_free_buffers(pdev);
-
-        /* Turn off LEDS and power down camera, but only when not unplugged */
-        if (!pdev->unplugged) {
-                /* Turn LEDs off */
-                if (pwc_set_leds(pdev, 0, 0) < 0)
-                        PWC_DEBUG_MODULE("Failed to set LED on/off time.\n");
-                if (power_save) {
-                        i = pwc_camera_power(pdev, 0);
-                        if (i < 0)
-                                PWC_ERROR("Failed to power down camera (%d)\n", i);
-                }
-                pdev->vopen--;
-                PWC_DEBUG_OPEN("<< video_close() vopen=%d\n", pdev->vopen);
+        if (pdev->capt_file == file) {
+                vb2_queue_release(&pdev->vb_queue);
+                pdev->capt_file = NULL;
         }
 
+        PWC_DEBUG_OPEN("<< video_close()\n");
         return 0;
 }
 
-/*
- *      FIXME: what about two parallel reads ????
- *      ANSWER: Not supported. You can't open the device more than once,
-                despite what the V4L1 interface says. First, I don't see
-                the need, second there's no mechanism of alerting the
-                2nd/3rd/... process of events like changing image size.
-                And I don't see the point of blocking that for the
-                2nd/3rd/... process.
-                In multi-threaded environments reading parallel from any
-                device is tricky anyhow.
- */
-
 static ssize_t pwc_video_read(struct file *file, char __user *buf,
-                          size_t count, loff_t *ppos)
+                              size_t count, loff_t *ppos)
 {
         struct video_device *vdev = file->private_data;
-        struct pwc_device *pdev;
-        int noblock = file->f_flags & O_NONBLOCK;
-        DECLARE_WAITQUEUE(wait, current);
-        int bytes_to_read, rv = 0;
-        void *image_buffer_addr;
-
-        PWC_DEBUG_READ("pwc_video_read(vdev=0x%p, buf=%p, count=%zd) called.\n",
-                        vdev, buf, count);
-        if (vdev == NULL)
-                return -EFAULT;
-        pdev = video_get_drvdata(vdev);
-        if (pdev == NULL)
-                return -EFAULT;
+        struct pwc_device *pdev = video_get_drvdata(vdev);
 
-        if (pdev->error_status) {
-                rv = -pdev->error_status; /* Something happened, report what. */
-                goto err_out;
-        }
+        if (!pdev->udev)
+                return -ENODEV;
 
-        /* Start the stream (if not already started) */
-        rv = pwc_isoc_init(pdev);
-        if (rv)
-                goto err_out;
-
-        /* In case we're doing partial reads, we don't have to wait for a frame */
-        if (pdev->image_read_pos == 0) {
-                /* Do wait queueing according to the (doc)book */
-                add_wait_queue(&pdev->frameq, &wait);
-                while (pdev->full_frames == NULL) {
-                        /* Check for unplugged/etc. here */
-                        if (pdev->error_status) {
-                                remove_wait_queue(&pdev->frameq, &wait);
-                                set_current_state(TASK_RUNNING);
-                                rv = -pdev->error_status ;
-                                goto err_out;
-                        }
-                        if (noblock) {
-                                remove_wait_queue(&pdev->frameq, &wait);
-                                set_current_state(TASK_RUNNING);
-                                rv = -EWOULDBLOCK;
-                                goto err_out;
-                        }
-                        if (signal_pending(current)) {
-                                remove_wait_queue(&pdev->frameq, &wait);
-                                set_current_state(TASK_RUNNING);
-                                rv = -ERESTARTSYS;
-                                goto err_out;
-                        }
-                        mutex_unlock(&pdev->modlock);
-                        schedule();
-                        set_current_state(TASK_INTERRUPTIBLE);
-                        mutex_lock(&pdev->modlock);
-                }
-                remove_wait_queue(&pdev->frameq, &wait);
-                set_current_state(TASK_RUNNING);
+        if (pdev->capt_file != NULL &&
+            pdev->capt_file != file)
+                return -EBUSY;
 
-                /* Decompress and release frame */
-                if (pwc_handle_frame(pdev)) {
-                        rv = -EFAULT;
-                        goto err_out;
-                }
-        }
+        pdev->capt_file = file;
 
-        PWC_DEBUG_READ("Copying data to user space.\n");
-        if (pdev->pixfmt != V4L2_PIX_FMT_YUV420)
-                bytes_to_read = pdev->frame_size + sizeof(struct pwc_raw_frame);
-        else
-                bytes_to_read = pdev->view.size;
-
-        /* copy bytes to user space; we allow for partial reads */
-        if (count + pdev->image_read_pos > bytes_to_read)
-                count = bytes_to_read - pdev->image_read_pos;
-        image_buffer_addr = pdev->image_data;
-        image_buffer_addr += pdev->images[pdev->fill_image].offset;
-        image_buffer_addr += pdev->image_read_pos;
-        if (copy_to_user(buf, image_buffer_addr, count)) {
-                rv = -EFAULT;
-                goto err_out;
-        }
-        pdev->image_read_pos += count;
-        if (pdev->image_read_pos >= bytes_to_read) { /* All data has been read */
-                pdev->image_read_pos = 0;
-                pwc_next_image(pdev);
-        }
-        return count;
-err_out:
-        return rv;
+        return vb2_read(&pdev->vb_queue, buf, count, ppos,
+                        file->f_flags & O_NONBLOCK);
 }
 
 static unsigned int pwc_video_poll(struct file *file, poll_table *wait)
 {
         struct video_device *vdev = file->private_data;
-        struct pwc_device *pdev;
-        int ret;
+        struct pwc_device *pdev = video_get_drvdata(vdev);
 
-        if (vdev == NULL)
-                return -EFAULT;
-        pdev = video_get_drvdata(vdev);
-        if (pdev == NULL)
-                return -EFAULT;
+        if (!pdev->udev)
+                return POLL_ERR;
 
-        /* Start the stream (if not already started) */
-        ret = pwc_isoc_init(pdev);
-        if (ret)
-                return ret;
+        return vb2_poll(&pdev->vb_queue, file, wait);
+}
+
+static int pwc_video_mmap(struct file *file, struct vm_area_struct *vma)
+{
+        struct video_device *vdev = file->private_data;
+        struct pwc_device *pdev = video_get_drvdata(vdev);
+
+        if (pdev->capt_file != file)
+                return -EBUSY;
+
+        return vb2_mmap(&pdev->vb_queue, vma);
+}
+
+/***************************************************************************/
+/* Videobuf2 operations */
+
+static int queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+                                unsigned int *nplanes, unsigned long sizes[],
+                                void *alloc_ctxs[])
+{
+        struct pwc_device *pdev = vb2_get_drv_priv(vq);
+
+        if (*nbuffers < MIN_FRAMES)
+                *nbuffers = MIN_FRAMES;
+        else if (*nbuffers > MAX_FRAMES)
+                *nbuffers = MAX_FRAMES;
+
+        *nplanes = 1;
 
-        poll_wait(file, &pdev->frameq, wait);
-        if (pdev->error_status)
-                return POLLERR;
-        if (pdev->full_frames != NULL) /* we have frames waiting */
-                return (POLLIN | POLLRDNORM);
+        sizes[0] = PAGE_ALIGN((pdev->abs_max.x * pdev->abs_max.y * 3) / 2);
 
         return 0;
 }
 
-static int pwc_video_mmap(struct file *file, struct vm_area_struct *vma)
+static int buffer_init(struct vb2_buffer *vb)
 {
-        struct video_device *vdev = file->private_data;
-        struct pwc_device *pdev;
-        unsigned long start;
-        unsigned long size;
-        unsigned long page, pos = 0;
-        int index;
+        struct pwc_frame_buf *buf = container_of(vb, struct pwc_frame_buf, vb);
 
-        PWC_DEBUG_MEMORY(">> %s\n", __func__);
-        pdev = video_get_drvdata(vdev);
-        size = vma->vm_end - vma->vm_start;
-        start = vma->vm_start;
+        /* need vmalloc since frame buffer > 128K */
+        buf->data = vzalloc(PWC_FRAME_SIZE);
+        if (buf->data == NULL)
+                return -ENOMEM;
 
-        /* Find the idx buffer for this mapping */
-        for (index = 0; index < pwc_mbufs; index++) {
-                pos = pdev->images[index].offset;
-                if ((pos>>PAGE_SHIFT) == vma->vm_pgoff)
-                        break;
+        return 0;
+}
+
+static int buffer_prepare(struct vb2_buffer *vb)
+{
+        struct pwc_device *pdev = vb2_get_drv_priv(vb->vb2_queue);
+
+        /* Don't allow queing new buffers after device disconnection */
+        if (!pdev->udev)
+                return -ENODEV;
+
+        return 0;
+}
+
+static int buffer_finish(struct vb2_buffer *vb)
+{
+        struct pwc_device *pdev = vb2_get_drv_priv(vb->vb2_queue);
+        struct pwc_frame_buf *buf = container_of(vb, struct pwc_frame_buf, vb);
+
+        /*
+         * Application has called dqbuf and is getting back a buffer we've
+         * filled, take the pwc data we've stored in buf->data and decompress
+         * it into a usable format, storing the result in the vb2_buffer
+         */
+        return pwc_decompress(pdev, buf);
+}
+
+static void buffer_cleanup(struct vb2_buffer *vb)
+{
+        struct pwc_frame_buf *buf = container_of(vb, struct pwc_frame_buf, vb);
+
+        vfree(buf->data);
+}
+
+static void buffer_queue(struct vb2_buffer *vb)
+{
+        struct pwc_device *pdev = vb2_get_drv_priv(vb->vb2_queue);
+        struct pwc_frame_buf *buf = container_of(vb, struct pwc_frame_buf, vb);
+        unsigned long flags = 0;
+
+        spin_lock_irqsave(&pdev->queued_bufs_lock, flags);
+        list_add_tail(&buf->list, &pdev->queued_bufs);
+        spin_unlock_irqrestore(&pdev->queued_bufs_lock, flags);
+}
+
+static int start_streaming(struct vb2_queue *vq)
+{
+        struct pwc_device *pdev = vb2_get_drv_priv(vq);
+
+        if (!pdev->udev)
+                return -ENODEV;
+
+        /* Turn on camera and set LEDS on */
+        pwc_camera_power(pdev, 1);
+        if (pdev->power_save) {
+                /* Restore video mode */
+                pwc_set_video_mode(pdev, pdev->view.x, pdev->view.y,
+                                   pdev->vframes, pdev->vcompression,
+                                   pdev->vsnapshot);
         }
-        if (index == MAX_IMAGES)
-                return -EINVAL;
-        if (index == 0) {
-                /*
-                 * Special case for v4l1. In v4l1, we map only one big buffer,
-                 * but in v4l2 each buffer is mapped
-                 */
-                unsigned long total_size;
-                total_size = pwc_mbufs * pdev->len_per_image;
-                if (size != pdev->len_per_image && size != total_size) {
-                        PWC_ERROR("Wrong size (%lu) needed to be len_per_image=%d or total_size=%lu\n",
-                                   size, pdev->len_per_image, total_size);
-                        return -EINVAL;
-                }
-        } else if (size > pdev->len_per_image)
-                return -EINVAL;
-
-        vma->vm_flags |= VM_IO; /* from 2.6.9-acX */
-
-        pos += (unsigned long)pdev->image_data;
-        while (size > 0) {
-                page = vmalloc_to_pfn((void *)pos);
-                if (remap_pfn_range(vma, start, page, PAGE_SIZE, PAGE_SHARED))
-                        return -EAGAIN;
-                start += PAGE_SIZE;
-                pos += PAGE_SIZE;
-                if (size > PAGE_SIZE)
-                        size -= PAGE_SIZE;
-                else
-                        size = 0;
+        pwc_set_leds(pdev, led_on, led_off);
+
+        return pwc_isoc_init(pdev);
+}
+
+static int stop_streaming(struct vb2_queue *vq)
+{
+        struct pwc_device *pdev = vb2_get_drv_priv(vq);
+
+        if (pdev->udev) {
+                pwc_set_leds(pdev, 0, 0);
+                pwc_camera_power(pdev, 0);
+                pwc_isoc_cleanup(pdev);
         }
+        pwc_cleanup_queued_bufs(pdev);
+
         return 0;
 }
 
+static void pwc_lock(struct vb2_queue *vq)
+{
+        struct pwc_device *pdev = vb2_get_drv_priv(vq);
+        mutex_lock(&pdev->modlock);
+}
+
+static void pwc_unlock(struct vb2_queue *vq)
+{
+        struct pwc_device *pdev = vb2_get_drv_priv(vq);
+        mutex_unlock(&pdev->modlock);
+}
+
+static struct vb2_ops pwc_vb_queue_ops = {
+        .queue_setup            = queue_setup,
+        .buf_init               = buffer_init,
+        .buf_prepare            = buffer_prepare,
+        .buf_finish             = buffer_finish,
+        .buf_cleanup            = buffer_cleanup,
+        .buf_queue              = buffer_queue,
+        .start_streaming        = start_streaming,
+        .stop_streaming         = stop_streaming,
+        .wait_prepare           = pwc_unlock,
+        .wait_finish            = pwc_lock,
+};
+
 /***************************************************************************/
 /* USB functions */
 
@@ -1406,6 +890,7 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
         int hint, rc;
         int features = 0;
         int video_nr = -1; /* default: use next available device */
+        int my_power_save = power_save;
         char serial_number[30], *name;
 
         vendor_id = le16_to_cpu(udev->descriptor.idVendor);
@@ -1513,6 +998,8 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
                         PWC_INFO("Logitech QuickCam 4000 Pro USB webcam detected.\n");
                         name = "Logitech QuickCam Pro 4000";
                         type_id = 740; /* CCD sensor */
+                        if (my_power_save == -1)
+                                my_power_save = 1;
                         break;
                 case 0x08b3:
                         PWC_INFO("Logitech QuickCam Zoom USB webcam detected.\n");
@@ -1523,12 +1010,15 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
                         PWC_INFO("Logitech QuickCam Zoom (new model) USB webcam detected.\n");
                         name = "Logitech QuickCam Zoom";
                         type_id = 740; /* CCD sensor */
-                        power_save = 1;
+                        if (my_power_save == -1)
+                                my_power_save = 1;
                         break;
                 case 0x08b5:
                         PWC_INFO("Logitech QuickCam Orbit/Sphere USB webcam detected.\n");
                         name = "Logitech QuickCam Orbit";
                         type_id = 740; /* CCD sensor */
+                        if (my_power_save == -1)
+                                my_power_save = 1;
                         features |= FEATURE_MOTOR_PANTILT;
                         break;
                 case 0x08b6:
@@ -1583,6 +1073,8 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
                         PWC_INFO("Creative Labs Webcam 5 detected.\n");
                         name = "Creative Labs Webcam 5";
                         type_id = 730;
+                        if (my_power_save == -1)
+                                my_power_save = 1;
                         break;
                 case 0x4011:
                         PWC_INFO("Creative Labs Webcam Pro Ex detected.\n");
@@ -1640,6 +1132,9 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
         else
                 return -ENODEV; /* Not any of the know types; but the list keeps growing. */
 
+        if (my_power_save == -1)
+                my_power_save = 0;
+
         memset(serial_number, 0, 30);
         usb_string(udev, udev->descriptor.iSerialNumber, serial_number, 29);
         PWC_DEBUG_PROBE("Device serial number is %s\n", serial_number);
@@ -1654,7 +1149,6 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
                 return -ENOMEM;
         }
         pdev->type = type_id;
-        pdev->vsize = default_size;
         pdev->vframes = default_fps;
         strcpy(pdev->serial, serial_number);
         pdev->features = features;
@@ -1668,13 +1162,26 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
                 pdev->angle_range.tilt_min = -3000;
                 pdev->angle_range.tilt_max =  2500;
         }
+        pwc_construct(pdev); /* set min/max sizes correct */
 
         mutex_init(&pdev->modlock);
-        spin_lock_init(&pdev->ptrlock);
+        mutex_init(&pdev->udevlock);
+        spin_lock_init(&pdev->queued_bufs_lock);
+        INIT_LIST_HEAD(&pdev->queued_bufs);
 
         pdev->udev = udev;
-        init_waitqueue_head(&pdev->frameq);
         pdev->vcompression = pwc_preferred_compression;
+        pdev->power_save = my_power_save;
+
+        /* Init videobuf2 queue structure */
+        memset(&pdev->vb_queue, 0, sizeof(pdev->vb_queue));
+        pdev->vb_queue.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        pdev->vb_queue.io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
+        pdev->vb_queue.drv_priv = pdev;
+        pdev->vb_queue.buf_struct_size = sizeof(struct pwc_frame_buf);
+        pdev->vb_queue.ops = &pwc_vb_queue_ops;
+        pdev->vb_queue.mem_ops = &vb2_vmalloc_memops;
+        vb2_queue_init(&pdev->vb_queue);
 
         /* Init video_device structure */
         memcpy(&pdev->vdev, &pwc_template, sizeof(pwc_template));
@@ -1707,14 +1214,40 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
         PWC_DEBUG_PROBE("probe() function returning struct at 0x%p.\n", pdev);
         usb_set_intfdata(intf, pdev);
 
+#ifdef CONFIG_USB_PWC_DEBUG
+        /* Query sensor type */
+        if (pwc_get_cmos_sensor(pdev, &rc) >= 0) {
+                PWC_DEBUG_OPEN("This %s camera is equipped with a %s (%d).\n",
+                                pdev->vdev.name,
+                                pwc_sensor_type_to_string(rc), rc);
+        }
+#endif
+
         /* Set the leds off */
         pwc_set_leds(pdev, 0, 0);
+
+        /* Setup intial videomode */
+        rc = pwc_set_video_mode(pdev, pdev->view_max.x, pdev->view_max.y,
+                                pdev->vframes, pdev->vcompression, 0);
+        if (rc)
+                goto err_free_mem;
+
+        /* Register controls (and read default values from camera */
+        rc = pwc_init_controls(pdev);
+        if (rc) {
+                PWC_ERROR("Failed to register v4l2 controls (%d).\n", rc);
+                goto err_free_mem;
+        }
+
+        pdev->vdev.ctrl_handler = &pdev->ctrl_handler;
+
+        /* And powerdown the camera until streaming starts */
         pwc_camera_power(pdev, 0);
 
         rc = video_register_device(&pdev->vdev, VFL_TYPE_GRABBER, video_nr);
         if (rc < 0) {
                 PWC_ERROR("Failed to register as video device (%d).\n", rc);
-                goto err_free_mem;
+                goto err_free_controls;
         }
         rc = pwc_create_sysfs_files(pdev);
         if (rc)
@@ -1757,7 +1290,10 @@ err_video_unreg:
         if (hint < MAX_DEV_HINTS)
                 device_hint[hint].pdev = NULL;
         video_unregister_device(&pdev->vdev);
+err_free_controls:
+        v4l2_ctrl_handler_free(&pdev->ctrl_handler);
 err_free_mem:
+        usb_set_intfdata(intf, NULL);
         kfree(pdev);
         return rc;
 }
@@ -1767,33 +1303,17 @@ static void usb_pwc_disconnect(struct usb_interface *intf)
 {
         struct pwc_device *pdev  = usb_get_intfdata(intf);
 
+        mutex_lock(&pdev->udevlock);
         mutex_lock(&pdev->modlock);
-        usb_set_intfdata (intf, NULL);
-        if (pdev == NULL) {
-                PWC_ERROR("pwc_disconnect() Called without private pointer.\n");
-                goto disconnect_out;
-        }
-        if (pdev->udev == NULL) {
-                PWC_ERROR("pwc_disconnect() already called for %p\n", pdev);
-                goto disconnect_out;
-        }
-        if (pdev->udev != interface_to_usbdev(intf)) {
-                PWC_ERROR("pwc_disconnect() Woops: pointer mismatch udev/pdev.\n");
-                goto disconnect_out;
-        }
-
-        /* We got unplugged; this is signalled by an EPIPE error code */
-        pdev->error_status = EPIPE;
-        pdev->unplugged = 1;
-
-        /* Alert waiting processes */
-        wake_up_interruptible(&pdev->frameq);
 
+        usb_set_intfdata(intf, NULL);
         /* No need to keep the urbs around after disconnection */
         pwc_isoc_cleanup(pdev);
+        pwc_cleanup_queued_bufs(pdev);
+        pdev->udev = NULL;
 
-disconnect_out:
         mutex_unlock(&pdev->modlock);
+        mutex_unlock(&pdev->udevlock);
 
         pwc_remove_sysfs_files(pdev);
         video_unregister_device(&pdev->vdev);
@@ -1809,36 +1329,27 @@ disconnect_out:
  * Initialization code & module stuff
  */
 
-static char *size;
 static int fps;
-static int fbufs;
-static int mbufs;
 static int compression = -1;
 static int leds[2] = { -1, -1 };
 static unsigned int leds_nargs;
 static char *dev_hint[MAX_DEV_HINTS];
 static unsigned int dev_hint_nargs;
 
-module_param(size, charp, 0444);
 module_param(fps, int, 0444);
-module_param(fbufs, int, 0444);
-module_param(mbufs, int, 0444);
 #ifdef CONFIG_USB_PWC_DEBUG
 module_param_named(trace, pwc_trace, int, 0644);
 #endif
-module_param(power_save, int, 0444);
+module_param(power_save, int, 0644);
 module_param(compression, int, 0444);
 module_param_array(leds, int, &leds_nargs, 0444);
 module_param_array(dev_hint, charp, &dev_hint_nargs, 0444);
 
-MODULE_PARM_DESC(size, "Initial image size. One of sqcif, qsif, qcif, sif, cif, vga");
 MODULE_PARM_DESC(fps, "Initial frames per second. Varies with model, useful range 5-30");
-MODULE_PARM_DESC(fbufs, "Number of internal frame buffers to reserve");
-MODULE_PARM_DESC(mbufs, "Number of external (mmap()ed) image buffers");
 #ifdef CONFIG_USB_PWC_DEBUG
 MODULE_PARM_DESC(trace, "For debugging purposes");
 #endif
-MODULE_PARM_DESC(power_save, "Turn power save feature in camera on or off");
+MODULE_PARM_DESC(power_save, "Turn power saving for new cameras on or off");
 MODULE_PARM_DESC(compression, "Preferred compression quality. Range 0 (uncompressed) to 3 (high compression)");
 MODULE_PARM_DESC(leds, "LED on,off time in milliseconds");
 MODULE_PARM_DESC(dev_hint, "Device node hints");
@@ -1851,14 +1362,19 @@ MODULE_VERSION( PWC_VERSION );
 
 static int __init usb_pwc_init(void)
 {
-        int i, sz;
-        char *sizenames[PSZ_MAX] = { "sqcif", "qsif", "qcif", "sif", "cif", "vga" };
+        int i;
 
+#ifdef CONFIG_USB_PWC_DEBUG
         PWC_INFO("Philips webcam module version " PWC_VERSION " loaded.\n");
         PWC_INFO("Supports Philips PCA645/646, PCVC675/680/690, PCVC720[40]/730/740/750 & PCVC830/840.\n");
         PWC_INFO("Also supports the Askey VC010, various Logitech Quickcams, Samsung MPC-C10 and MPC-C30,\n");
         PWC_INFO("the Creative WebCam 5 & Pro Ex, SOTEC Afina Eye and Visionite VCS-UC300 and VCS-UM100.\n");
 
+        if (pwc_trace >= 0) {
+                PWC_DEBUG_MODULE("Trace options: 0x%04x\n", pwc_trace);
+        }
+#endif
+
         if (fps) {
                 if (fps < 4 || fps > 30) {
                         PWC_ERROR("Framerate out of bounds (4-30).\n");
@@ -1868,41 +1384,6 @@ static int __init usb_pwc_init(void)
                 PWC_DEBUG_MODULE("Default framerate set to %d.\n", default_fps);
         }
 
-        if (size) {
-                /* string; try matching with array */
-                for (sz = 0; sz < PSZ_MAX; sz++) {
-                        if (!strcmp(sizenames[sz], size)) { /* Found! */
-                                default_size = sz;
-                                break;
-                        }
-                }
-                if (sz == PSZ_MAX) {
-                        PWC_ERROR("Size not recognized; try size=[sqcif | qsif | qcif | sif | cif | vga].\n");
-                        return -EINVAL;
-                }
-                PWC_DEBUG_MODULE("Default image size set to %s [%dx%d].\n", sizenames[default_size], pwc_image_sizes[default_size].x, pwc_image_sizes[default_size].y);
-        }
-        if (mbufs) {
-                if (mbufs < 1 || mbufs > MAX_IMAGES) {
-                        PWC_ERROR("Illegal number of mmap() buffers; use a number between 1 and %d.\n", MAX_IMAGES);
-                        return -EINVAL;
-                }
-                pwc_mbufs = mbufs;
-                PWC_DEBUG_MODULE("Number of image buffers set to %d.\n", pwc_mbufs);
-        }
-        if (fbufs) {
-                if (fbufs < 2 || fbufs > MAX_FRAMES) {
-                        PWC_ERROR("Illegal number of frame buffers; use a number between 2 and %d.\n", MAX_FRAMES);
-                        return -EINVAL;
-                }
-                default_fbufs = fbufs;
-                PWC_DEBUG_MODULE("Number of frame buffers set to %d.\n", default_fbufs);
-        }
-#ifdef CONFIG_USB_PWC_DEBUG
-        if (pwc_trace >= 0) {
-                PWC_DEBUG_MODULE("Trace options: 0x%04x\n", pwc_trace);
-        }
-#endif
         if (compression >= 0) {
                 if (compression > 3) {
                         PWC_ERROR("Invalid compression setting; use a number between 0 (uncompressed) and 3 (high).\n");
@@ -1911,8 +1392,6 @@ static int __init usb_pwc_init(void)
                 pwc_preferred_compression = compression;
                 PWC_DEBUG_MODULE("Preferred compression set to %d.\n", pwc_preferred_compression);
         }
-        if (power_save)
-                PWC_DEBUG_MODULE("Enabling power save on open/close.\n");
         if (leds[0] >= 0)
                 led_on = leds[0];
         if (leds[1] >= 0)
diff --git a/drivers/media/video/pwc/pwc-ioctl.h b/drivers/media/video/pwc/pwc-ioctl.h
deleted file mode 100644
index 8c0cae7b3da..00000000000
--- a/drivers/media/video/pwc/pwc-ioctl.h
+++ /dev/null
@@ -1,323 +0,0 @@
-#ifndef PWC_IOCTL_H
-#define PWC_IOCTL_H
-
-/* (C) 2001-2004 Nemosoft Unv.
-   (C) 2004-2006 Luc Saillard (luc@saillard.org)
-
-   NOTE: this version of pwc is an unofficial (modified) release of pwc & pcwx
-   driver and thus may have bugs that are not present in the original version.
-   Please send bug reports and support requests to <luc@saillard.org>.
-   The decompression routines have been implemented by reverse-engineering the
-   Nemosoft binary pwcx module. Caveat emptor.
-
-   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
-*/
-
-/* This is pwc-ioctl.h belonging to PWC 10.0.10
-   It contains structures and defines to communicate from user space
-   directly to the driver.
- */
-
-/*
-   Changes
-   2001/08/03  Alvarado   Added ioctl constants to access methods for
-                          changing white balance and red/blue gains
-   2002/12/15  G. H. Fernandez-Toribio   VIDIOCGREALSIZE
-   2003/12/13  Nemosft Unv. Some modifications to make interfacing to
-               PWCX easier
- */
-
-/* These are private ioctl() commands, specific for the Philips webcams.
-   They contain functions not found in other webcams, and settings not
-   specified in the Video4Linux API.
-
-   The #define names are built up like follows:
-   VIDIOC               VIDeo IOCtl prefix
-         PWC            Philps WebCam
-            G           optional: Get
-            S           optional: Set
-             ...        the function
- */
-
-#include <linux/types.h>
-#include <linux/version.h>
-
- /* Enumeration of image sizes */
-#define PSZ_SQCIF       0x00
-#define PSZ_QSIF        0x01
-#define PSZ_QCIF        0x02
-#define PSZ_SIF         0x03
-#define PSZ_CIF         0x04
-#define PSZ_VGA         0x05
-#define PSZ_MAX         6
-
-
-/* The frame rate is encoded in the video_window.flags parameter using
-   the upper 16 bits, since some flags are defined nowadays. The following
-   defines provide a mask and shift to filter out this value.
-   This value can also be passing using the private flag when using v4l2 and
-   VIDIOC_S_FMT ioctl.
-
-   In 'Snapshot' mode the camera freezes its automatic exposure and colour
-   balance controls.
- */
-#define PWC_FPS_SHIFT           16
-#define PWC_FPS_MASK            0x00FF0000
-#define PWC_FPS_FRMASK          0x003F0000
-#define PWC_FPS_SNAPSHOT        0x00400000
-#define PWC_QLT_MASK            0x03000000
-#define PWC_QLT_SHIFT           24
-
-
-/* structure for transferring x & y coordinates */
-struct pwc_coord
-{
-        int x, y;               /* guess what */
-        int size;               /* size, or offset */
-};
-
-
-/* Used with VIDIOCPWCPROBE */
-struct pwc_probe
-{
-        char name[32];
-        int type;
-};
-
-struct pwc_serial
-{
-        char serial[30];        /* String with serial number. Contains terminating 0 */
-};
-
-/* pwc_whitebalance.mode values */
-#define PWC_WB_INDOOR           0
-#define PWC_WB_OUTDOOR          1
-#define PWC_WB_FL               2
-#define PWC_WB_MANUAL           3
-#define PWC_WB_AUTO             4
-
-/* Used with VIDIOCPWC[SG]AWB (Auto White Balance).
-   Set mode to one of the PWC_WB_* values above.
-   *red and *blue are the respective gains of these colour components inside
-   the camera; range 0..65535
-   When 'mode' == PWC_WB_MANUAL, 'manual_red' and 'manual_blue' are set or read;
-   otherwise undefined.
-   'read_red' and 'read_blue' are read-only.
-*/
-struct pwc_whitebalance
-{
-        int mode;
-        int manual_red, manual_blue;    /* R/W */
-        int read_red, read_blue;        /* R/O */
-};
-
-/*
-   'control_speed' and 'control_delay' are used in automatic whitebalance mode,
-   and tell the camera how fast it should react to changes in lighting, and
-   with how much delay. Valid values are 0..65535.
-*/
-struct pwc_wb_speed
-{
-        int control_speed;
-        int control_delay;
-
-};
-
-/* Used with VIDIOCPWC[SG]LED */
-struct pwc_leds
-{
-        int led_on;                     /* Led on-time; range = 0..25000 */
-        int led_off;                    /* Led off-time; range = 0..25000  */
-};
-
-/* Image size (used with GREALSIZE) */
-struct pwc_imagesize
-{
-        int width;
-        int height;
-};
-
-/* Defines and structures for Motorized Pan & Tilt */
-#define PWC_MPT_PAN             0x01
-#define PWC_MPT_TILT            0x02
-#define PWC_MPT_TIMEOUT         0x04 /* for status */
-
-/* Set angles; when absolute != 0, the angle is absolute and the
-   driver calculates the relative offset for you. This can only
-   be used with VIDIOCPWCSANGLE; VIDIOCPWCGANGLE always returns
-   absolute angles.
- */
-struct pwc_mpt_angles
-{
-        int absolute;           /* write-only */
-        int pan;                /* degrees * 100 */
-        int tilt;               /* degress * 100 */
-};
-
-/* Range of angles of the camera, both horizontally and vertically.
- */
-struct pwc_mpt_range
-{
-        int pan_min, pan_max;           /* degrees * 100 */
-        int tilt_min, tilt_max;
-};
-
-struct pwc_mpt_status
-{
-        int status;
-        int time_pan;
-        int time_tilt;
-};
-
-
-/* This is used for out-of-kernel decompression. With it, you can get
-   all the necessary information to initialize and use the decompressor
-   routines in standalone applications.
- */
-struct pwc_video_command
-{
-        int type;               /* camera type (645, 675, 730, etc.) */
-        int release;            /* release number */
-
-        int size;               /* one of PSZ_* */
-        int alternate;
-        int command_len;        /* length of USB video command */
-        unsigned char command_buf[13];  /* Actual USB video command */
-        int bandlength;         /* >0 = compressed */
-        int frame_size;         /* Size of one (un)compressed frame */
-};
-
-/* Flags for PWCX subroutines. Not all modules honour all flags. */
-#define PWCX_FLAG_PLANAR        0x0001
-#define PWCX_FLAG_BAYER         0x0008
-
-
-/* IOCTL definitions */
-
- /* Restore user settings */
-#define VIDIOCPWCRUSER          _IO('v', 192)
- /* Save user settings */
-#define VIDIOCPWCSUSER          _IO('v', 193)
- /* Restore factory settings */
-#define VIDIOCPWCFACTORY        _IO('v', 194)
-
- /* You can manipulate the compression factor. A compression preference of 0
-    means use uncompressed modes when available; 1 is low compression, 2 is
-    medium and 3 is high compression preferred. Of course, the higher the
-    compression, the lower the bandwidth used but more chance of artefacts
-    in the image. The driver automatically chooses a higher compression when
-    the preferred mode is not available.
-  */
- /* Set preferred compression quality (0 = uncompressed, 3 = highest compression) */
-#define VIDIOCPWCSCQUAL         _IOW('v', 195, int)
- /* Get preferred compression quality */
-#define VIDIOCPWCGCQUAL         _IOR('v', 195, int)
-
-
-/* Retrieve serial number of camera */
-#define VIDIOCPWCGSERIAL        _IOR('v', 198, struct pwc_serial)
-
- /* This is a probe function; since so many devices are supported, it
-    becomes difficult to include all the names in programs that want to
-    check for the enhanced Philips stuff. So in stead, try this PROBE;
-    it returns a structure with the original name, and the corresponding
-    Philips type.
-    To use, fill the structure with zeroes, call PROBE and if that succeeds,
-    compare the name with that returned from VIDIOCGCAP; they should be the
-    same. If so, you can be assured it is a Philips (OEM) cam and the type
-    is valid.
- */
-#define VIDIOCPWCPROBE          _IOR('v', 199, struct pwc_probe)
-
- /* Set AGC (Automatic Gain Control); int < 0 = auto, 0..65535 = fixed */
-#define VIDIOCPWCSAGC           _IOW('v', 200, int)
- /* Get AGC; int < 0 = auto; >= 0 = fixed, range 0..65535 */
-#define VIDIOCPWCGAGC           _IOR('v', 200, int)
- /* Set shutter speed; int < 0 = auto; >= 0 = fixed, range 0..65535 */
-#define VIDIOCPWCSSHUTTER       _IOW('v', 201, int)
-
- /* Color compensation (Auto White Balance) */
-#define VIDIOCPWCSAWB           _IOW('v', 202, struct pwc_whitebalance)
-#define VIDIOCPWCGAWB           _IOR('v', 202, struct pwc_whitebalance)
-
- /* Auto WB speed */
-#define VIDIOCPWCSAWBSPEED      _IOW('v', 203, struct pwc_wb_speed)
-#define VIDIOCPWCGAWBSPEED      _IOR('v', 203, struct pwc_wb_speed)
-
- /* LEDs on/off/blink; int range 0..65535 */
-#define VIDIOCPWCSLED           _IOW('v', 205, struct pwc_leds)
-#define VIDIOCPWCGLED           _IOR('v', 205, struct pwc_leds)
-
-  /* Contour (sharpness); int < 0 = auto, 0..65536 = fixed */
-#define VIDIOCPWCSCONTOUR       _IOW('v', 206, int)
-#define VIDIOCPWCGCONTOUR       _IOR('v', 206, int)
-
-  /* Backlight compensation; 0 = off, otherwise on */
-#define VIDIOCPWCSBACKLIGHT     _IOW('v', 207, int)
-#define VIDIOCPWCGBACKLIGHT     _IOR('v', 207, int)
-
-  /* Flickerless mode; = 0 off, otherwise on */
-#define VIDIOCPWCSFLICKER       _IOW('v', 208, int)
-#define VIDIOCPWCGFLICKER       _IOR('v', 208, int)
-
-  /* Dynamic noise reduction; 0 off, 3 = high noise reduction */
-#define VIDIOCPWCSDYNNOISE      _IOW('v', 209, int)
-#define VIDIOCPWCGDYNNOISE      _IOR('v', 209, int)
-
- /* Real image size as used by the camera; tells you whether or not there's a gray border around the image */
-#define VIDIOCPWCGREALSIZE      _IOR('v', 210, struct pwc_imagesize)
-
- /* Motorized pan & tilt functions */
-#define VIDIOCPWCMPTRESET       _IOW('v', 211, int)
-#define VIDIOCPWCMPTGRANGE      _IOR('v', 211, struct pwc_mpt_range)
-#define VIDIOCPWCMPTSANGLE      _IOW('v', 212, struct pwc_mpt_angles)
-#define VIDIOCPWCMPTGANGLE      _IOR('v', 212, struct pwc_mpt_angles)
-#define VIDIOCPWCMPTSTATUS      _IOR('v', 213, struct pwc_mpt_status)
-
- /* Get the USB set-video command; needed for initializing libpwcx */
-#define VIDIOCPWCGVIDCMD        _IOR('v', 215, struct pwc_video_command)
-struct pwc_table_init_buffer {
-   int len;
-   char *buffer;
-
-};
-#define VIDIOCPWCGVIDTABLE      _IOR('v', 216, struct pwc_table_init_buffer)
-
-/*
- * This is private command used when communicating with v4l2.
- * In the future all private ioctl will be remove/replace to
- * use interface offer by v4l2.
- */
-
-#define V4L2_CID_PRIVATE_SAVE_USER       (V4L2_CID_PRIVATE_BASE + 0)
-#define V4L2_CID_PRIVATE_RESTORE_USER    (V4L2_CID_PRIVATE_BASE + 1)
-#define V4L2_CID_PRIVATE_RESTORE_FACTORY (V4L2_CID_PRIVATE_BASE + 2)
-#define V4L2_CID_PRIVATE_COLOUR_MODE     (V4L2_CID_PRIVATE_BASE + 3)
-#define V4L2_CID_PRIVATE_AUTOCONTOUR     (V4L2_CID_PRIVATE_BASE + 4)
-#define V4L2_CID_PRIVATE_CONTOUR         (V4L2_CID_PRIVATE_BASE + 5)
-#define V4L2_CID_PRIVATE_BACKLIGHT       (V4L2_CID_PRIVATE_BASE + 6)
-#define V4L2_CID_PRIVATE_FLICKERLESS     (V4L2_CID_PRIVATE_BASE + 7)
-#define V4L2_CID_PRIVATE_NOISE_REDUCTION (V4L2_CID_PRIVATE_BASE + 8)
-
-struct pwc_raw_frame {
-   __le16 type;         /* type of the webcam */
-   __le16 vbandlength;  /* Size of 4lines compressed (used by the decompressor) */
-   __u8   cmd[4];       /* the four byte of the command (in case of nala,
-                           only the first 3 bytes is filled) */
-   __u8   rawframe[0];  /* frame_size = H/4*vbandlength */
-} __attribute__ ((packed));
-
-
-#endif
diff --git a/drivers/media/video/pwc/pwc-kiara.c b/drivers/media/video/pwc/pwc-kiara.c
index f4ae83c0cf2..e5f4fd81712 100644
--- a/drivers/media/video/pwc/pwc-kiara.c
+++ b/drivers/media/video/pwc/pwc-kiara.c
@@ -40,7 +40,6 @@
 
 
 #include "pwc-kiara.h"
-#include "pwc-uncompress.h"
 
 const unsigned int Kiara_fps_vector[PWC_FPS_MAX_KIARA] = { 5, 10, 15, 20, 25, 30 };
 
diff --git a/drivers/media/video/pwc/pwc-misc.c b/drivers/media/video/pwc/pwc-misc.c
index 6af5bb53835..0b031336eab 100644
--- a/drivers/media/video/pwc/pwc-misc.c
+++ b/drivers/media/video/pwc/pwc-misc.c
@@ -126,8 +126,4 @@ void pwc_construct(struct pwc_device *pdev)
         pdev->pixfmt = V4L2_PIX_FMT_YUV420; /* default */
         pdev->view_min.size = pdev->view_min.x * pdev->view_min.y;
         pdev->view_max.size = pdev->view_max.x * pdev->view_max.y;
-        /* length of image, in YUV format; always allocate enough memory. */
-        pdev->len_per_image = PAGE_ALIGN((pdev->abs_max.x * pdev->abs_max.y * 3) / 2);
 }
-
-
diff --git a/drivers/media/video/pwc/pwc-uncompress.c b/drivers/media/video/pwc/pwc-uncompress.c
index 3b73f295f03..51265092bd3 100644
--- a/drivers/media/video/pwc/pwc-uncompress.c
+++ b/drivers/media/video/pwc/pwc-uncompress.c
@@ -30,26 +30,17 @@
 #include <asm/types.h>
 
 #include "pwc.h"
-#include "pwc-uncompress.h"
 #include "pwc-dec1.h"
 #include "pwc-dec23.h"
 
-int pwc_decompress(struct pwc_device *pdev)
+int pwc_decompress(struct pwc_device *pdev, struct pwc_frame_buf *fbuf)
 {
-        struct pwc_frame_buf *fbuf;
         int n, line, col, stride;
         void *yuv, *image;
         u16 *src;
         u16 *dsty, *dstu, *dstv;
 
-        if (pdev == NULL)
-                return -EFAULT;
-
-        fbuf = pdev->read_frame;
-        if (fbuf == NULL)
-                return -EFAULT;
-        image  = pdev->image_data;
-        image += pdev->images[pdev->fill_image].offset;
+        image = vb2_plane_vaddr(&fbuf->vb, 0);
 
         yuv = fbuf->data + pdev->frame_header_size;  /* Skip header */
 
@@ -64,9 +55,13 @@ int pwc_decompress(struct pwc_device *pdev)
                          * determine this using the type of the webcam */
                 memcpy(raw_frame->cmd, pdev->cmd_buf, 4);
                 memcpy(raw_frame+1, yuv, pdev->frame_size);
+                vb2_set_plane_payload(&fbuf->vb, 0,
+                        pdev->frame_size + sizeof(struct pwc_raw_frame));
                 return 0;
         }
 
+        vb2_set_plane_payload(&fbuf->vb, 0, pdev->view.size);
+
         if (pdev->vbandlength == 0) {
                 /* Uncompressed mode.
                  * We copy the data into the output buffer, using the viewport
diff --git a/drivers/media/video/pwc/pwc-uncompress.h b/drivers/media/video/pwc/pwc-uncompress.h
deleted file mode 100644
index 43028e74e9e..00000000000
--- a/drivers/media/video/pwc/pwc-uncompress.h
+++ /dev/null
@@ -1,40 +0,0 @@
-/* (C) 1999-2003 Nemosoft Unv.
-   (C) 2004-2006 Luc Saillard (luc@saillard.org)
-
-   NOTE: this version of pwc is an unofficial (modified) release of pwc & pcwx
-   driver and thus may have bugs that are not present in the original version.
-   Please send bug reports and support requests to <luc@saillard.org>.
-   The decompression routines have been implemented by reverse-engineering the
-   Nemosoft binary pwcx module. Caveat emptor.
-
-   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
-*/
-
-/* This file is the bridge between the kernel module and the plugin; it
-   describes the structures and datatypes used in both modules. Any
-   significant change should be reflected by increasing the
-   pwc_decompressor_version major number.
- */
-#ifndef PWC_UNCOMPRESS_H
-#define PWC_UNCOMPRESS_H
-
-
-#include <media/pwc-ioctl.h>
-
-/* from pwc-dec.h */
-#define PWCX_FLAG_PLANAR        0x0001
-/* */
-
-#endif
diff --git a/drivers/media/video/pwc/pwc-v4l.c b/drivers/media/video/pwc/pwc-v4l.c
index f85c51249c7..e9a0e94b999 100644
--- a/drivers/media/video/pwc/pwc-v4l.c
+++ b/drivers/media/video/pwc/pwc-v4l.c
@@ -2,6 +2,7 @@
    USB and Video4Linux interface part.
    (C) 1999-2004 Nemosoft Unv.
    (C) 2004-2006 Luc Saillard (luc@saillard.org)
+   (C) 2011 Hans de Goede <hdegoede@redhat.com>
 
    NOTE: this version of pwc is an unofficial (modified) release of pwc & pcwx
    driver and thus may have bugs that are not present in the original version.
@@ -31,184 +32,330 @@
 #include <linux/module.h>
 #include <linux/poll.h>
 #include <linux/vmalloc.h>
+#include <linux/jiffies.h>
 #include <asm/io.h>
 
 #include "pwc.h"
 
-static struct v4l2_queryctrl pwc_controls[] = {
-        {
-            .id      = V4L2_CID_BRIGHTNESS,
-            .type    = V4L2_CTRL_TYPE_INTEGER,
-            .name    = "Brightness",
-            .minimum = 0,
-            .maximum = 128,
-            .step    = 1,
-            .default_value = 64,
-        },
-        {
-            .id      = V4L2_CID_CONTRAST,
-            .type    = V4L2_CTRL_TYPE_INTEGER,
-            .name    = "Contrast",
-            .minimum = 0,
-            .maximum = 64,
-            .step    = 1,
-            .default_value = 0,
-        },
-        {
-            .id      = V4L2_CID_SATURATION,
-            .type    = V4L2_CTRL_TYPE_INTEGER,
-            .name    = "Saturation",
-            .minimum = -100,
-            .maximum = 100,
-            .step    = 1,
-            .default_value = 0,
-        },
-        {
-            .id      = V4L2_CID_GAMMA,
-            .type    = V4L2_CTRL_TYPE_INTEGER,
-            .name    = "Gamma",
-            .minimum = 0,
-            .maximum = 32,
-            .step    = 1,
-            .default_value = 0,
-        },
-        {
-            .id      = V4L2_CID_RED_BALANCE,
-            .type    = V4L2_CTRL_TYPE_INTEGER,
-            .name    = "Red Gain",
-            .minimum = 0,
-            .maximum = 256,
-            .step    = 1,
-            .default_value = 0,
-        },
-        {
-            .id      = V4L2_CID_BLUE_BALANCE,
-            .type    = V4L2_CTRL_TYPE_INTEGER,
-            .name    = "Blue Gain",
-            .minimum = 0,
-            .maximum = 256,
-            .step    = 1,
-            .default_value = 0,
-        },
-        {
-            .id      = V4L2_CID_AUTO_WHITE_BALANCE,
-            .type    = V4L2_CTRL_TYPE_BOOLEAN,
-            .name    = "Auto White Balance",
-            .minimum = 0,
-            .maximum = 1,
-            .step    = 1,
-            .default_value = 0,
-        },
-        {
-            .id      = V4L2_CID_EXPOSURE,
-            .type    = V4L2_CTRL_TYPE_INTEGER,
-            .name    = "Shutter Speed (Exposure)",
-            .minimum = 0,
-            .maximum = 256,
-            .step    = 1,
-            .default_value = 200,
-        },
-        {
-            .id      = V4L2_CID_AUTOGAIN,
-            .type    = V4L2_CTRL_TYPE_BOOLEAN,
-            .name    = "Auto Gain Enabled",
-            .minimum = 0,
-            .maximum = 1,
-            .step    = 1,
-            .default_value = 1,
-        },
-        {
-            .id      = V4L2_CID_GAIN,
-            .type    = V4L2_CTRL_TYPE_INTEGER,
-            .name    = "Gain Level",
-            .minimum = 0,
-            .maximum = 256,
-            .step    = 1,
-            .default_value = 0,
-        },
-        {
-            .id      = V4L2_CID_PRIVATE_SAVE_USER,
-            .type    = V4L2_CTRL_TYPE_BUTTON,
-            .name    = "Save User Settings",
-            .minimum = 0,
-            .maximum = 0,
-            .step    = 0,
-            .default_value = 0,
-        },
-        {
-            .id      = V4L2_CID_PRIVATE_RESTORE_USER,
-            .type    = V4L2_CTRL_TYPE_BUTTON,
-            .name    = "Restore User Settings",
-            .minimum = 0,
-            .maximum = 0,
-            .step    = 0,
-            .default_value = 0,
-        },
-        {
-            .id      = V4L2_CID_PRIVATE_RESTORE_FACTORY,
-            .type    = V4L2_CTRL_TYPE_BUTTON,
-            .name    = "Restore Factory Settings",
-            .minimum = 0,
-            .maximum = 0,
-            .step    = 0,
-            .default_value = 0,
-        },
-        {
-            .id      = V4L2_CID_PRIVATE_COLOUR_MODE,
-            .type    = V4L2_CTRL_TYPE_BOOLEAN,
-            .name    = "Colour mode",
-            .minimum = 0,
-            .maximum = 1,
-            .step    = 1,
-            .default_value = 0,
-        },
-        {
-            .id      = V4L2_CID_PRIVATE_AUTOCONTOUR,
-            .type    = V4L2_CTRL_TYPE_BOOLEAN,
-            .name    = "Auto contour",
-            .minimum = 0,
-            .maximum = 1,
-            .step    = 1,
-            .default_value = 0,
-        },
-        {
-            .id      = V4L2_CID_PRIVATE_CONTOUR,
-            .type    = V4L2_CTRL_TYPE_INTEGER,
-            .name    = "Contour",
-            .minimum = 0,
-            .maximum = 63,
-            .step    = 1,
-            .default_value = 0,
-        },
-        {
-            .id      = V4L2_CID_PRIVATE_BACKLIGHT,
-            .type    = V4L2_CTRL_TYPE_BOOLEAN,
-            .name    = "Backlight compensation",
-            .minimum = 0,
-            .maximum = 1,
-            .step    = 1,
-            .default_value = 0,
-        },
-        {
-          .id      = V4L2_CID_PRIVATE_FLICKERLESS,
-            .type    = V4L2_CTRL_TYPE_BOOLEAN,
-            .name    = "Flickerless",
-            .minimum = 0,
-            .maximum = 1,
-            .step    = 1,
-            .default_value = 0,
-        },
-        {
-            .id      = V4L2_CID_PRIVATE_NOISE_REDUCTION,
-            .type    = V4L2_CTRL_TYPE_INTEGER,
-            .name    = "Noise reduction",
-            .minimum = 0,
-            .maximum = 3,
-            .step    = 1,
-            .default_value = 0,
-        },
+#define PWC_CID_CUSTOM(ctrl) ((V4L2_CID_USER_BASE | 0xf000) + custom_ ## ctrl)
+
+static int pwc_g_volatile_ctrl(struct v4l2_ctrl *ctrl);
+static int pwc_s_ctrl(struct v4l2_ctrl *ctrl);
+
+static const struct v4l2_ctrl_ops pwc_ctrl_ops = {
+        .g_volatile_ctrl = pwc_g_volatile_ctrl,
+        .s_ctrl = pwc_s_ctrl,
+};
+
+enum { awb_indoor, awb_outdoor, awb_fl, awb_manual, awb_auto };
+enum { custom_autocontour, custom_contour, custom_noise_reduction,
+        custom_save_user, custom_restore_user, custom_restore_factory };
+
+const char * const pwc_auto_whitebal_qmenu[] = {
+        "Indoor (Incandescant Lighting) Mode",
+        "Outdoor (Sunlight) Mode",
+        "Indoor (Fluorescent Lighting) Mode",
+        "Manual Mode",
+        "Auto Mode",
+        NULL
+};
+
+static const struct v4l2_ctrl_config pwc_auto_white_balance_cfg = {
+        .ops    = &pwc_ctrl_ops,
+        .id     = V4L2_CID_AUTO_WHITE_BALANCE,
+        .type   = V4L2_CTRL_TYPE_MENU,
+        .max    = awb_auto,
+        .qmenu  = pwc_auto_whitebal_qmenu,
+};
+
+static const struct v4l2_ctrl_config pwc_autocontour_cfg = {
+        .ops    = &pwc_ctrl_ops,
+        .id     = PWC_CID_CUSTOM(autocontour),
+        .type   = V4L2_CTRL_TYPE_BOOLEAN,
+        .name   = "Auto contour",
+        .min    = 0,
+        .max    = 1,
+        .step   = 1,
+};
+
+static const struct v4l2_ctrl_config pwc_contour_cfg = {
+        .ops    = &pwc_ctrl_ops,
+        .id     = PWC_CID_CUSTOM(contour),
+        .type   = V4L2_CTRL_TYPE_INTEGER,
+        .name   = "Contour",
+        .min    = 0,
+        .max    = 63,
+        .step   = 1,
+};
+
+static const struct v4l2_ctrl_config pwc_backlight_cfg = {
+        .ops    = &pwc_ctrl_ops,
+        .id     = V4L2_CID_BACKLIGHT_COMPENSATION,
+        .type   = V4L2_CTRL_TYPE_BOOLEAN,
+        .min    = 0,
+        .max    = 1,
+        .step   = 1,
+};
+
+static const struct v4l2_ctrl_config pwc_flicker_cfg = {
+        .ops    = &pwc_ctrl_ops,
+        .id     = V4L2_CID_BAND_STOP_FILTER,
+        .type   = V4L2_CTRL_TYPE_BOOLEAN,
+        .min    = 0,
+        .max    = 1,
+        .step   = 1,
+};
+
+static const struct v4l2_ctrl_config pwc_noise_reduction_cfg = {
+        .ops    = &pwc_ctrl_ops,
+        .id     = PWC_CID_CUSTOM(noise_reduction),
+        .type   = V4L2_CTRL_TYPE_INTEGER,
+        .name   = "Dynamic Noise Reduction",
+        .min    = 0,
+        .max    = 3,
+        .step   = 1,
+};
+
+static const struct v4l2_ctrl_config pwc_save_user_cfg = {
+        .ops    = &pwc_ctrl_ops,
+        .id     = PWC_CID_CUSTOM(save_user),
+        .type   = V4L2_CTRL_TYPE_BUTTON,
+        .name    = "Save User Settings",
 };
 
+static const struct v4l2_ctrl_config pwc_restore_user_cfg = {
+        .ops    = &pwc_ctrl_ops,
+        .id     = PWC_CID_CUSTOM(restore_user),
+        .type   = V4L2_CTRL_TYPE_BUTTON,
+        .name    = "Restore User Settings",
+};
+
+static const struct v4l2_ctrl_config pwc_restore_factory_cfg = {
+        .ops    = &pwc_ctrl_ops,
+        .id     = PWC_CID_CUSTOM(restore_factory),
+        .type   = V4L2_CTRL_TYPE_BUTTON,
+        .name    = "Restore Factory Settings",
+};
+
+int pwc_init_controls(struct pwc_device *pdev)
+{
+        struct v4l2_ctrl_handler *hdl;
+        struct v4l2_ctrl_config cfg;
+        int r, def;
+
+        hdl = &pdev->ctrl_handler;
+        r = v4l2_ctrl_handler_init(hdl, 20);
+        if (r)
+                return r;
+
+        /* Brightness, contrast, saturation, gamma */
+        r = pwc_get_u8_ctrl(pdev, GET_LUM_CTL, BRIGHTNESS_FORMATTER, &def);
+        if (r || def > 127)
+                def = 63;
+        pdev->brightness = v4l2_ctrl_new_std(hdl, &pwc_ctrl_ops,
+                                V4L2_CID_BRIGHTNESS, 0, 127, 1, def);
+
+        r = pwc_get_u8_ctrl(pdev, GET_LUM_CTL, CONTRAST_FORMATTER, &def);
+        if (r || def > 63)
+                def = 31;
+        pdev->contrast = v4l2_ctrl_new_std(hdl, &pwc_ctrl_ops,
+                                V4L2_CID_CONTRAST, 0, 63, 1, def);
+
+        if (pdev->type >= 675) {
+                if (pdev->type < 730)
+                        pdev->saturation_fmt = SATURATION_MODE_FORMATTER2;
+                else
+                        pdev->saturation_fmt = SATURATION_MODE_FORMATTER1;
+                r = pwc_get_s8_ctrl(pdev, GET_CHROM_CTL, pdev->saturation_fmt,
+                                    &def);
+                if (r || def < -100 || def > 100)
+                        def = 0;
+                pdev->saturation = v4l2_ctrl_new_std(hdl, &pwc_ctrl_ops,
+                                      V4L2_CID_SATURATION, -100, 100, 1, def);
+        }
+
+        r = pwc_get_u8_ctrl(pdev, GET_LUM_CTL, GAMMA_FORMATTER, &def);
+        if (r || def > 31)
+                def = 15;
+        pdev->gamma = v4l2_ctrl_new_std(hdl, &pwc_ctrl_ops,
+                                V4L2_CID_GAMMA, 0, 31, 1, def);
+
+        /* auto white balance, red gain, blue gain */
+        r = pwc_get_u8_ctrl(pdev, GET_CHROM_CTL, WB_MODE_FORMATTER, &def);
+        if (r || def > awb_auto)
+                def = awb_auto;
+        cfg = pwc_auto_white_balance_cfg;
+        cfg.name = v4l2_ctrl_get_name(cfg.id);
+        cfg.def = def;
+        pdev->auto_white_balance = v4l2_ctrl_new_custom(hdl, &cfg, NULL);
+        /* check auto controls to avoid NULL deref in v4l2_ctrl_auto_cluster */
+        if (!pdev->auto_white_balance)
+                return hdl->error;
+
+        r = pwc_get_u8_ctrl(pdev, GET_CHROM_CTL,
+                            PRESET_MANUAL_RED_GAIN_FORMATTER, &def);
+        if (r)
+                def = 127;
+        pdev->red_balance = v4l2_ctrl_new_std(hdl, &pwc_ctrl_ops,
+                                V4L2_CID_RED_BALANCE, 0, 255, 1, def);
+
+        r = pwc_get_u8_ctrl(pdev, GET_CHROM_CTL,
+                            PRESET_MANUAL_BLUE_GAIN_FORMATTER, &def);
+        if (r)
+                def = 127;
+        pdev->blue_balance = v4l2_ctrl_new_std(hdl, &pwc_ctrl_ops,
+                                V4L2_CID_BLUE_BALANCE, 0, 255, 1, def);
+
+        v4l2_ctrl_auto_cluster(3, &pdev->auto_white_balance, awb_manual,
+                               pdev->auto_white_balance->cur.val == awb_auto);
+
+        /* autogain, gain */
+        r = pwc_get_u8_ctrl(pdev, GET_LUM_CTL, AGC_MODE_FORMATTER, &def);
+        if (r || (def != 0 && def != 0xff))
+                def = 0;
+        /* Note a register value if 0 means auto gain is on */
+        pdev->autogain = v4l2_ctrl_new_std(hdl, &pwc_ctrl_ops,
+                                V4L2_CID_AUTOGAIN, 0, 1, 1, def == 0);
+        if (!pdev->autogain)
+                return hdl->error;
+
+        r = pwc_get_u8_ctrl(pdev, GET_LUM_CTL, PRESET_AGC_FORMATTER, &def);
+        if (r || def > 63)
+                def = 31;
+        pdev->gain = v4l2_ctrl_new_std(hdl, &pwc_ctrl_ops,
+                                V4L2_CID_GAIN, 0, 63, 1, def);
+
+        /* auto exposure, exposure */
+        if (DEVICE_USE_CODEC2(pdev->type)) {
+                r = pwc_get_u8_ctrl(pdev, GET_LUM_CTL, SHUTTER_MODE_FORMATTER,
+                                    &def);
+                if (r || (def != 0 && def != 0xff))
+                        def = 0;
+                /*
+                 * def = 0 auto, def = ff manual
+                 * menu idx 0 = auto, idx 1 = manual
+                 */
+                pdev->exposure_auto = v4l2_ctrl_new_std_menu(hdl,
+                                        &pwc_ctrl_ops,
+                                        V4L2_CID_EXPOSURE_AUTO,
+                                        1, 0, def != 0);
+                if (!pdev->exposure_auto)
+                        return hdl->error;
+
+                /* GET_LUM_CTL, PRESET_SHUTTER_FORMATTER is unreliable */
+                r = pwc_get_u16_ctrl(pdev, GET_STATUS_CTL,
+                                     READ_SHUTTER_FORMATTER, &def);
+                if (r || def > 655)
+                        def = 655;
+                pdev->exposure = v4l2_ctrl_new_std(hdl, &pwc_ctrl_ops,
+                                        V4L2_CID_EXPOSURE, 0, 655, 1, def);
+                /* CODEC2: separate auto gain & auto exposure */
+                v4l2_ctrl_auto_cluster(2, &pdev->autogain, 0, true);
+                v4l2_ctrl_auto_cluster(2, &pdev->exposure_auto,
+                                       V4L2_EXPOSURE_MANUAL, true);
+        } else if (DEVICE_USE_CODEC3(pdev->type)) {
+                /* GET_LUM_CTL, PRESET_SHUTTER_FORMATTER is unreliable */
+                r = pwc_get_u16_ctrl(pdev, GET_STATUS_CTL,
+                                     READ_SHUTTER_FORMATTER, &def);
+                if (r || def > 255)
+                        def = 255;
+                pdev->exposure = v4l2_ctrl_new_std(hdl, &pwc_ctrl_ops,
+                                        V4L2_CID_EXPOSURE, 0, 255, 1, def);
+                /* CODEC3: both gain and exposure controlled by autogain */
+                pdev->autogain_expo_cluster[0] = pdev->autogain;
+                pdev->autogain_expo_cluster[1] = pdev->gain;
+                pdev->autogain_expo_cluster[2] = pdev->exposure;
+                v4l2_ctrl_auto_cluster(3, pdev->autogain_expo_cluster,
+                                       0, true);
+        }
+
+        /* color / bw setting */
+        r = pwc_get_u8_ctrl(pdev, GET_CHROM_CTL, COLOUR_MODE_FORMATTER,
+                         &def);
+        if (r || (def != 0 && def != 0xff))
+                def = 0xff;
+        /* def = 0 bw, def = ff color, menu idx 0 = color, idx 1 = bw */
+        pdev->colorfx = v4l2_ctrl_new_std_menu(hdl, &pwc_ctrl_ops,
+                                V4L2_CID_COLORFX, 1, 0, def == 0);
+
+        /* autocontour, contour */
+        r = pwc_get_u8_ctrl(pdev, GET_LUM_CTL, AUTO_CONTOUR_FORMATTER, &def);
+        if (r || (def != 0 && def != 0xff))
+                def = 0;
+        cfg = pwc_autocontour_cfg;
+        cfg.def = def == 0;
+        pdev->autocontour = v4l2_ctrl_new_custom(hdl, &cfg, NULL);
+        if (!pdev->autocontour)
+                return hdl->error;
+
+        r = pwc_get_u8_ctrl(pdev, GET_LUM_CTL, PRESET_CONTOUR_FORMATTER, &def);
+        if (r || def > 63)
+                def = 31;
+        cfg = pwc_contour_cfg;
+        cfg.def = def;
+        pdev->contour = v4l2_ctrl_new_custom(hdl, &cfg, NULL);
+
+        v4l2_ctrl_auto_cluster(2, &pdev->autocontour, 0, false);
+
+        /* backlight */
+        r = pwc_get_u8_ctrl(pdev, GET_LUM_CTL,
+                            BACK_LIGHT_COMPENSATION_FORMATTER, &def);
+        if (r || (def != 0 && def != 0xff))
+                def = 0;
+        cfg = pwc_backlight_cfg;
+        cfg.name = v4l2_ctrl_get_name(cfg.id);
+        cfg.def = def == 0;
+        pdev->backlight = v4l2_ctrl_new_custom(hdl, &cfg, NULL);
+
+        /* flikker rediction */
+        r = pwc_get_u8_ctrl(pdev, GET_LUM_CTL,
+                            FLICKERLESS_MODE_FORMATTER, &def);
+        if (r || (def != 0 && def != 0xff))
+                def = 0;
+        cfg = pwc_flicker_cfg;
+        cfg.name = v4l2_ctrl_get_name(cfg.id);
+        cfg.def = def == 0;
+        pdev->flicker = v4l2_ctrl_new_custom(hdl, &cfg, NULL);
+
+        /* Dynamic noise reduction */
+        r = pwc_get_u8_ctrl(pdev, GET_LUM_CTL,
+                            DYNAMIC_NOISE_CONTROL_FORMATTER, &def);
+        if (r || def > 3)
+                def = 2;
+        cfg = pwc_noise_reduction_cfg;
+        cfg.def = def;
+        pdev->noise_reduction = v4l2_ctrl_new_custom(hdl, &cfg, NULL);
+
+        /* Save / Restore User / Factory Settings */
+        pdev->save_user = v4l2_ctrl_new_custom(hdl, &pwc_save_user_cfg, NULL);
+        pdev->restore_user = v4l2_ctrl_new_custom(hdl, &pwc_restore_user_cfg,
+                                                  NULL);
+        if (pdev->restore_user)
+                pdev->restore_user->flags = V4L2_CTRL_FLAG_UPDATE;
+        pdev->restore_factory = v4l2_ctrl_new_custom(hdl,
+                                                     &pwc_restore_factory_cfg,
+                                                     NULL);
+        if (pdev->restore_factory)
+                pdev->restore_factory->flags = V4L2_CTRL_FLAG_UPDATE;
+
+        if (!pdev->features & FEATURE_MOTOR_PANTILT)
+                return hdl->error;
+
+        /* Motor pan / tilt / reset */
+        pdev->motor_pan = v4l2_ctrl_new_std(hdl, &pwc_ctrl_ops,
+                                V4L2_CID_PAN_RELATIVE, -4480, 4480, 64, 0);
+        if (!pdev->motor_pan)
+                return hdl->error;
+        pdev->motor_tilt = v4l2_ctrl_new_std(hdl, &pwc_ctrl_ops,
+                                V4L2_CID_TILT_RELATIVE, -1920, 1920, 64, 0);
+        pdev->motor_pan_reset = v4l2_ctrl_new_std(hdl, &pwc_ctrl_ops,
+                                V4L2_CID_PAN_RESET, 0, 0, 0, 0);
+        pdev->motor_tilt_reset = v4l2_ctrl_new_std(hdl, &pwc_ctrl_ops,
+                                V4L2_CID_TILT_RESET, 0, 0, 0, 0);
+        v4l2_ctrl_cluster(4, &pdev->motor_pan);
+
+        return hdl->error;
+}
 
 static void pwc_vidioc_fill_fmt(const struct pwc_device *pdev, struct v4l2_format *f)
 {
@@ -284,10 +431,21 @@ static int pwc_vidioc_try_fmt(struct pwc_device *pdev, struct v4l2_format *f)
 }
 
 /* ioctl(VIDIOC_SET_FMT) */
-static int pwc_vidioc_set_fmt(struct pwc_device *pdev, struct v4l2_format *f)
+
+static int pwc_s_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f)
 {
+        struct pwc_device *pdev = video_drvdata(file);
         int ret, fps, snapshot, compression, pixelformat;
 
+        if (!pdev->udev)
+                return -ENODEV;
+
+        if (pdev->capt_file != NULL &&
+            pdev->capt_file != file)
+                return -EBUSY;
+
+        pdev->capt_file = file;
+
         ret = pwc_vidioc_try_fmt(pdev, f);
         if (ret<0)
                 return ret;
@@ -309,7 +467,7 @@ static int pwc_vidioc_set_fmt(struct pwc_device *pdev, struct v4l2_format *f)
             pixelformat != V4L2_PIX_FMT_PWC2)
                 return -EINVAL;
 
-        if (pdev->iso_init)
+        if (vb2_is_streaming(&pdev->vb_queue))
                 return -EBUSY;
 
         PWC_DEBUG_IOCTL("Trying to set format to: width=%d height=%d fps=%d "
@@ -343,13 +501,14 @@ static int pwc_vidioc_set_fmt(struct pwc_device *pdev, struct v4l2_format *f)
 
 static int pwc_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
 {
-        struct video_device *vdev = video_devdata(file);
         struct pwc_device *pdev = video_drvdata(file);
 
+        if (!pdev->udev)
+                return -ENODEV;
+
         strcpy(cap->driver, PWC_NAME);
-        strlcpy(cap->card, vdev->name, sizeof(cap->card));
+        strlcpy(cap->card, pdev->vdev.name, sizeof(cap->card));
         usb_make_path(pdev->udev, cap->bus_info, sizeof(cap->bus_info));
-        cap->version = PWC_VERSION_CODE;
         cap->capabilities =
                 V4L2_CAP_VIDEO_CAPTURE  |
                 V4L2_CAP_STREAMING      |
@@ -377,255 +536,396 @@ static int pwc_s_input(struct file *file, void *fh, unsigned int i)
         return i ? -EINVAL : 0;
 }
 
-static int pwc_queryctrl(struct file *file, void *fh, struct v4l2_queryctrl *c)
+static int pwc_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 {
-        int i, idx;
-        u32 id;
-
-        id = c->id;
-        if (id & V4L2_CTRL_FLAG_NEXT_CTRL) {
-                id &= V4L2_CTRL_ID_MASK;
-                id++;
-                idx = -1;
-                for (i = 0; i < ARRAY_SIZE(pwc_controls); i++) {
-                        if (pwc_controls[i].id < id)
-                                continue;
-                        if (idx >= 0
-                         && pwc_controls[i].id > pwc_controls[idx].id)
-                                continue;
-                        idx = i;
+        struct pwc_device *pdev =
+                container_of(ctrl->handler, struct pwc_device, ctrl_handler);
+        int ret = 0;
+
+        /*
+         * Sometimes it can take quite long for the pwc to complete usb control
+         * transfers, so release the modlock to give streaming by another
+         * process / thread the chance to continue with a dqbuf.
+         */
+        mutex_unlock(&pdev->modlock);
+
+        /*
+         * Take the udev-lock to protect against the disconnect handler
+         * completing and setting dev->udev to NULL underneath us. Other code
+         * does not need to do this since it is protected by the modlock.
+         */
+        mutex_lock(&pdev->udevlock);
+
+        if (!pdev->udev) {
+                ret = -ENODEV;
+                goto leave;
+        }
+
+        switch (ctrl->id) {
+        case V4L2_CID_AUTO_WHITE_BALANCE:
+                if (pdev->color_bal_valid && time_before(jiffies,
+                                pdev->last_color_bal_update + HZ / 4)) {
+                        pdev->red_balance->val  = pdev->last_red_balance;
+                        pdev->blue_balance->val = pdev->last_blue_balance;
+                        break;
                 }
-                if (idx < 0)
-                        return -EINVAL;
-                memcpy(c, &pwc_controls[idx], sizeof pwc_controls[0]);
-                return 0;
+                ret = pwc_get_u8_ctrl(pdev, GET_STATUS_CTL,
+                                      READ_RED_GAIN_FORMATTER,
+                                      &pdev->red_balance->val);
+                if (ret)
+                        break;
+                ret = pwc_get_u8_ctrl(pdev, GET_STATUS_CTL,
+                                      READ_BLUE_GAIN_FORMATTER,
+                                      &pdev->blue_balance->val);
+                if (ret)
+                        break;
+                pdev->last_red_balance  = pdev->red_balance->val;
+                pdev->last_blue_balance = pdev->blue_balance->val;
+                pdev->last_color_bal_update = jiffies;
+                pdev->color_bal_valid = true;
+                break;
+        case V4L2_CID_AUTOGAIN:
+                if (pdev->gain_valid && time_before(jiffies,
+                                pdev->last_gain_update + HZ / 4)) {
+                        pdev->gain->val = pdev->last_gain;
+                        break;
+                }
+                ret = pwc_get_u8_ctrl(pdev, GET_STATUS_CTL,
+                                      READ_AGC_FORMATTER, &pdev->gain->val);
+                if (ret)
+                        break;
+                pdev->last_gain = pdev->gain->val;
+                pdev->last_gain_update = jiffies;
+                pdev->gain_valid = true;
+                if (!DEVICE_USE_CODEC3(pdev->type))
+                        break;
+                /* Fall through for CODEC3 where autogain also controls expo */
+        case V4L2_CID_EXPOSURE_AUTO:
+                if (pdev->exposure_valid && time_before(jiffies,
+                                pdev->last_exposure_update + HZ / 4)) {
+                        pdev->exposure->val = pdev->last_exposure;
+                        break;
+                }
+                ret = pwc_get_u16_ctrl(pdev, GET_STATUS_CTL,
+                                       READ_SHUTTER_FORMATTER,
+                                       &pdev->exposure->val);
+                if (ret)
+                        break;
+                pdev->last_exposure = pdev->exposure->val;
+                pdev->last_exposure_update = jiffies;
+                pdev->exposure_valid = true;
+                break;
+        default:
+                ret = -EINVAL;
         }
-        for (i = 0; i < sizeof(pwc_controls) / sizeof(struct v4l2_queryctrl); i++) {
-                if (pwc_controls[i].id == c->id) {
-                        PWC_DEBUG_IOCTL("ioctl(VIDIOC_QUERYCTRL) found\n");
-                        memcpy(c, &pwc_controls[i], sizeof(struct v4l2_queryctrl));
-                        return 0;
+
+        if (ret)
+                PWC_ERROR("g_ctrl %s error %d\n", ctrl->name, ret);
+
+leave:
+        mutex_unlock(&pdev->udevlock);
+        mutex_lock(&pdev->modlock);
+        return ret;
+}
+
+static int pwc_set_awb(struct pwc_device *pdev)
+{
+        int ret = 0;
+
+        if (pdev->auto_white_balance->is_new) {
+                ret = pwc_set_u8_ctrl(pdev, SET_CHROM_CTL,
+                                      WB_MODE_FORMATTER,
+                                      pdev->auto_white_balance->val);
+                if (ret)
+                        return ret;
+
+                /* Update val when coming from auto or going to a preset */
+                if (pdev->red_balance->is_volatile ||
+                    pdev->auto_white_balance->val == awb_indoor ||
+                    pdev->auto_white_balance->val == awb_outdoor ||
+                    pdev->auto_white_balance->val == awb_fl) {
+                        if (!pdev->red_balance->is_new)
+                                pwc_get_u8_ctrl(pdev, GET_STATUS_CTL,
+                                        READ_RED_GAIN_FORMATTER,
+                                        &pdev->red_balance->val);
+                        if (!pdev->blue_balance->is_new)
+                                pwc_get_u8_ctrl(pdev, GET_STATUS_CTL,
+                                        READ_BLUE_GAIN_FORMATTER,
+                                        &pdev->blue_balance->val);
+                }
+                if (pdev->auto_white_balance->val == awb_auto) {
+                        pdev->red_balance->is_volatile = true;
+                        pdev->blue_balance->is_volatile = true;
+                        pdev->color_bal_valid = false; /* Force cache update */
+                } else {
+                        pdev->red_balance->is_volatile = false;
+                        pdev->blue_balance->is_volatile = false;
                 }
         }
-        return -EINVAL;
+
+        if (ret == 0 && pdev->red_balance->is_new) {
+                if (pdev->auto_white_balance->val != awb_manual)
+                        return -EBUSY;
+                ret = pwc_set_u8_ctrl(pdev, SET_CHROM_CTL,
+                                      PRESET_MANUAL_RED_GAIN_FORMATTER,
+                                      pdev->red_balance->val);
+        }
+
+        if (ret == 0 && pdev->blue_balance->is_new) {
+                if (pdev->auto_white_balance->val != awb_manual)
+                        return -EBUSY;
+                ret = pwc_set_u8_ctrl(pdev, SET_CHROM_CTL,
+                                      PRESET_MANUAL_BLUE_GAIN_FORMATTER,
+                                      pdev->blue_balance->val);
+        }
+        return ret;
 }
 
-static int pwc_g_ctrl(struct file *file, void *fh, struct v4l2_control *c)
+/* For CODEC2 models which have separate autogain and auto exposure */
+static int pwc_set_autogain(struct pwc_device *pdev)
 {
-        struct pwc_device *pdev = video_drvdata(file);
-        int ret;
+        int ret = 0;
+
+        if (pdev->autogain->is_new) {
+                ret = pwc_set_u8_ctrl(pdev, SET_LUM_CTL,
+                                      AGC_MODE_FORMATTER,
+                                      pdev->autogain->val ? 0 : 0xff);
+                if (ret)
+                        return ret;
+                if (pdev->autogain->val)
+                        pdev->gain_valid = false; /* Force cache update */
+                else if (!pdev->gain->is_new)
+                        pwc_get_u8_ctrl(pdev, GET_STATUS_CTL,
+                                        READ_AGC_FORMATTER,
+                                        &pdev->gain->val);
+        }
+        if (ret == 0 && pdev->gain->is_new) {
+                if (pdev->autogain->val)
+                        return -EBUSY;
+                ret = pwc_set_u8_ctrl(pdev, SET_LUM_CTL,
+                                      PRESET_AGC_FORMATTER,
+                                      pdev->gain->val);
+        }
+        return ret;
+}
 
-        switch (c->id) {
-        case V4L2_CID_BRIGHTNESS:
-                c->value = pwc_get_brightness(pdev);
-                if (c->value < 0)
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_CONTRAST:
-                c->value = pwc_get_contrast(pdev);
-                if (c->value < 0)
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_SATURATION:
-                ret = pwc_get_saturation(pdev, &c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_GAMMA:
-                c->value = pwc_get_gamma(pdev);
-                if (c->value < 0)
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_RED_BALANCE:
-                ret = pwc_get_red_gain(pdev, &c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                c->value >>= 8;
-                return 0;
-        case V4L2_CID_BLUE_BALANCE:
-                ret = pwc_get_blue_gain(pdev, &c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                c->value >>= 8;
-                return 0;
-        case V4L2_CID_AUTO_WHITE_BALANCE:
-                ret = pwc_get_awb(pdev);
-                if (ret < 0)
-                        return -EINVAL;
-                c->value = (ret == PWC_WB_MANUAL) ? 0 : 1;
-                return 0;
-        case V4L2_CID_GAIN:
-                ret = pwc_get_agc(pdev, &c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                c->value >>= 8;
-                return 0;
-        case V4L2_CID_AUTOGAIN:
-                ret = pwc_get_agc(pdev, &c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                c->value = (c->value < 0) ? 1 : 0;
-                return 0;
-        case V4L2_CID_EXPOSURE:
-                ret = pwc_get_shutter_speed(pdev, &c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_PRIVATE_COLOUR_MODE:
-                ret = pwc_get_colour_mode(pdev, &c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_PRIVATE_AUTOCONTOUR:
-                ret = pwc_get_contour(pdev, &c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                c->value = (c->value == -1 ? 1 : 0);
-                return 0;
-        case V4L2_CID_PRIVATE_CONTOUR:
-                ret = pwc_get_contour(pdev, &c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                c->value >>= 10;
-                return 0;
-        case V4L2_CID_PRIVATE_BACKLIGHT:
-                ret = pwc_get_backlight(pdev, &c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_PRIVATE_FLICKERLESS:
-                ret = pwc_get_flicker(pdev, &c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                c->value = (c->value ? 1 : 0);
-                return 0;
-        case V4L2_CID_PRIVATE_NOISE_REDUCTION:
-                ret = pwc_get_dynamic_noise(pdev, &c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
+/* For CODEC2 models which have separate autogain and auto exposure */
+static int pwc_set_exposure_auto(struct pwc_device *pdev)
+{
+        int ret = 0;
+        int is_auto = pdev->exposure_auto->val == V4L2_EXPOSURE_AUTO;
+
+        if (pdev->exposure_auto->is_new) {
+                ret = pwc_set_u8_ctrl(pdev, SET_LUM_CTL,
+                                      SHUTTER_MODE_FORMATTER,
+                                      is_auto ? 0 : 0xff);
+                if (ret)
+                        return ret;
+                if (is_auto)
+                        pdev->exposure_valid = false; /* Force cache update */
+                else if (!pdev->exposure->is_new)
+                        pwc_get_u16_ctrl(pdev, GET_STATUS_CTL,
+                                         READ_SHUTTER_FORMATTER,
+                                         &pdev->exposure->val);
+        }
+        if (ret == 0 && pdev->exposure->is_new) {
+                if (is_auto)
+                        return -EBUSY;
+                ret = pwc_set_u16_ctrl(pdev, SET_LUM_CTL,
+                                       PRESET_SHUTTER_FORMATTER,
+                                       pdev->exposure->val);
+        }
+        return ret;
+}
 
-        case V4L2_CID_PRIVATE_SAVE_USER:
-        case V4L2_CID_PRIVATE_RESTORE_USER:
-        case V4L2_CID_PRIVATE_RESTORE_FACTORY:
-                return -EINVAL;
+/* For CODEC3 models which have autogain controlling both gain and exposure */
+static int pwc_set_autogain_expo(struct pwc_device *pdev)
+{
+        int ret = 0;
+
+        if (pdev->autogain->is_new) {
+                ret = pwc_set_u8_ctrl(pdev, SET_LUM_CTL,
+                                      AGC_MODE_FORMATTER,
+                                      pdev->autogain->val ? 0 : 0xff);
+                if (ret)
+                        return ret;
+                if (pdev->autogain->val) {
+                        pdev->gain_valid     = false; /* Force cache update */
+                        pdev->exposure_valid = false; /* Force cache update */
+                } else {
+                        if (!pdev->gain->is_new)
+                                pwc_get_u8_ctrl(pdev, GET_STATUS_CTL,
+                                                READ_AGC_FORMATTER,
+                                                &pdev->gain->val);
+                        if (!pdev->exposure->is_new)
+                                pwc_get_u16_ctrl(pdev, GET_STATUS_CTL,
+                                                 READ_SHUTTER_FORMATTER,
+                                                 &pdev->exposure->val);
+                }
         }
-        return -EINVAL;
+        if (ret == 0 && pdev->gain->is_new) {
+                if (pdev->autogain->val)
+                        return -EBUSY;
+                ret = pwc_set_u8_ctrl(pdev, SET_LUM_CTL,
+                                      PRESET_AGC_FORMATTER,
+                                      pdev->gain->val);
+        }
+        if (ret == 0 && pdev->exposure->is_new) {
+                if (pdev->autogain->val)
+                        return -EBUSY;
+                ret = pwc_set_u16_ctrl(pdev, SET_LUM_CTL,
+                                       PRESET_SHUTTER_FORMATTER,
+                                       pdev->exposure->val);
+        }
+        return ret;
 }
 
-static int pwc_s_ctrl(struct file *file, void *fh, struct v4l2_control *c)
+static int pwc_set_motor(struct pwc_device *pdev)
 {
-        struct pwc_device *pdev = video_drvdata(file);
         int ret;
+        u8 buf[4];
+
+        buf[0] = 0;
+        if (pdev->motor_pan_reset->is_new)
+                buf[0] |= 0x01;
+        if (pdev->motor_tilt_reset->is_new)
+                buf[0] |= 0x02;
+        if (pdev->motor_pan_reset->is_new || pdev->motor_tilt_reset->is_new) {
+                ret = send_control_msg(pdev, SET_MPT_CTL,
+                                       PT_RESET_CONTROL_FORMATTER, buf, 1);
+                if (ret < 0)
+                        return ret;
+        }
 
-        switch (c->id) {
-        case V4L2_CID_BRIGHTNESS:
-                c->value <<= 9;
-                ret = pwc_set_brightness(pdev, c->value);
+        memset(buf, 0, sizeof(buf));
+        if (pdev->motor_pan->is_new) {
+                buf[0] = pdev->motor_pan->val & 0xFF;
+                buf[1] = (pdev->motor_pan->val >> 8);
+        }
+        if (pdev->motor_tilt->is_new) {
+                buf[2] = pdev->motor_tilt->val & 0xFF;
+                buf[3] = (pdev->motor_tilt->val >> 8);
+        }
+        if (pdev->motor_pan->is_new || pdev->motor_tilt->is_new) {
+                ret = send_control_msg(pdev, SET_MPT_CTL,
+                                       PT_RELATIVE_CONTROL_FORMATTER,
+                                       buf, sizeof(buf));
                 if (ret < 0)
-                        return -EINVAL;
-                return 0;
+                        return ret;
+        }
+
+        return 0;
+}
+
+static int pwc_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct pwc_device *pdev =
+                container_of(ctrl->handler, struct pwc_device, ctrl_handler);
+        int ret = 0;
+
+        /* See the comments on locking in pwc_g_volatile_ctrl */
+        mutex_unlock(&pdev->modlock);
+        mutex_lock(&pdev->udevlock);
+
+        if (!pdev->udev) {
+                ret = -ENODEV;
+                goto leave;
+        }
+
+        switch (ctrl->id) {
+        case V4L2_CID_BRIGHTNESS:
+                ret = pwc_set_u8_ctrl(pdev, SET_LUM_CTL,
+                                      BRIGHTNESS_FORMATTER, ctrl->val);
+                break;
         case V4L2_CID_CONTRAST:
-                c->value <<= 10;
-                ret = pwc_set_contrast(pdev, c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
+                ret = pwc_set_u8_ctrl(pdev, SET_LUM_CTL,
+                                      CONTRAST_FORMATTER, ctrl->val);
+                break;
         case V4L2_CID_SATURATION:
-                ret = pwc_set_saturation(pdev, c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
+                ret = pwc_set_s8_ctrl(pdev, SET_CHROM_CTL,
+                                      pdev->saturation_fmt, ctrl->val);
+                break;
         case V4L2_CID_GAMMA:
-                c->value <<= 11;
-                ret = pwc_set_gamma(pdev, c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_RED_BALANCE:
-                c->value <<= 8;
-                ret = pwc_set_red_gain(pdev, c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_BLUE_BALANCE:
-                c->value <<= 8;
-                ret = pwc_set_blue_gain(pdev, c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
+                ret = pwc_set_u8_ctrl(pdev, SET_LUM_CTL,
+                                      GAMMA_FORMATTER, ctrl->val);
+                break;
         case V4L2_CID_AUTO_WHITE_BALANCE:
-                c->value = (c->value == 0) ? PWC_WB_MANUAL : PWC_WB_AUTO;
-                ret = pwc_set_awb(pdev, c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_EXPOSURE:
-                c->value <<= 8;
-                ret = pwc_set_shutter_speed(pdev, c->value ? 0 : 1, c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
+                ret = pwc_set_awb(pdev);
+                break;
         case V4L2_CID_AUTOGAIN:
-                /* autogain off means nothing without a gain */
-                if (c->value == 0)
-                        return 0;
-                ret = pwc_set_agc(pdev, c->value, 0);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_GAIN:
-                c->value <<= 8;
-                ret = pwc_set_agc(pdev, 0, c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_PRIVATE_SAVE_USER:
-                if (pwc_save_user(pdev))
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_PRIVATE_RESTORE_USER:
-                if (pwc_restore_user(pdev))
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_PRIVATE_RESTORE_FACTORY:
-                if (pwc_restore_factory(pdev))
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_PRIVATE_COLOUR_MODE:
-                ret = pwc_set_colour_mode(pdev, c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_PRIVATE_AUTOCONTOUR:
-                c->value = (c->value == 1) ? -1 : 0;
-                ret = pwc_set_contour(pdev, c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_PRIVATE_CONTOUR:
-                c->value <<= 10;
-                ret = pwc_set_contour(pdev, c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_PRIVATE_BACKLIGHT:
-                ret = pwc_set_backlight(pdev, c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
-        case V4L2_CID_PRIVATE_FLICKERLESS:
-                ret = pwc_set_flicker(pdev, c->value);
-                if (ret < 0)
-                        return -EINVAL;
-        case V4L2_CID_PRIVATE_NOISE_REDUCTION:
-                ret = pwc_set_dynamic_noise(pdev, c->value);
-                if (ret < 0)
-                        return -EINVAL;
-                return 0;
-
+                if (DEVICE_USE_CODEC2(pdev->type))
+                        ret = pwc_set_autogain(pdev);
+                else if (DEVICE_USE_CODEC3(pdev->type))
+                        ret = pwc_set_autogain_expo(pdev);
+                else
+                        ret = -EINVAL;
+                break;
+        case V4L2_CID_EXPOSURE_AUTO:
+                if (DEVICE_USE_CODEC2(pdev->type))
+                        ret = pwc_set_exposure_auto(pdev);
+                else
+                        ret = -EINVAL;
+                break;
+        case V4L2_CID_COLORFX:
+                ret = pwc_set_u8_ctrl(pdev, SET_CHROM_CTL,
+                                      COLOUR_MODE_FORMATTER,
+                                      ctrl->val ? 0 : 0xff);
+                break;
+        case PWC_CID_CUSTOM(autocontour):
+                if (pdev->autocontour->is_new) {
+                        ret = pwc_set_u8_ctrl(pdev, SET_LUM_CTL,
+                                        AUTO_CONTOUR_FORMATTER,
+                                        pdev->autocontour->val ? 0 : 0xff);
+                }
+                if (ret == 0 && pdev->contour->is_new) {
+                        if (pdev->autocontour->val) {
+                                ret = -EBUSY;
+                                break;
+                        }
+                        ret = pwc_set_u8_ctrl(pdev, SET_LUM_CTL,
+                                              PRESET_CONTOUR_FORMATTER,
+                                              pdev->contour->val);
+                }
+                break;
+        case V4L2_CID_BACKLIGHT_COMPENSATION:
+                ret = pwc_set_u8_ctrl(pdev, SET_LUM_CTL,
+                                      BACK_LIGHT_COMPENSATION_FORMATTER,
+                                      ctrl->val ? 0 : 0xff);
+                break;
+        case V4L2_CID_BAND_STOP_FILTER:
+                ret = pwc_set_u8_ctrl(pdev, SET_LUM_CTL,
+                                      FLICKERLESS_MODE_FORMATTER,
+                                      ctrl->val ? 0 : 0xff);
+                break;
+        case PWC_CID_CUSTOM(noise_reduction):
+                ret = pwc_set_u8_ctrl(pdev, SET_LUM_CTL,
+                                      DYNAMIC_NOISE_CONTROL_FORMATTER,
+                                      ctrl->val);
+                break;
+        case PWC_CID_CUSTOM(save_user):
+                ret = pwc_button_ctrl(pdev, SAVE_USER_DEFAULTS_FORMATTER);
+                break;
+        case PWC_CID_CUSTOM(restore_user):
+                ret = pwc_button_ctrl(pdev, RESTORE_USER_DEFAULTS_FORMATTER);
+                break;
+        case PWC_CID_CUSTOM(restore_factory):
+                ret = pwc_button_ctrl(pdev,
+                                      RESTORE_FACTORY_DEFAULTS_FORMATTER);
+                break;
+        case V4L2_CID_PAN_RELATIVE:
+                ret = pwc_set_motor(pdev);
+                break;
+        default:
+                ret = -EINVAL;
         }
-        return -EINVAL;
+
+        if (ret)
+                PWC_ERROR("s_ctrl %s error %d\n", ctrl->name, ret);
+
+leave:
+        mutex_unlock(&pdev->udevlock);
+        mutex_lock(&pdev->modlock);
+        return ret;
 }
 
 static int pwc_enum_fmt_vid_cap(struct file *file, void *fh, struct v4l2_fmtdesc *f)
@@ -667,157 +967,77 @@ static int pwc_try_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *
         return pwc_vidioc_try_fmt(pdev, f);
 }
 
-static int pwc_s_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f)
+static int pwc_reqbufs(struct file *file, void *fh,
+                       struct v4l2_requestbuffers *rb)
 {
         struct pwc_device *pdev = video_drvdata(file);
 
-        return pwc_vidioc_set_fmt(pdev, f);
-}
-
-static int pwc_reqbufs(struct file *file, void *fh, struct v4l2_requestbuffers *rb)
-{
-        int nbuffers;
+        if (pdev->capt_file != NULL &&
+            pdev->capt_file != file)
+                return -EBUSY;
 
-        PWC_DEBUG_IOCTL("ioctl(VIDIOC_REQBUFS) count=%d\n", rb->count);
-        if (rb->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-                return -EINVAL;
-        if (rb->memory != V4L2_MEMORY_MMAP)
-                return -EINVAL;
+        pdev->capt_file = file;
 
-        nbuffers = rb->count;
-        if (nbuffers < 2)
-                nbuffers = 2;
-        else if (nbuffers > pwc_mbufs)
-                nbuffers = pwc_mbufs;
-        /* Force to use our # of buffers */
-        rb->count = pwc_mbufs;
-        return 0;
+        return vb2_reqbufs(&pdev->vb_queue, rb);
 }
 
 static int pwc_querybuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 {
         struct pwc_device *pdev = video_drvdata(file);
-        int index;
 
-        PWC_DEBUG_IOCTL("ioctl(VIDIOC_QUERYBUF) index=%d\n", buf->index);
-        if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-                PWC_DEBUG_IOCTL("ioctl(VIDIOC_QUERYBUF) Bad type\n");
-                return -EINVAL;
-        }
-        index = buf->index;
-        if (index < 0 || index >= pwc_mbufs) {
-                PWC_DEBUG_IOCTL("ioctl(VIDIOC_QUERYBUF) Bad index %d\n", buf->index);
-                return -EINVAL;
-        }
-
-        buf->m.offset = index * pdev->len_per_image;
-        if (pdev->pixfmt != V4L2_PIX_FMT_YUV420)
-                buf->bytesused = pdev->frame_size + sizeof(struct pwc_raw_frame);
-        else
-                buf->bytesused = pdev->view.size;
-        buf->field = V4L2_FIELD_NONE;
-        buf->memory = V4L2_MEMORY_MMAP;
-        /*buf->flags = V4L2_BUF_FLAG_MAPPED;*/
-        buf->length = pdev->len_per_image;
-
-        PWC_DEBUG_READ("VIDIOC_QUERYBUF: index=%d\n", buf->index);
-        PWC_DEBUG_READ("VIDIOC_QUERYBUF: m.offset=%d\n", buf->m.offset);
-        PWC_DEBUG_READ("VIDIOC_QUERYBUF: bytesused=%d\n", buf->bytesused);
-
-        return 0;
+        return vb2_querybuf(&pdev->vb_queue, buf);
 }
 
 static int pwc_qbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 {
-        PWC_DEBUG_IOCTL("ioctl(VIDIOC_QBUF) index=%d\n", buf->index);
-        if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-                return -EINVAL;
-        if (buf->memory != V4L2_MEMORY_MMAP)
-                return -EINVAL;
-        if (buf->index >= pwc_mbufs)
-                return -EINVAL;
+        struct pwc_device *pdev = video_drvdata(file);
 
-        buf->flags |= V4L2_BUF_FLAG_QUEUED;
-        buf->flags &= ~V4L2_BUF_FLAG_DONE;
+        if (!pdev->udev)
+                return -ENODEV;
 
-        return 0;
+        if (pdev->capt_file != file)
+                return -EBUSY;
+
+        return vb2_qbuf(&pdev->vb_queue, buf);
 }
 
 static int pwc_dqbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 {
-        DECLARE_WAITQUEUE(wait, current);
         struct pwc_device *pdev = video_drvdata(file);
-        int ret;
 
-        PWC_DEBUG_IOCTL("ioctl(VIDIOC_DQBUF)\n");
+        if (!pdev->udev)
+                return -ENODEV;
 
-        if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-                return -EINVAL;
-
-        add_wait_queue(&pdev->frameq, &wait);
-        while (pdev->full_frames == NULL) {
-                if (pdev->error_status) {
-                        remove_wait_queue(&pdev->frameq, &wait);
-                        set_current_state(TASK_RUNNING);
-                        return -pdev->error_status;
-                }
-
-                if (signal_pending(current)) {
-                        remove_wait_queue(&pdev->frameq, &wait);
-                        set_current_state(TASK_RUNNING);
-                        return -ERESTARTSYS;
-                }
-                mutex_unlock(&pdev->modlock);
-                schedule();
-                set_current_state(TASK_INTERRUPTIBLE);
-                mutex_lock(&pdev->modlock);
-        }
-        remove_wait_queue(&pdev->frameq, &wait);
-        set_current_state(TASK_RUNNING);
-
-        PWC_DEBUG_IOCTL("VIDIOC_DQBUF: frame ready.\n");
-        /* Decompress data in pdev->images[pdev->fill_image] */
-        ret = pwc_handle_frame(pdev);
-        if (ret)
-                return -EFAULT;
-        PWC_DEBUG_IOCTL("VIDIOC_DQBUF: after pwc_handle_frame\n");
-
-        buf->index = pdev->fill_image;
-        if (pdev->pixfmt != V4L2_PIX_FMT_YUV420)
-                buf->bytesused = pdev->frame_size + sizeof(struct pwc_raw_frame);
-        else
-                buf->bytesused = pdev->view.size;
-        buf->flags = V4L2_BUF_FLAG_MAPPED;
-        buf->field = V4L2_FIELD_NONE;
-        do_gettimeofday(&buf->timestamp);
-        buf->sequence = 0;
-        buf->memory = V4L2_MEMORY_MMAP;
-        buf->m.offset = pdev->fill_image * pdev->len_per_image;
-        buf->length = pdev->len_per_image;
-        pwc_next_image(pdev);
-
-        PWC_DEBUG_IOCTL("VIDIOC_DQBUF: buf->index=%d\n", buf->index);
-        PWC_DEBUG_IOCTL("VIDIOC_DQBUF: buf->length=%d\n", buf->length);
-        PWC_DEBUG_IOCTL("VIDIOC_DQBUF: m.offset=%d\n", buf->m.offset);
-        PWC_DEBUG_IOCTL("VIDIOC_DQBUF: bytesused=%d\n", buf->bytesused);
-        PWC_DEBUG_IOCTL("VIDIOC_DQBUF: leaving\n");
-        return 0;
+        if (pdev->capt_file != file)
+                return -EBUSY;
 
+        return vb2_dqbuf(&pdev->vb_queue, buf, file->f_flags & O_NONBLOCK);
 }
 
 static int pwc_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
 {
         struct pwc_device *pdev = video_drvdata(file);
 
-        return pwc_isoc_init(pdev);
+        if (!pdev->udev)
+                return -ENODEV;
+
+        if (pdev->capt_file != file)
+                return -EBUSY;
+
+        return vb2_streamon(&pdev->vb_queue, i);
 }
 
 static int pwc_streamoff(struct file *file, void *fh, enum v4l2_buf_type i)
 {
         struct pwc_device *pdev = video_drvdata(file);
 
-        pwc_isoc_cleanup(pdev);
-        return 0;
+        if (!pdev->udev)
+                return -ENODEV;
+
+        if (pdev->capt_file != file)
+                return -EBUSY;
+
+        return vb2_streamoff(&pdev->vb_queue, i);
 }
 
 static int pwc_enum_framesizes(struct file *file, void *fh,
@@ -896,9 +1116,6 @@ const struct v4l2_ioctl_ops pwc_ioctl_ops = {
         .vidioc_g_fmt_vid_cap               = pwc_g_fmt_vid_cap,
         .vidioc_s_fmt_vid_cap               = pwc_s_fmt_vid_cap,
         .vidioc_try_fmt_vid_cap             = pwc_try_fmt_vid_cap,
-        .vidioc_queryctrl                   = pwc_queryctrl,
-        .vidioc_g_ctrl                      = pwc_g_ctrl,
-        .vidioc_s_ctrl                      = pwc_s_ctrl,
         .vidioc_reqbufs                     = pwc_reqbufs,
         .vidioc_querybuf                    = pwc_querybuf,
         .vidioc_qbuf                        = pwc_qbuf,
diff --git a/drivers/media/video/pwc/pwc.h b/drivers/media/video/pwc/pwc.h
index 083f8b15df7..0e4e2d7b787 100644
--- a/drivers/media/video/pwc/pwc.h
+++ b/drivers/media/video/pwc/pwc.h
@@ -29,7 +29,6 @@
 #include <linux/usb.h>
 #include <linux/spinlock.h>
 #include <linux/wait.h>
-#include <linux/version.h>
 #include <linux/mutex.h>
 #include <linux/mm.h>
 #include <linux/slab.h>
@@ -37,19 +36,16 @@
 #include <linux/videodev2.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/videobuf2-vmalloc.h>
 #ifdef CONFIG_USB_PWC_INPUT_EVDEV
 #include <linux/input.h>
 #endif
 
-#include "pwc-uncompress.h"
 #include <media/pwc-ioctl.h>
 
 /* Version block */
-#define PWC_MAJOR       10
-#define PWC_MINOR       0
-#define PWC_EXTRAMINOR  12
-#define PWC_VERSION_CODE KERNEL_VERSION(PWC_MAJOR,PWC_MINOR,PWC_EXTRAMINOR)
-#define PWC_VERSION     "10.0.14"
+#define PWC_VERSION     "10.0.15"
 #define PWC_NAME        "pwc"
 #define PFX             PWC_NAME ": "
 
@@ -81,9 +77,9 @@
 #define PWC_DEBUG_LEVEL (PWC_DEBUG_LEVEL_MODULE)
 
 #define PWC_DEBUG(level, fmt, args...) do {\
-          if ((PWC_DEBUG_LEVEL_ ##level) & pwc_trace) \
-             printk(KERN_DEBUG PFX fmt, ##args); \
-          } while(0)
+        if ((PWC_DEBUG_LEVEL_ ##level) & pwc_trace) \
+                printk(KERN_DEBUG PFX fmt, ##args); \
+        } while (0)
 
 #define PWC_ERROR(fmt, args...) printk(KERN_ERR PFX fmt, ##args)
 #define PWC_WARNING(fmt, args...) printk(KERN_WARNING PFX fmt, ##args)
@@ -110,25 +106,21 @@
 #define FEATURE_CODEC1                  0x0002
 #define FEATURE_CODEC2                  0x0004
 
-/* Turn certain features on/off */
-#define PWC_INT_PIPE 0
-
 /* Ignore errors in the first N frames, to allow for startup delays */
 #define FRAME_LOWMARK 5
 
 /* Size and number of buffers for the ISO pipe. */
-#define MAX_ISO_BUFS            2
+#define MAX_ISO_BUFS            3
 #define ISO_FRAMES_PER_DESC     10
 #define ISO_MAX_FRAME_SIZE      960
 #define ISO_BUFFER_SIZE         (ISO_FRAMES_PER_DESC * ISO_MAX_FRAME_SIZE)
 
-/* Frame buffers: contains compressed or uncompressed video data. */
-#define MAX_FRAMES              5
 /* Maximum size after decompression is 640x480 YUV data, 1.5 * 640 * 480 */
 #define PWC_FRAME_SIZE          (460800 + TOUCAM_HEADER_SIZE + TOUCAM_TRAILER_SIZE)
 
-/* Absolute maximum number of buffers available for mmap() */
-#define MAX_IMAGES              10
+/* Absolute minimum and maximum number of buffers available for mmap() */
+#define MIN_FRAMES              2
+#define MAX_FRAMES              16
 
 /* Some macros to quickly find the type of a webcam */
 #define DEVICE_USE_CODEC1(x) ((x)<675)
@@ -136,149 +128,221 @@
 #define DEVICE_USE_CODEC3(x) ((x)>=700)
 #define DEVICE_USE_CODEC23(x) ((x)>=675)
 
-/* The following structures were based on cpia.h. Why reinvent the wheel? :-) */
-struct pwc_iso_buf
-{
-        void *data;
-        int  length;
-        int  read;
-        struct urb *urb;
-};
+/* from pwc-dec.h */
+#define PWCX_FLAG_PLANAR        0x0001
+
+/* Request types: video */
+#define SET_LUM_CTL                     0x01
+#define GET_LUM_CTL                     0x02
+#define SET_CHROM_CTL                   0x03
+#define GET_CHROM_CTL                   0x04
+#define SET_STATUS_CTL                  0x05
+#define GET_STATUS_CTL                  0x06
+#define SET_EP_STREAM_CTL               0x07
+#define GET_EP_STREAM_CTL               0x08
+#define GET_XX_CTL                      0x09
+#define SET_XX_CTL                      0x0A
+#define GET_XY_CTL                      0x0B
+#define SET_XY_CTL                      0x0C
+#define SET_MPT_CTL                     0x0D
+#define GET_MPT_CTL                     0x0E
+
+/* Selectors for the Luminance controls [GS]ET_LUM_CTL */
+#define AGC_MODE_FORMATTER                      0x2000
+#define PRESET_AGC_FORMATTER                    0x2100
+#define SHUTTER_MODE_FORMATTER                  0x2200
+#define PRESET_SHUTTER_FORMATTER                0x2300
+#define PRESET_CONTOUR_FORMATTER                0x2400
+#define AUTO_CONTOUR_FORMATTER                  0x2500
+#define BACK_LIGHT_COMPENSATION_FORMATTER       0x2600
+#define CONTRAST_FORMATTER                      0x2700
+#define DYNAMIC_NOISE_CONTROL_FORMATTER         0x2800
+#define FLICKERLESS_MODE_FORMATTER              0x2900
+#define AE_CONTROL_SPEED                        0x2A00
+#define BRIGHTNESS_FORMATTER                    0x2B00
+#define GAMMA_FORMATTER                         0x2C00
+
+/* Selectors for the Chrominance controls [GS]ET_CHROM_CTL */
+#define WB_MODE_FORMATTER                       0x1000
+#define AWB_CONTROL_SPEED_FORMATTER             0x1100
+#define AWB_CONTROL_DELAY_FORMATTER             0x1200
+#define PRESET_MANUAL_RED_GAIN_FORMATTER        0x1300
+#define PRESET_MANUAL_BLUE_GAIN_FORMATTER       0x1400
+#define COLOUR_MODE_FORMATTER                   0x1500
+#define SATURATION_MODE_FORMATTER1              0x1600
+#define SATURATION_MODE_FORMATTER2              0x1700
+
+/* Selectors for the Status controls [GS]ET_STATUS_CTL */
+#define SAVE_USER_DEFAULTS_FORMATTER            0x0200
+#define RESTORE_USER_DEFAULTS_FORMATTER         0x0300
+#define RESTORE_FACTORY_DEFAULTS_FORMATTER      0x0400
+#define READ_AGC_FORMATTER                      0x0500
+#define READ_SHUTTER_FORMATTER                  0x0600
+#define READ_RED_GAIN_FORMATTER                 0x0700
+#define READ_BLUE_GAIN_FORMATTER                0x0800
+
+/* Formatters for the motorized pan & tilt [GS]ET_MPT_CTL */
+#define PT_RELATIVE_CONTROL_FORMATTER           0x01
+#define PT_RESET_CONTROL_FORMATTER              0x02
+#define PT_STATUS_FORMATTER                     0x03
 
 /* intermediate buffers with raw data from the USB cam */
 struct pwc_frame_buf
 {
-   void *data;
-   volatile int filled;         /* number of bytes filled */
-   struct pwc_frame_buf *next;  /* list */
-};
-
-/* additionnal informations used when dealing image between kernel and userland */
-struct pwc_imgbuf
-{
-        unsigned long offset;   /* offset of this buffer in the big array of image_data */
-        int   vma_use_count;    /* count the number of time this memory is mapped */
+        struct vb2_buffer vb;   /* common v4l buffer stuff -- must be first */
+        struct list_head list;
+        void *data;
+        int filled;             /* number of bytes filled */
 };
 
 struct pwc_device
 {
         struct video_device vdev;
-
-   /* Pointer to our usb_device, may be NULL after unplug */
-   struct usb_device *udev;
-
-   int type;                    /* type of cam (645, 646, 675, 680, 690, 720, 730, 740, 750) */
-   int release;                 /* release number */
-   int features;                /* feature bits */
-   char serial[30];             /* serial number (string) */
-   int error_status;            /* set when something goes wrong with the cam (unplugged, USB errors) */
-   int usb_init;                /* set when the cam has been initialized over USB */
-
-   /*** Video data ***/
-   int vopen;                   /* flag */
-   int vendpoint;               /* video isoc endpoint */
-   int vcinterface;             /* video control interface */
-   int valternate;              /* alternate interface needed */
-   int vframes, vsize;          /* frames-per-second & size (see PSZ_*) */
-   int pixfmt;                  /* pixelformat: V4L2_PIX_FMT_YUV420 or raw: _PWC1, _PWC2 */
-   int vframe_count;            /* received frames */
-   int vframes_dumped;          /* counter for dumped frames */
-   int vframes_error;           /* frames received in error */
-   int vmax_packet_size;        /* USB maxpacket size */
-   int vlast_packet_size;       /* for frame synchronisation */
-   int visoc_errors;            /* number of contiguous ISOC errors */
-   int vcompression;            /* desired compression factor */
-   int vbandlength;             /* compressed band length; 0 is uncompressed */
-   char vsnapshot;              /* snapshot mode */
-   char vsync;                  /* used by isoc handler */
-   char vmirror;                /* for ToUCaM series */
-        char unplugged;
-
-   int cmd_len;
-   unsigned char cmd_buf[13];
-
-   /* The image acquisition requires 3 to 4 steps:
-      1. data is gathered in short packets from the USB controller
-      2. data is synchronized and packed into a frame buffer
-      3a. in case data is compressed, decompress it directly into image buffer
-      3b. in case data is uncompressed, copy into image buffer with viewport
-      4. data is transferred to the user process
-
-      Note that MAX_ISO_BUFS != MAX_FRAMES != MAX_IMAGES....
-      We have in effect a back-to-back-double-buffer system.
-    */
-   /* 1: isoc */
-   struct pwc_iso_buf sbuf[MAX_ISO_BUFS];
-   char iso_init;
-
-   /* 2: frame */
-   struct pwc_frame_buf *fbuf;  /* all frames */
-   struct pwc_frame_buf *empty_frames, *empty_frames_tail;      /* all empty frames */
-   struct pwc_frame_buf *full_frames, *full_frames_tail;        /* all filled frames */
-   struct pwc_frame_buf *fill_frame;    /* frame currently being filled */
-   struct pwc_frame_buf *read_frame;    /* frame currently read by user process */
-   int frame_header_size, frame_trailer_size;
-   int frame_size;
-   int frame_total_size; /* including header & trailer */
-   int drop_frames;
-
-   /* 3: decompression */
-   void *decompress_data;               /* private data for decompression engine */
-
-   /* 4: image */
-   /* We have an 'image' and a 'view', where 'image' is the fixed-size image
-      as delivered by the camera, and 'view' is the size requested by the
-      program. The camera image is centered in this viewport, laced with
-      a gray or black border. view_min <= image <= view <= view_max;
-    */
-   int image_mask;                      /* bitmask of supported sizes */
-   struct pwc_coord view_min, view_max; /* minimum and maximum viewable sizes */
-   struct pwc_coord abs_max;            /* maximum supported size with compression */
-   struct pwc_coord image, view;        /* image and viewport size */
-   struct pwc_coord offset;             /* offset within the viewport */
-
-   void *image_data;                    /* total buffer, which is subdivided into ... */
-   struct pwc_imgbuf images[MAX_IMAGES];/* ...several images... */
-   int fill_image;                      /* ...which are rotated. */
-   int len_per_image;                   /* length per image */
-   int image_read_pos;                  /* In case we read data in pieces, keep track of were we are in the imagebuffer */
-   int image_used[MAX_IMAGES];          /* For MCAPTURE and SYNC */
-
-   struct mutex modlock;                /* to prevent races in video_open(), etc */
-   spinlock_t ptrlock;                  /* for manipulating the buffer pointers */
-
-   /*** motorized pan/tilt feature */
-   struct pwc_mpt_range angle_range;
-   int pan_angle;                       /* in degrees * 100 */
-   int tilt_angle;                      /* absolute angle; 0,0 is home position */
-   int snapshot_button_status;          /* set to 1 when the user push the button, reset to 0 when this value is read */
+        struct mutex modlock;
+
+        /* Pointer to our usb_device, may be NULL after unplug */
+        struct usb_device *udev;
+        /* Protects the setting of udev to NULL by our disconnect handler */
+        struct mutex udevlock;
+
+        /* type of cam (645, 646, 675, 680, 690, 720, 730, 740, 750) */
+        int type;
+        int release;            /* release number */
+        int features;           /* feature bits */
+        char serial[30];        /* serial number (string) */
+
+        /*** Video data ***/
+        struct file *capt_file; /* file doing video capture */
+        int vendpoint;          /* video isoc endpoint */
+        int vcinterface;        /* video control interface */
+        int valternate;         /* alternate interface needed */
+        int vframes, vsize;     /* frames-per-second & size (see PSZ_*) */
+        int pixfmt;             /* pixelformat: V4L2_PIX_FMT_YUV420 or _PWCX */
+        int vframe_count;       /* received frames */
+        int vmax_packet_size;   /* USB maxpacket size */
+        int vlast_packet_size;  /* for frame synchronisation */
+        int visoc_errors;       /* number of contiguous ISOC errors */
+        int vcompression;       /* desired compression factor */
+        int vbandlength;        /* compressed band length; 0 is uncompressed */
+        char vsnapshot;         /* snapshot mode */
+        char vsync;             /* used by isoc handler */
+        char vmirror;           /* for ToUCaM series */
+        char power_save;        /* Do powersaving for this cam */
+
+        int cmd_len;
+        unsigned char cmd_buf[13];
+
+        struct urb *urbs[MAX_ISO_BUFS];
+        char iso_init;
+
+        /* videobuf2 queue and queued buffers list */
+        struct vb2_queue vb_queue;
+        struct list_head queued_bufs;
+        spinlock_t queued_bufs_lock;
+
+        /*
+         * Frame currently being filled, this only gets touched by the
+         * isoc urb complete handler, and by stream start / stop since
+         * start / stop touch it before / after starting / killing the urbs
+         * no locking is needed around this
+         */
+        struct pwc_frame_buf *fill_buf;
+
+        int frame_header_size, frame_trailer_size;
+        int frame_size;
+        int frame_total_size;   /* including header & trailer */
+        int drop_frames;
+
+        void *decompress_data;  /* private data for decompression engine */
+
+        /*
+         * We have an 'image' and a 'view', where 'image' is the fixed-size img
+         * as delivered by the camera, and 'view' is the size requested by the
+         * program. The camera image is centered in this viewport, laced with
+         * a gray or black border. view_min <= image <= view <= view_max;
+         */
+        int image_mask;                         /* supported sizes */
+        struct pwc_coord view_min, view_max;    /* minimum and maximum view */
+        struct pwc_coord abs_max;               /* maximum supported size */
+        struct pwc_coord image, view;           /* image and viewport size */
+        struct pwc_coord offset;                /* offset of the viewport */
+
+        /*** motorized pan/tilt feature */
+        struct pwc_mpt_range angle_range;
+        int pan_angle;                  /* in degrees * 100 */
+        int tilt_angle;                 /* absolute angle; 0,0 is home */
+
+        /*
+         * Set to 1 when the user push the button, reset to 0
+         * when this value is read from sysfs.
+         */
+        int snapshot_button_status;
 #ifdef CONFIG_USB_PWC_INPUT_EVDEV
-   struct input_dev *button_dev;        /* webcam snapshot button input */
-   char button_phys[64];
+        struct input_dev *button_dev;   /* webcam snapshot button input */
+        char button_phys[64];
 #endif
 
-   /*** Misc. data ***/
-   wait_queue_head_t frameq;            /* When waiting for a frame to finish... */
-#if PWC_INT_PIPE
-   void *usb_int_handler;               /* for the interrupt endpoint */
-#endif
+        /* controls */
+        struct v4l2_ctrl_handler        ctrl_handler;
+        u16                             saturation_fmt;
+        struct v4l2_ctrl                *brightness;
+        struct v4l2_ctrl                *contrast;
+        struct v4l2_ctrl                *saturation;
+        struct v4l2_ctrl                *gamma;
+        struct {
+                /* awb / red-blue balance cluster */
+                struct v4l2_ctrl        *auto_white_balance;
+                struct v4l2_ctrl        *red_balance;
+                struct v4l2_ctrl        *blue_balance;
+                /* usb ctrl transfers are slow, so we cache things */
+                int                     color_bal_valid;
+                unsigned long           last_color_bal_update; /* In jiffies */
+                s32                     last_red_balance;
+                s32                     last_blue_balance;
+        };
+        struct {
+                /* autogain / gain cluster */
+                struct v4l2_ctrl        *autogain;
+                struct v4l2_ctrl        *gain;
+                int                     gain_valid;
+                unsigned long           last_gain_update; /* In jiffies */
+                s32                     last_gain;
+        };
+        struct {
+                /* exposure_auto / exposure cluster */
+                struct v4l2_ctrl        *exposure_auto;
+                struct v4l2_ctrl        *exposure;
+                int                     exposure_valid;
+                unsigned long           last_exposure_update; /* In jiffies */
+                s32                     last_exposure;
+        };
+        struct v4l2_ctrl                *colorfx;
+        struct {
+                /* autocontour/contour cluster */
+                struct v4l2_ctrl        *autocontour;
+                struct v4l2_ctrl        *contour;
+        };
+        struct v4l2_ctrl                *backlight;
+        struct v4l2_ctrl                *flicker;
+        struct v4l2_ctrl                *noise_reduction;
+        struct v4l2_ctrl                *save_user;
+        struct v4l2_ctrl                *restore_user;
+        struct v4l2_ctrl                *restore_factory;
+        struct {
+                /* motor control cluster */
+                struct v4l2_ctrl        *motor_pan;
+                struct v4l2_ctrl        *motor_tilt;
+                struct v4l2_ctrl        *motor_pan_reset;
+                struct v4l2_ctrl        *motor_tilt_reset;
+        };
+        /* CODEC3 models have both gain and exposure controlled by autogain */
+        struct v4l2_ctrl                *autogain_expo_cluster[3];
 };
 
-#ifdef __cplusplus
-extern "C" {
-#endif
-
 /* Global variables */
 #ifdef CONFIG_USB_PWC_DEBUG
 extern int pwc_trace;
 #endif
-extern int pwc_mbufs;
-
-/** functions in pwc-if.c */
-int pwc_handle_frame(struct pwc_device *pdev);
-void pwc_next_image(struct pwc_device *pdev);
-int pwc_isoc_init(struct pwc_device *pdev);
-void pwc_isoc_cleanup(struct pwc_device *pdev);
 
 /** Functions in pwc-misc.c */
 /* sizes in pixels */
@@ -291,50 +355,25 @@ void pwc_construct(struct pwc_device *pdev);
 /* Request a certain video mode. Returns < 0 if not possible */
 extern int pwc_set_video_mode(struct pwc_device *pdev, int width, int height, int frames, int compression, int snapshot);
 extern unsigned int pwc_get_fps(struct pwc_device *pdev, unsigned int index, unsigned int size);
-/* Calculate the number of bytes per image (not frame) */
 extern int pwc_mpt_reset(struct pwc_device *pdev, int flags);
 extern int pwc_mpt_set_angle(struct pwc_device *pdev, int pan, int tilt);
-
-/* Various controls; should be obvious. Value 0..65535, or < 0 on error */
-extern int pwc_get_brightness(struct pwc_device *pdev);
-extern int pwc_set_brightness(struct pwc_device *pdev, int value);
-extern int pwc_get_contrast(struct pwc_device *pdev);
-extern int pwc_set_contrast(struct pwc_device *pdev, int value);
-extern int pwc_get_gamma(struct pwc_device *pdev);
-extern int pwc_set_gamma(struct pwc_device *pdev, int value);
-extern int pwc_get_saturation(struct pwc_device *pdev, int *value);
-extern int pwc_set_saturation(struct pwc_device *pdev, int value);
 extern int pwc_set_leds(struct pwc_device *pdev, int on_value, int off_value);
 extern int pwc_get_cmos_sensor(struct pwc_device *pdev, int *sensor);
-extern int pwc_restore_user(struct pwc_device *pdev);
-extern int pwc_save_user(struct pwc_device *pdev);
-extern int pwc_restore_factory(struct pwc_device *pdev);
-
-/* exported for use by v4l2 controls */
-extern int pwc_get_red_gain(struct pwc_device *pdev, int *value);
-extern int pwc_set_red_gain(struct pwc_device *pdev, int value);
-extern int pwc_get_blue_gain(struct pwc_device *pdev, int *value);
-extern int pwc_set_blue_gain(struct pwc_device *pdev, int value);
-extern int pwc_get_awb(struct pwc_device *pdev);
-extern int pwc_set_awb(struct pwc_device *pdev, int mode);
-extern int pwc_set_agc(struct pwc_device *pdev, int mode, int value);
-extern int pwc_get_agc(struct pwc_device *pdev, int *value);
-extern int pwc_set_shutter_speed(struct pwc_device *pdev, int mode, int value);
-extern int pwc_get_shutter_speed(struct pwc_device *pdev, int *value);
-
-extern int pwc_set_colour_mode(struct pwc_device *pdev, int colour);
-extern int pwc_get_colour_mode(struct pwc_device *pdev, int *colour);
-extern int pwc_set_contour(struct pwc_device *pdev, int contour);
-extern int pwc_get_contour(struct pwc_device *pdev, int *contour);
-extern int pwc_set_backlight(struct pwc_device *pdev, int backlight);
-extern int pwc_get_backlight(struct pwc_device *pdev, int *backlight);
-extern int pwc_set_flicker(struct pwc_device *pdev, int flicker);
-extern int pwc_get_flicker(struct pwc_device *pdev, int *flicker);
-extern int pwc_set_dynamic_noise(struct pwc_device *pdev, int noise);
-extern int pwc_get_dynamic_noise(struct pwc_device *pdev, int *noise);
+extern int send_control_msg(struct pwc_device *pdev,
+                            u8 request, u16 value, void *buf, int buflen);
+
+/* Control get / set helpers */
+int pwc_get_u8_ctrl(struct pwc_device *pdev, u8 request, u16 value, int *data);
+int pwc_set_u8_ctrl(struct pwc_device *pdev, u8 request, u16 value, u8 data);
+int pwc_get_s8_ctrl(struct pwc_device *pdev, u8 request, u16 value, int *data);
+#define pwc_set_s8_ctrl pwc_set_u8_ctrl
+int pwc_get_u16_ctrl(struct pwc_device *pdev, u8 request, u16 value, int *dat);
+int pwc_set_u16_ctrl(struct pwc_device *pdev, u8 request, u16 value, u16 data);
+int pwc_button_ctrl(struct pwc_device *pdev, u16 value);
+int pwc_init_controls(struct pwc_device *pdev);
 
 /* Power down or up the camera; not supported by all models */
-extern int pwc_camera_power(struct pwc_device *pdev, int power);
+extern void pwc_camera_power(struct pwc_device *pdev, int power);
 
 /* Private ioctl()s; see pwc-ioctl.h */
 extern long pwc_ioctl(struct pwc_device *pdev, unsigned int cmd, void *arg);
@@ -343,12 +382,6 @@ extern const struct v4l2_ioctl_ops pwc_ioctl_ops;
 
 /** pwc-uncompress.c */
 /* Expand frame to image, possibly including decompression. Uses read_frame and fill_image */
-extern int pwc_decompress(struct pwc_device *pdev);
-
-#ifdef __cplusplus
-}
-#endif
-
+int pwc_decompress(struct pwc_device *pdev, struct pwc_frame_buf *fbuf);
 
 #endif
-/* vim: set cino= formatoptions=croql cindent shiftwidth=8 tabstop=8: */
diff --git a/drivers/media/video/pxa_camera.c b/drivers/media/video/pxa_camera.c
index b42bfa5ccdf..d07df22a5ec 100644
--- a/drivers/media/video/pxa_camera.c
+++ b/drivers/media/video/pxa_camera.c
@@ -22,7 +22,6 @@
 #include <linux/mm.h>
 #include <linux/moduleparam.h>
 #include <linux/time.h>
-#include <linux/version.h>
 #include <linux/device.h>
 #include <linux/platform_device.h>
 #include <linux/clk.h>
@@ -40,7 +39,7 @@
 #include <mach/dma.h>
 #include <mach/camera.h>
 
-#define PXA_CAM_VERSION_CODE KERNEL_VERSION(0, 0, 5)
+#define PXA_CAM_VERSION "0.0.6"
 #define PXA_CAM_DRV_NAME "pxa27x-camera"
 
 /* Camera Interface */
@@ -247,7 +246,7 @@ static int pxa_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
         if (bytes_per_line < 0)
                 return bytes_per_line;
 
-        dev_dbg(icd->dev.parent, "count=%d, size=%d\n", *count, *size);
+        dev_dbg(icd->parent, "count=%d, size=%d\n", *count, *size);
 
         *size = bytes_per_line * icd->user_height;
 
@@ -262,13 +261,13 @@ static int pxa_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 static void free_buffer(struct videobuf_queue *vq, struct pxa_buffer *buf)
 {
         struct soc_camera_device *icd = vq->priv_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
         int i;
 
         BUG_ON(in_interrupt());
 
-        dev_dbg(icd->dev.parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
+        dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
                 &buf->vb, buf->vb.baddr, buf->vb.bsize);
 
         /*
@@ -429,7 +428,7 @@ static int pxa_videobuf_prepare(struct videobuf_queue *vq,
                 struct videobuf_buffer *vb, enum v4l2_field field)
 {
         struct soc_camera_device *icd = vq->priv_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct pxa_camera_dev *pcdev = ici->priv;
         struct device *dev = pcdev->soc_host.v4l2_dev.dev;
         struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
@@ -636,11 +635,11 @@ static void pxa_videobuf_queue(struct videobuf_queue *vq,
                                struct videobuf_buffer *vb)
 {
         struct soc_camera_device *icd = vq->priv_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct pxa_camera_dev *pcdev = ici->priv;
         struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
 
-        dev_dbg(icd->dev.parent, "%s (vb=0x%p) 0x%08lx %d active=%p\n",
+        dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d active=%p\n",
                 __func__, vb, vb->baddr, vb->bsize, pcdev->active);
 
         list_add_tail(&vb->queue, &pcdev->capture);
@@ -658,7 +657,7 @@ static void pxa_videobuf_release(struct videobuf_queue *vq,
         struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
 #ifdef DEBUG
         struct soc_camera_device *icd = vq->priv_data;
-        struct device *dev = icd->dev.parent;
+        struct device *dev = icd->parent;
 
         dev_dbg(dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
                 vb, vb->baddr, vb->bsize);
@@ -843,7 +842,7 @@ static struct videobuf_queue_ops pxa_videobuf_ops = {
 static void pxa_camera_init_videobuf(struct videobuf_queue *q,
                               struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct pxa_camera_dev *pcdev = ici->priv;
 
         /*
@@ -972,7 +971,7 @@ static irqreturn_t pxa_camera_irq(int irq, void *data)
  */
 static int pxa_camera_add_device(struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct pxa_camera_dev *pcdev = ici->priv;
 
         if (pcdev->icd)
@@ -982,7 +981,7 @@ static int pxa_camera_add_device(struct soc_camera_device *icd)
 
         pcdev->icd = icd;
 
-        dev_info(icd->dev.parent, "PXA Camera driver attached to camera %d\n",
+        dev_info(icd->parent, "PXA Camera driver attached to camera %d\n",
                  icd->devnum);
 
         return 0;
@@ -991,12 +990,12 @@ static int pxa_camera_add_device(struct soc_camera_device *icd)
 /* Called with .video_lock held */
 static void pxa_camera_remove_device(struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct pxa_camera_dev *pcdev = ici->priv;
 
         BUG_ON(icd != pcdev->icd);
 
-        dev_info(icd->dev.parent, "PXA Camera driver detached from camera %d\n",
+        dev_info(icd->parent, "PXA Camera driver detached from camera %d\n",
                  icd->devnum);
 
         /* disable capture, disable interrupts */
@@ -1057,7 +1056,7 @@ static int test_platform_param(struct pxa_camera_dev *pcdev,
 static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
                                   unsigned long flags, __u32 pixfmt)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct pxa_camera_dev *pcdev = ici->priv;
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
         unsigned long dw, bpp;
@@ -1152,7 +1151,7 @@ static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
 
 static int pxa_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct pxa_camera_dev *pcdev = ici->priv;
         unsigned long bus_flags, camera_flags, common_flags;
         int ret;
@@ -1210,7 +1209,7 @@ static int pxa_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
 static int pxa_camera_try_bus_param(struct soc_camera_device *icd,
                                     unsigned char buswidth)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct pxa_camera_dev *pcdev = ici->priv;
         unsigned long bus_flags, camera_flags;
         int ret = test_platform_param(pcdev, buswidth, &bus_flags);
@@ -1247,7 +1246,7 @@ static int pxa_camera_get_formats(struct soc_camera_device *icd, unsigned int id
                                   struct soc_camera_format_xlate *xlate)
 {
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        struct device *dev = icd->dev.parent;
+        struct device *dev = icd->parent;
         int formats = 0, ret;
         struct pxa_cam *cam;
         enum v4l2_mbus_pixelcode code;
@@ -1335,9 +1334,9 @@ static int pxa_camera_set_crop(struct soc_camera_device *icd,
                                struct v4l2_crop *a)
 {
         struct v4l2_rect *rect = &a->c;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct device *dev = icd->parent;
+        struct soc_camera_host *ici = to_soc_camera_host(dev);
         struct pxa_camera_dev *pcdev = ici->priv;
-        struct device *dev = icd->dev.parent;
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
         struct soc_camera_sense sense = {
                 .master_clock = pcdev->mclk,
@@ -1379,7 +1378,7 @@ static int pxa_camera_set_crop(struct soc_camera_device *icd,
                         return ret;
 
                 if (pxa_camera_check_frame(mf.width, mf.height)) {
-                        dev_warn(icd->dev.parent,
+                        dev_warn(icd->parent,
                                  "Inconsistent state. Use S_FMT to repair\n");
                         return -EINVAL;
                 }
@@ -1406,9 +1405,9 @@ static int pxa_camera_set_crop(struct soc_camera_device *icd,
 static int pxa_camera_set_fmt(struct soc_camera_device *icd,
                               struct v4l2_format *f)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct device *dev = icd->parent;
+        struct soc_camera_host *ici = to_soc_camera_host(dev);
         struct pxa_camera_dev *pcdev = ici->priv;
-        struct device *dev = icd->dev.parent;
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
         const struct soc_camera_format_xlate *xlate = NULL;
         struct soc_camera_sense sense = {
@@ -1485,7 +1484,7 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
 
         xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
         if (!xlate) {
-                dev_warn(icd->dev.parent, "Format %x not found\n", pixfmt);
+                dev_warn(icd->parent, "Format %x not found\n", pixfmt);
                 return -EINVAL;
         }
 
@@ -1499,16 +1498,11 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
                               &pix->height, 32, 2048, 0,
                               pixfmt == V4L2_PIX_FMT_YUV422P ? 4 : 0);
 
-        pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
-                                                    xlate->host_fmt);
-        if (pix->bytesperline < 0)
-                return pix->bytesperline;
-        pix->sizeimage = pix->height * pix->bytesperline;
-
         /* limit to sensor capabilities */
         mf.width        = pix->width;
         mf.height       = pix->height;
-        mf.field        = pix->field;
+        /* Only progressive video supported so far */
+        mf.field        = V4L2_FIELD_NONE;
         mf.colorspace   = pix->colorspace;
         mf.code         = xlate->code;
 
@@ -1527,7 +1521,7 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
                 break;
         default:
                 /* TODO: support interlaced at least in pass-through mode */
-                dev_err(icd->dev.parent, "Field type %d unsupported.\n",
+                dev_err(icd->parent, "Field type %d unsupported.\n",
                         mf.field);
                 return -EINVAL;
         }
@@ -1578,15 +1572,14 @@ static int pxa_camera_querycap(struct soc_camera_host *ici,
 {
         /* cap->name is set by the firendly caller:-> */
         strlcpy(cap->card, pxa_cam_driver_description, sizeof(cap->card));
-        cap->version = PXA_CAM_VERSION_CODE;
         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 
         return 0;
 }
 
-static int pxa_camera_suspend(struct soc_camera_device *icd, pm_message_t state)
+static int pxa_camera_suspend(struct device *dev)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(dev);
         struct pxa_camera_dev *pcdev = ici->priv;
         int i = 0, ret = 0;
 
@@ -1596,15 +1589,19 @@ static int pxa_camera_suspend(struct soc_camera_device *icd, pm_message_t state)
         pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR3);
         pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR4);
 
-        if ((pcdev->icd) && (pcdev->icd->ops->suspend))
-                ret = pcdev->icd->ops->suspend(pcdev->icd, state);
+        if (pcdev->icd) {
+                struct v4l2_subdev *sd = soc_camera_to_subdev(pcdev->icd);
+                ret = v4l2_subdev_call(sd, core, s_power, 0);
+                if (ret == -ENOIOCTLCMD)
+                        ret = 0;
+        }
 
         return ret;
 }
 
-static int pxa_camera_resume(struct soc_camera_device *icd)
+static int pxa_camera_resume(struct device *dev)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(dev);
         struct pxa_camera_dev *pcdev = ici->priv;
         int i = 0, ret = 0;
 
@@ -1618,8 +1615,12 @@ static int pxa_camera_resume(struct soc_camera_device *icd)
         __raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR3);
         __raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR4);
 
-        if ((pcdev->icd) && (pcdev->icd->ops->resume))
-                ret = pcdev->icd->ops->resume(pcdev->icd);
+        if (pcdev->icd) {
+                struct v4l2_subdev *sd = soc_camera_to_subdev(pcdev->icd);
+                ret = v4l2_subdev_call(sd, core, s_power, 1);
+                if (ret == -ENOIOCTLCMD)
+                        ret = 0;
+        }
 
         /* Restart frame capture if active buffer exists */
         if (!ret && pcdev->active)
@@ -1632,8 +1633,6 @@ static struct soc_camera_host_ops pxa_soc_camera_host_ops = {
         .owner          = THIS_MODULE,
         .add            = pxa_camera_add_device,
         .remove         = pxa_camera_remove_device,
-        .suspend        = pxa_camera_suspend,
-        .resume         = pxa_camera_resume,
         .set_crop       = pxa_camera_set_crop,
         .get_formats    = pxa_camera_get_formats,
         .put_formats    = pxa_camera_put_formats,
@@ -1818,9 +1817,15 @@ static int __devexit pxa_camera_remove(struct platform_device *pdev)
         return 0;
 }
 
+static struct dev_pm_ops pxa_camera_pm = {
+        .suspend        = pxa_camera_suspend,
+        .resume         = pxa_camera_resume,
+};
+
 static struct platform_driver pxa_camera_driver = {
         .driver         = {
                 .name   = PXA_CAM_DRV_NAME,
+                .pm     = &pxa_camera_pm,
         },
         .probe          = pxa_camera_probe,
         .remove         = __devexit_p(pxa_camera_remove),
@@ -1843,4 +1848,5 @@ module_exit(pxa_camera_exit);
 MODULE_DESCRIPTION("PXA27x SoC Camera Host driver");
 MODULE_AUTHOR("Guennadi Liakhovetski <kernel@pengutronix.de>");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(PXA_CAM_VERSION);
 MODULE_ALIAS("platform:" PXA_CAM_DRV_NAME);
diff --git a/drivers/media/video/rj54n1cb0c.c b/drivers/media/video/rj54n1cb0c.c
index 57e11b6f19f..847ccc067e8 100644
--- a/drivers/media/video/rj54n1cb0c.c
+++ b/drivers/media/video/rj54n1cb0c.c
@@ -1364,10 +1364,9 @@ static int rj54n1_video_probe(struct soc_camera_device *icd,
         int data1, data2;
         int ret;
 
-        /* This could be a BUG_ON() or a WARN_ON(), or remove it completely */
-        if (!icd->dev.parent ||
-            to_soc_camera_host(icd->dev.parent)->nr != icd->iface)
-                return -ENODEV;
+        /* We must have a parent by now. And it cannot be a wrong one. */
+        BUG_ON(!icd->parent ||
+               to_soc_camera_host(icd->parent)->nr != icd->iface);
 
         /* Read out the chip version register */
         data1 = reg_read(client, RJ54N1_DEV_CODE);
diff --git a/drivers/media/video/s2255drv.c b/drivers/media/video/s2255drv.c
index 5b9dce85645..803c9c82e49 100644
--- a/drivers/media/video/s2255drv.c
+++ b/drivers/media/video/s2255drv.c
@@ -16,15 +16,10 @@
  * Example maximum bandwidth utilization:
  *
  * -full size, color mode YUYV or YUV422P: 2 channels at once
- *
  * -full or half size Grey scale: all 4 channels at once
- *
  * -half size, color mode YUYV or YUV422P: all 4 channels at once
- *
  * -full size, color mode YUYV or YUV422P 1/2 frame rate: all 4 channels
  *  at once.
- *  (TODO: Incorporate videodev2 frame rate(FR) enumeration,
- *  which is currently experimental.)
  *
  * 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
@@ -47,7 +42,6 @@
 #include <linux/mutex.h>
 #include <linux/slab.h>
 #include <linux/videodev2.h>
-#include <linux/version.h>
 #include <linux/mm.h>
 #include <media/videobuf-vmalloc.h>
 #include <media/v4l2-common.h>
@@ -56,12 +50,7 @@
 #include <linux/vmalloc.h>
 #include <linux/usb.h>
 
-#define S2255_MAJOR_VERSION     1
-#define S2255_MINOR_VERSION     21
-#define S2255_RELEASE           0
-#define S2255_VERSION           KERNEL_VERSION(S2255_MAJOR_VERSION, \
-                                               S2255_MINOR_VERSION, \
-                                               S2255_RELEASE)
+#define S2255_VERSION           "1.22.1"
 #define FIRMWARE_FILE_NAME "f2255usb.bin"
 
 /* default JPEG quality */
@@ -126,7 +115,7 @@
 #define MASK_COLOR       0x000000ff
 #define MASK_JPG_QUALITY 0x0000ff00
 #define MASK_INPUT_TYPE  0x000f0000
-/* frame decimation. Not implemented by V4L yet(experimental in V4L) */
+/* frame decimation. */
 #define FDEC_1          1       /* capture every frame. default */
 #define FDEC_2          2       /* capture every 2nd frame */
 #define FDEC_3          3       /* capture every 3rd frame */
@@ -312,9 +301,9 @@ struct s2255_fh {
 };
 
 /* current cypress EEPROM firmware version */
-#define S2255_CUR_USB_FWVER     ((3 << 8) | 11)
+#define S2255_CUR_USB_FWVER     ((3 << 8) | 12)
 /* current DSP FW version */
-#define S2255_CUR_DSP_FWVER     10102
+#define S2255_CUR_DSP_FWVER     10104
 /* Need DSP version 5+ for video status feature */
 #define S2255_MIN_DSP_STATUS      5
 #define S2255_MIN_DSP_COLORFILTER 8
@@ -502,7 +491,7 @@ static void planar422p_to_yuv_packed(const unsigned char *in,
 
 static void s2255_reset_dsppower(struct s2255_dev *dev)
 {
-        s2255_vendor_req(dev, 0x40, 0x0b0b, 0x0b01, NULL, 0, 1);
+        s2255_vendor_req(dev, 0x40, 0x0000, 0x0001, NULL, 0, 1);
         msleep(10);
         s2255_vendor_req(dev, 0x50, 0x0000, 0x0000, NULL, 0, 1);
         msleep(600);
@@ -856,7 +845,6 @@ static int vidioc_querycap(struct file *file, void *priv,
         strlcpy(cap->driver, "s2255", sizeof(cap->driver));
         strlcpy(cap->card, "s2255", sizeof(cap->card));
         usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
-        cap->version = S2255_VERSION;
         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
         return 0;
 }
@@ -1984,9 +1972,8 @@ static int s2255_probe_v4l(struct s2255_dev *dev)
                           video_device_node_name(&channel->vdev));
 
         }
-        printk(KERN_INFO "Sensoray 2255 V4L driver Revision: %d.%d\n",
-               S2255_MAJOR_VERSION,
-               S2255_MINOR_VERSION);
+        printk(KERN_INFO "Sensoray 2255 V4L driver Revision: %s\n",
+               S2255_VERSION);
         /* if no channels registered, return error and probe will fail*/
         if (atomic_read(&dev->num_channels) == 0) {
                 v4l2_device_unregister(&dev->v4l2_dev);
@@ -2302,15 +2289,12 @@ static int s2255_board_init(struct s2255_dev *dev)
         /* query the firmware */
         fw_ver = s2255_get_fx2fw(dev);
 
-        printk(KERN_INFO "2255 usb firmware version %d.%d\n",
+        printk(KERN_INFO "s2255: usb firmware version %d.%d\n",
                (fw_ver >> 8) & 0xff,
                fw_ver & 0xff);
 
         if (fw_ver < S2255_CUR_USB_FWVER)
-                dev_err(&dev->udev->dev,
-                        "usb firmware not up to date %d.%d\n",
-                        (fw_ver >> 8) & 0xff,
-                        fw_ver & 0xff);
+                printk(KERN_INFO "s2255: newer USB firmware available\n");
 
         for (j = 0; j < MAX_CHANNELS; j++) {
                 struct s2255_channel *channel = &dev->channel[j];
@@ -2721,3 +2705,4 @@ module_exit(usb_s2255_exit);
 MODULE_DESCRIPTION("Sensoray 2255 Video for Linux driver");
 MODULE_AUTHOR("Dean Anderson (Sensoray Company Inc.)");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(S2255_VERSION);
diff --git a/drivers/media/video/s5p-fimc/fimc-capture.c b/drivers/media/video/s5p-fimc/fimc-capture.c
index 81b4a826ee5..0d730e55605 100644
--- a/drivers/media/video/s5p-fimc/fimc-capture.c
+++ b/drivers/media/video/s5p-fimc/fimc-capture.c
@@ -11,7 +11,6 @@
 
 #include <linux/module.h>
 #include <linux/kernel.h>
-#include <linux/version.h>
 #include <linux/types.h>
 #include <linux/errno.h>
 #include <linux/bug.h>
@@ -451,7 +450,6 @@ static int fimc_vidioc_querycap_capture(struct file *file, void *priv,
         strncpy(cap->driver, fimc->pdev->name, sizeof(cap->driver) - 1);
         strncpy(cap->card, fimc->pdev->name, sizeof(cap->card) - 1);
         cap->bus_info[0] = 0;
-        cap->version = KERNEL_VERSION(1, 0, 0);
         cap->capabilities = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE |
                             V4L2_CAP_VIDEO_CAPTURE_MPLANE;
 
diff --git a/drivers/media/video/s5p-fimc/fimc-core.c b/drivers/media/video/s5p-fimc/fimc-core.c
index bdf19ada917..aa550666cc0 100644
--- a/drivers/media/video/s5p-fimc/fimc-core.c
+++ b/drivers/media/video/s5p-fimc/fimc-core.c
@@ -12,7 +12,6 @@
 
 #include <linux/module.h>
 #include <linux/kernel.h>
-#include <linux/version.h>
 #include <linux/types.h>
 #include <linux/errno.h>
 #include <linux/bug.h>
@@ -774,7 +773,6 @@ static int fimc_m2m_querycap(struct file *file, void *priv,
         strncpy(cap->driver, fimc->pdev->name, sizeof(cap->driver) - 1);
         strncpy(cap->card, fimc->pdev->name, sizeof(cap->card) - 1);
         cap->bus_info[0] = 0;
-        cap->version = KERNEL_VERSION(1, 0, 0);
         cap->capabilities = V4L2_CAP_STREAMING |
                 V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT |
                 V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
@@ -1937,3 +1935,4 @@ module_exit(fimc_exit);
 MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
 MODULE_DESCRIPTION("S5P FIMC camera host interface/video postprocessor driver");
 MODULE_LICENSE("GPL");
+MODULE_VERSION("1.0.1");
diff --git a/drivers/media/video/s5p-mfc/Makefile b/drivers/media/video/s5p-mfc/Makefile
new file mode 100644
index 00000000000..d0663409af0
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/Makefile
@@ -0,0 +1,5 @@
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_MFC) := s5p-mfc.o
+s5p-mfc-y += s5p_mfc.o s5p_mfc_intr.o s5p_mfc_opr.o
+s5p-mfc-y += s5p_mfc_dec.o s5p_mfc_enc.o
+s5p-mfc-y += s5p_mfc_ctrl.o s5p_mfc_cmd.o
+s5p-mfc-y += s5p_mfc_pm.o s5p_mfc_shm.o
diff --git a/drivers/media/video/s5p-mfc/regs-mfc.h b/drivers/media/video/s5p-mfc/regs-mfc.h
new file mode 100644
index 00000000000..053a8a872fd
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/regs-mfc.h
@@ -0,0 +1,413 @@
+/*
+ * Register definition file for Samsung MFC V5.1 Interface (FIMV) driver
+ *
+ * Kamil Debski, Copyright (c) 2010 Samsung Electronics
+ * http://www.samsung.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.
+*/
+
+#ifndef _REGS_FIMV_H
+#define _REGS_FIMV_H
+
+#define S5P_FIMV_REG_SIZE       (S5P_FIMV_END_ADDR - S5P_FIMV_START_ADDR)
+#define S5P_FIMV_REG_COUNT      ((S5P_FIMV_END_ADDR - S5P_FIMV_START_ADDR) / 4)
+
+/* Number of bits that the buffer address should be shifted for particular
+ * MFC buffers.  */
+#define S5P_FIMV_START_ADDR             0x0000
+#define S5P_FIMV_END_ADDR               0xe008
+
+#define S5P_FIMV_SW_RESET               0x0000
+#define S5P_FIMV_RISC_HOST_INT          0x0008
+
+/* Command from HOST to RISC */
+#define S5P_FIMV_HOST2RISC_CMD          0x0030
+#define S5P_FIMV_HOST2RISC_ARG1         0x0034
+#define S5P_FIMV_HOST2RISC_ARG2         0x0038
+#define S5P_FIMV_HOST2RISC_ARG3         0x003c
+#define S5P_FIMV_HOST2RISC_ARG4         0x0040
+
+/* Command from RISC to HOST */
+#define S5P_FIMV_RISC2HOST_CMD          0x0044
+#define S5P_FIMV_RISC2HOST_CMD_MASK     0x1FFFF
+#define S5P_FIMV_RISC2HOST_ARG1         0x0048
+#define S5P_FIMV_RISC2HOST_ARG2         0x004c
+#define S5P_FIMV_RISC2HOST_ARG3         0x0050
+#define S5P_FIMV_RISC2HOST_ARG4         0x0054
+
+#define S5P_FIMV_FW_VERSION             0x0058
+#define S5P_FIMV_SYS_MEM_SZ             0x005c
+#define S5P_FIMV_FW_STATUS              0x0080
+
+/* Memory controller register */
+#define S5P_FIMV_MC_DRAMBASE_ADR_A      0x0508
+#define S5P_FIMV_MC_DRAMBASE_ADR_B      0x050c
+#define S5P_FIMV_MC_STATUS              0x0510
+
+/* Common register */
+#define S5P_FIMV_COMMON_BASE_A          0x0600
+#define S5P_FIMV_COMMON_BASE_B          0x0700
+
+/* Decoder */
+#define S5P_FIMV_DEC_CHROMA_ADR         (S5P_FIMV_COMMON_BASE_A)
+#define S5P_FIMV_DEC_LUMA_ADR           (S5P_FIMV_COMMON_BASE_B)
+
+/* H.264 decoding */
+#define S5P_FIMV_H264_VERT_NB_MV_ADR    (S5P_FIMV_COMMON_BASE_A + 0x8c)
+                                        /* vertical neighbor motion vector */
+#define S5P_FIMV_H264_NB_IP_ADR         (S5P_FIMV_COMMON_BASE_A + 0x90)
+                                        /* neighbor pixels for intra pred */
+#define S5P_FIMV_H264_MV_ADR            (S5P_FIMV_COMMON_BASE_B + 0x80)
+                                        /* H264 motion vector */
+
+/* MPEG4 decoding */
+#define S5P_FIMV_MPEG4_NB_DCAC_ADR      (S5P_FIMV_COMMON_BASE_A + 0x8c)
+                                        /* neighbor AC/DC coeff. */
+#define S5P_FIMV_MPEG4_UP_NB_MV_ADR     (S5P_FIMV_COMMON_BASE_A + 0x90)
+                                        /* upper neighbor motion vector */
+#define S5P_FIMV_MPEG4_SA_MV_ADR        (S5P_FIMV_COMMON_BASE_A + 0x94)
+                                        /* subseq. anchor motion vector */
+#define S5P_FIMV_MPEG4_OT_LINE_ADR      (S5P_FIMV_COMMON_BASE_A + 0x98)
+                                        /* overlap transform line */
+#define S5P_FIMV_MPEG4_SP_ADR           (S5P_FIMV_COMMON_BASE_A + 0xa8)
+                                        /* syntax parser */
+
+/* H.263 decoding */
+#define S5P_FIMV_H263_NB_DCAC_ADR       (S5P_FIMV_COMMON_BASE_A + 0x8c)
+#define S5P_FIMV_H263_UP_NB_MV_ADR      (S5P_FIMV_COMMON_BASE_A + 0x90)
+#define S5P_FIMV_H263_SA_MV_ADR         (S5P_FIMV_COMMON_BASE_A + 0x94)
+#define S5P_FIMV_H263_OT_LINE_ADR       (S5P_FIMV_COMMON_BASE_A + 0x98)
+
+/* VC-1 decoding */
+#define S5P_FIMV_VC1_NB_DCAC_ADR        (S5P_FIMV_COMMON_BASE_A + 0x8c)
+#define S5P_FIMV_VC1_UP_NB_MV_ADR       (S5P_FIMV_COMMON_BASE_A + 0x90)
+#define S5P_FIMV_VC1_SA_MV_ADR          (S5P_FIMV_COMMON_BASE_A + 0x94)
+#define S5P_FIMV_VC1_OT_LINE_ADR        (S5P_FIMV_COMMON_BASE_A + 0x98)
+#define S5P_FIMV_VC1_BITPLANE3_ADR      (S5P_FIMV_COMMON_BASE_A + 0x9c)
+                                        /* bitplane3 */
+#define S5P_FIMV_VC1_BITPLANE2_ADR      (S5P_FIMV_COMMON_BASE_A + 0xa0)
+                                        /* bitplane2 */
+#define S5P_FIMV_VC1_BITPLANE1_ADR      (S5P_FIMV_COMMON_BASE_A + 0xa4)
+                                        /* bitplane1 */
+
+/* Encoder */
+#define S5P_FIMV_ENC_REF0_LUMA_ADR      (S5P_FIMV_COMMON_BASE_A + 0x1c)
+#define S5P_FIMV_ENC_REF1_LUMA_ADR      (S5P_FIMV_COMMON_BASE_A + 0x20)
+                                        /* reconstructed luma */
+#define S5P_FIMV_ENC_REF0_CHROMA_ADR    (S5P_FIMV_COMMON_BASE_B)
+#define S5P_FIMV_ENC_REF1_CHROMA_ADR    (S5P_FIMV_COMMON_BASE_B + 0x04)
+                                        /* reconstructed chroma */
+#define S5P_FIMV_ENC_REF2_LUMA_ADR      (S5P_FIMV_COMMON_BASE_B + 0x10)
+#define S5P_FIMV_ENC_REF2_CHROMA_ADR    (S5P_FIMV_COMMON_BASE_B + 0x08)
+#define S5P_FIMV_ENC_REF3_LUMA_ADR      (S5P_FIMV_COMMON_BASE_B + 0x14)
+#define S5P_FIMV_ENC_REF3_CHROMA_ADR    (S5P_FIMV_COMMON_BASE_B + 0x0c)
+
+/* H.264 encoding */
+#define S5P_FIMV_H264_UP_MV_ADR         (S5P_FIMV_COMMON_BASE_A)
+                                        /* upper motion vector */
+#define S5P_FIMV_H264_NBOR_INFO_ADR     (S5P_FIMV_COMMON_BASE_A + 0x04)
+                                        /* entropy engine's neighbor info. */
+#define S5P_FIMV_H264_UP_INTRA_MD_ADR   (S5P_FIMV_COMMON_BASE_A + 0x08)
+                                        /* upper intra MD */
+#define S5P_FIMV_H264_COZERO_FLAG_ADR   (S5P_FIMV_COMMON_BASE_A + 0x10)
+                                        /* direct cozero flag */
+#define S5P_FIMV_H264_UP_INTRA_PRED_ADR (S5P_FIMV_COMMON_BASE_B + 0x40)
+                                        /* upper intra PRED */
+
+/* H.263 encoding */
+#define S5P_FIMV_H263_UP_MV_ADR         (S5P_FIMV_COMMON_BASE_A)
+                                        /* upper motion vector */
+#define S5P_FIMV_H263_ACDC_COEF_ADR     (S5P_FIMV_COMMON_BASE_A + 0x04)
+                                        /* upper Q coeff. */
+
+/* MPEG4 encoding */
+#define S5P_FIMV_MPEG4_UP_MV_ADR        (S5P_FIMV_COMMON_BASE_A)
+                                        /* upper motion vector */
+#define S5P_FIMV_MPEG4_ACDC_COEF_ADR    (S5P_FIMV_COMMON_BASE_A + 0x04)
+                                        /* upper Q coeff. */
+#define S5P_FIMV_MPEG4_COZERO_FLAG_ADR  (S5P_FIMV_COMMON_BASE_A + 0x10)
+                                        /* direct cozero flag */
+
+#define S5P_FIMV_ENC_REF_B_LUMA_ADR     0x062c /* ref B Luma addr */
+#define S5P_FIMV_ENC_REF_B_CHROMA_ADR   0x0630 /* ref B Chroma addr */
+
+#define S5P_FIMV_ENC_CUR_LUMA_ADR       0x0718 /* current Luma addr */
+#define S5P_FIMV_ENC_CUR_CHROMA_ADR     0x071C /* current Chroma addr */
+
+/* Codec common register */
+#define S5P_FIMV_ENC_HSIZE_PX           0x0818 /* frame width at encoder */
+#define S5P_FIMV_ENC_VSIZE_PX           0x081c /* frame height at encoder */
+#define S5P_FIMV_ENC_PROFILE            0x0830 /* profile register */
+#define S5P_FIMV_ENC_PROFILE_H264_MAIN                  0
+#define S5P_FIMV_ENC_PROFILE_H264_HIGH                  1
+#define S5P_FIMV_ENC_PROFILE_H264_BASELINE              2
+#define S5P_FIMV_ENC_PROFILE_MPEG4_SIMPLE               0
+#define S5P_FIMV_ENC_PROFILE_MPEG4_ADVANCED_SIMPLE      1
+#define S5P_FIMV_ENC_PIC_STRUCT         0x083c /* picture field/frame flag */
+#define S5P_FIMV_ENC_LF_CTRL            0x0848 /* loop filter control */
+#define S5P_FIMV_ENC_ALPHA_OFF          0x084c /* loop filter alpha offset */
+#define S5P_FIMV_ENC_BETA_OFF           0x0850 /* loop filter beta offset */
+#define S5P_FIMV_MR_BUSIF_CTRL          0x0854 /* hidden, bus interface ctrl */
+#define S5P_FIMV_ENC_PXL_CACHE_CTRL     0x0a00 /* pixel cache control */
+
+/* Channel & stream interface register */
+#define S5P_FIMV_SI_RTN_CHID            0x2000 /* Return CH inst ID register */
+#define S5P_FIMV_SI_CH0_INST_ID         0x2040 /* codec instance ID */
+#define S5P_FIMV_SI_CH1_INST_ID         0x2080 /* codec instance ID */
+/* Decoder */
+#define S5P_FIMV_SI_VRESOL              0x2004 /* vertical res of decoder */
+#define S5P_FIMV_SI_HRESOL              0x2008 /* horizontal res of decoder */
+#define S5P_FIMV_SI_BUF_NUMBER          0x200c /* number of frames in the
+                                                                decoded pic */
+#define S5P_FIMV_SI_DISPLAY_Y_ADR       0x2010 /* luma addr of displayed pic */
+#define S5P_FIMV_SI_DISPLAY_C_ADR       0x2014 /* chroma addrof displayed pic */
+#define S5P_FIMV_SI_CONSUMED_BYTES      0x2018 /* Consumed number of bytes to
+                                                        decode a frame */
+#define S5P_FIMV_SI_DISPLAY_STATUS      0x201c /* status of decoded picture */
+
+#define S5P_FIMV_SI_CH0_SB_ST_ADR       0x2044 /* start addr of stream buf */
+#define S5P_FIMV_SI_CH0_SB_FRM_SIZE     0x2048 /* size of stream buf */
+#define S5P_FIMV_SI_CH0_DESC_ADR        0x204c /* addr of descriptor buf */
+#define S5P_FIMV_SI_CH0_CPB_SIZE        0x2058 /* max size of coded pic. buf */
+#define S5P_FIMV_SI_CH0_DESC_SIZE       0x205c /* max size of descriptor buf */
+
+#define S5P_FIMV_SI_CH1_SB_ST_ADR       0x2084 /* start addr of stream buf */
+#define S5P_FIMV_SI_CH1_SB_FRM_SIZE     0x2088 /* size of stream buf */
+#define S5P_FIMV_SI_CH1_DESC_ADR        0x208c /* addr of descriptor buf */
+#define S5P_FIMV_SI_CH1_CPB_SIZE        0x2098 /* max size of coded pic. buf */
+#define S5P_FIMV_SI_CH1_DESC_SIZE       0x209c /* max size of descriptor buf */
+
+#define S5P_FIMV_CRC_LUMA0              0x2030 /* luma crc data per frame
+                                                                (top field) */
+#define S5P_FIMV_CRC_CHROMA0            0x2034 /* chroma crc data per frame
+                                                                (top field) */
+#define S5P_FIMV_CRC_LUMA1              0x2038 /* luma crc data per bottom
+                                                                field */
+#define S5P_FIMV_CRC_CHROMA1            0x203c /* chroma crc data per bottom
+                                                                field */
+
+/* Display status */
+#define S5P_FIMV_DEC_STATUS_DECODING_ONLY               0
+#define S5P_FIMV_DEC_STATUS_DECODING_DISPLAY            1
+#define S5P_FIMV_DEC_STATUS_DISPLAY_ONLY                2
+#define S5P_FIMV_DEC_STATUS_DECODING_EMPTY              3
+#define S5P_FIMV_DEC_STATUS_DECODING_STATUS_MASK        7
+#define S5P_FIMV_DEC_STATUS_PROGRESSIVE                 (0<<3)
+#define S5P_FIMV_DEC_STATUS_INTERLACE                   (1<<3)
+#define S5P_FIMV_DEC_STATUS_INTERLACE_MASK              (1<<3)
+#define S5P_FIMV_DEC_STATUS_CRC_NUMBER_TWO              (0<<4)
+#define S5P_FIMV_DEC_STATUS_CRC_NUMBER_FOUR             (1<<4)
+#define S5P_FIMV_DEC_STATUS_CRC_NUMBER_MASK             (1<<4)
+#define S5P_FIMV_DEC_STATUS_CRC_GENERATED               (1<<5)
+#define S5P_FIMV_DEC_STATUS_CRC_NOT_GENERATED           (0<<5)
+#define S5P_FIMV_DEC_STATUS_CRC_MASK                    (1<<5)
+
+#define S5P_FIMV_DEC_STATUS_RESOLUTION_MASK             (3<<4)
+#define S5P_FIMV_DEC_STATUS_RESOLUTION_INC              (1<<4)
+#define S5P_FIMV_DEC_STATUS_RESOLUTION_DEC              (2<<4)
+
+/* Decode frame address */
+#define S5P_FIMV_DECODE_Y_ADR                   0x2024
+#define S5P_FIMV_DECODE_C_ADR                   0x2028
+
+/* Decoded frame tpe */
+#define S5P_FIMV_DECODE_FRAME_TYPE              0x2020
+#define S5P_FIMV_DECODE_FRAME_MASK              7
+
+#define S5P_FIMV_DECODE_FRAME_SKIPPED           0
+#define S5P_FIMV_DECODE_FRAME_I_FRAME           1
+#define S5P_FIMV_DECODE_FRAME_P_FRAME           2
+#define S5P_FIMV_DECODE_FRAME_B_FRAME           3
+#define S5P_FIMV_DECODE_FRAME_OTHER_FRAME       4
+
+/* Sizes of buffers required for decoding */
+#define S5P_FIMV_DEC_NB_IP_SIZE                 (32 * 1024)
+#define S5P_FIMV_DEC_VERT_NB_MV_SIZE            (16 * 1024)
+#define S5P_FIMV_DEC_NB_DCAC_SIZE               (16 * 1024)
+#define S5P_FIMV_DEC_UPNB_MV_SIZE               (68 * 1024)
+#define S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE         (136 * 1024)
+#define S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE     (32 * 1024)
+#define S5P_FIMV_DEC_VC1_BITPLANE_SIZE          (2 * 1024)
+#define S5P_FIMV_DEC_STX_PARSER_SIZE            (68 * 1024)
+
+#define S5P_FIMV_DEC_BUF_ALIGN                  (8 * 1024)
+#define S5P_FIMV_ENC_BUF_ALIGN                  (8 * 1024)
+#define S5P_FIMV_NV12M_HALIGN                   16
+#define S5P_FIMV_NV12M_LVALIGN                  16
+#define S5P_FIMV_NV12M_CVALIGN                  8
+#define S5P_FIMV_NV12MT_HALIGN                  128
+#define S5P_FIMV_NV12MT_VALIGN                  32
+#define S5P_FIMV_NV12M_SALIGN                   2048
+#define S5P_FIMV_NV12MT_SALIGN                  8192
+
+/* Sizes of buffers required for encoding */
+#define S5P_FIMV_ENC_UPMV_SIZE          0x10000
+#define S5P_FIMV_ENC_COLFLG_SIZE        0x10000
+#define S5P_FIMV_ENC_INTRAMD_SIZE       0x10000
+#define S5P_FIMV_ENC_INTRAPRED_SIZE     0x4000
+#define S5P_FIMV_ENC_NBORINFO_SIZE      0x10000
+#define S5P_FIMV_ENC_ACDCCOEF_SIZE      0x10000
+
+/* Encoder */
+#define S5P_FIMV_ENC_SI_STRM_SIZE       0x2004 /* stream size */
+#define S5P_FIMV_ENC_SI_PIC_CNT         0x2008 /* picture count */
+#define S5P_FIMV_ENC_SI_WRITE_PTR       0x200c /* write pointer */
+#define S5P_FIMV_ENC_SI_SLICE_TYPE      0x2010 /* slice type(I/P/B/IDR) */
+#define S5P_FIMV_ENC_SI_SLICE_TYPE_NON_CODED    0
+#define S5P_FIMV_ENC_SI_SLICE_TYPE_I            1
+#define S5P_FIMV_ENC_SI_SLICE_TYPE_P            2
+#define S5P_FIMV_ENC_SI_SLICE_TYPE_B            3
+#define S5P_FIMV_ENC_SI_SLICE_TYPE_SKIPPED      4
+#define S5P_FIMV_ENC_SI_SLICE_TYPE_OTHERS       5
+#define S5P_FIMV_ENCODED_Y_ADDR         0x2014 /* the addr of the encoded
+                                                                luma pic */
+#define S5P_FIMV_ENCODED_C_ADDR         0x2018 /* the addr of the encoded
+                                                                chroma pic */
+
+#define S5P_FIMV_ENC_SI_CH0_SB_ADR      0x2044 /* addr of stream buf */
+#define S5P_FIMV_ENC_SI_CH0_SB_SIZE     0x204c /* size of stream buf */
+#define S5P_FIMV_ENC_SI_CH0_CUR_Y_ADR   0x2050 /* current Luma addr */
+#define S5P_FIMV_ENC_SI_CH0_CUR_C_ADR   0x2054 /* current Chroma addr */
+#define S5P_FIMV_ENC_SI_CH0_FRAME_INS   0x2058 /* frame insertion */
+
+#define S5P_FIMV_ENC_SI_CH1_SB_ADR      0x2084 /* addr of stream buf */
+#define S5P_FIMV_ENC_SI_CH1_SB_SIZE     0x208c /* size of stream buf */
+#define S5P_FIMV_ENC_SI_CH1_CUR_Y_ADR   0x2090 /* current Luma addr */
+#define S5P_FIMV_ENC_SI_CH1_CUR_C_ADR   0x2094 /* current Chroma addr */
+#define S5P_FIMV_ENC_SI_CH1_FRAME_INS   0x2098 /* frame insertion */
+
+#define S5P_FIMV_ENC_PIC_TYPE_CTRL      0xc504 /* pic type level control */
+#define S5P_FIMV_ENC_B_RECON_WRITE_ON   0xc508 /* B frame recon write ctrl */
+#define S5P_FIMV_ENC_MSLICE_CTRL        0xc50c /* multi slice control */
+#define S5P_FIMV_ENC_MSLICE_MB          0xc510 /* MB number in the one slice */
+#define S5P_FIMV_ENC_MSLICE_BIT         0xc514 /* bit count for one slice */
+#define S5P_FIMV_ENC_CIR_CTRL           0xc518 /* number of intra refresh MB */
+#define S5P_FIMV_ENC_MAP_FOR_CUR        0xc51c /* linear or tiled mode */
+#define S5P_FIMV_ENC_PADDING_CTRL       0xc520 /* padding control */
+
+#define S5P_FIMV_ENC_RC_CONFIG          0xc5a0 /* RC config */
+#define S5P_FIMV_ENC_RC_BIT_RATE        0xc5a8 /* bit rate */
+#define S5P_FIMV_ENC_RC_QBOUND          0xc5ac /* max/min QP */
+#define S5P_FIMV_ENC_RC_RPARA           0xc5b0 /* rate control reaction coeff */
+#define S5P_FIMV_ENC_RC_MB_CTRL         0xc5b4 /* MB adaptive scaling */
+
+/* Encoder for H264 only */
+#define S5P_FIMV_ENC_H264_ENTROPY_MODE  0xd004 /* CAVLC or CABAC */
+#define S5P_FIMV_ENC_H264_ALPHA_OFF     0xd008 /* loop filter alpha offset */
+#define S5P_FIMV_ENC_H264_BETA_OFF      0xd00c /* loop filter beta offset */
+#define S5P_FIMV_ENC_H264_NUM_OF_REF    0xd010 /* number of reference for P/B */
+#define S5P_FIMV_ENC_H264_TRANS_FLAG    0xd034 /* 8x8 transform flag in PPS &
+                                                                high profile */
+
+#define S5P_FIMV_ENC_RC_FRAME_RATE      0xd0d0 /* frame rate */
+
+/* Encoder for MPEG4 only */
+#define S5P_FIMV_ENC_MPEG4_QUART_PXL    0xe008 /* qpel interpolation ctrl */
+
+/* Additional */
+#define S5P_FIMV_SI_CH0_DPB_CONF_CTRL   0x2068 /* DPB Config Control Register */
+#define S5P_FIMV_SLICE_INT_MASK         1
+#define S5P_FIMV_SLICE_INT_SHIFT        31
+#define S5P_FIMV_DDELAY_ENA_SHIFT       30
+#define S5P_FIMV_DDELAY_VAL_MASK        0xff
+#define S5P_FIMV_DDELAY_VAL_SHIFT       16
+#define S5P_FIMV_DPB_COUNT_MASK         0xffff
+#define S5P_FIMV_DPB_FLUSH_MASK         1
+#define S5P_FIMV_DPB_FLUSH_SHIFT        14
+
+
+#define S5P_FIMV_SI_CH0_RELEASE_BUF     0x2060 /* DPB release buffer register */
+#define S5P_FIMV_SI_CH0_HOST_WR_ADR     0x2064 /* address of shared memory */
+
+/* Codec numbers  */
+#define S5P_FIMV_CODEC_NONE             -1
+
+#define S5P_FIMV_CODEC_H264_DEC         0
+#define S5P_FIMV_CODEC_VC1_DEC          1
+#define S5P_FIMV_CODEC_MPEG4_DEC        2
+#define S5P_FIMV_CODEC_MPEG2_DEC        3
+#define S5P_FIMV_CODEC_H263_DEC         4
+#define S5P_FIMV_CODEC_VC1RCV_DEC       5
+
+#define S5P_FIMV_CODEC_H264_ENC         16
+#define S5P_FIMV_CODEC_MPEG4_ENC        17
+#define S5P_FIMV_CODEC_H263_ENC         18
+
+/* Channel Control Register */
+#define S5P_FIMV_CH_SEQ_HEADER          1
+#define S5P_FIMV_CH_FRAME_START         2
+#define S5P_FIMV_CH_LAST_FRAME          3
+#define S5P_FIMV_CH_INIT_BUFS           4
+#define S5P_FIMV_CH_FRAME_START_REALLOC 5
+#define S5P_FIMV_CH_MASK                7
+#define S5P_FIMV_CH_SHIFT               16
+
+
+/* Host to RISC command */
+#define S5P_FIMV_H2R_CMD_EMPTY          0
+#define S5P_FIMV_H2R_CMD_OPEN_INSTANCE  1
+#define S5P_FIMV_H2R_CMD_CLOSE_INSTANCE 2
+#define S5P_FIMV_H2R_CMD_SYS_INIT       3
+#define S5P_FIMV_H2R_CMD_FLUSH          4
+#define S5P_FIMV_H2R_CMD_SLEEP          5
+#define S5P_FIMV_H2R_CMD_WAKEUP         6
+
+#define S5P_FIMV_R2H_CMD_EMPTY                  0
+#define S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET      1
+#define S5P_FIMV_R2H_CMD_CLOSE_INSTANCE_RET     2
+#define S5P_FIMV_R2H_CMD_RSV_RET                3
+#define S5P_FIMV_R2H_CMD_SEQ_DONE_RET           4
+#define S5P_FIMV_R2H_CMD_FRAME_DONE_RET         5
+#define S5P_FIMV_R2H_CMD_SLICE_DONE_RET         6
+#define S5P_FIMV_R2H_CMD_ENC_COMPLETE_RET       7
+#define S5P_FIMV_R2H_CMD_SYS_INIT_RET           8
+#define S5P_FIMV_R2H_CMD_FW_STATUS_RET          9
+#define S5P_FIMV_R2H_CMD_SLEEP_RET              10
+#define S5P_FIMV_R2H_CMD_WAKEUP_RET             11
+#define S5P_FIMV_R2H_CMD_FLUSH_RET              12
+#define S5P_FIMV_R2H_CMD_INIT_BUFFERS_RET       15
+#define S5P_FIMV_R2H_CMD_EDFU_INIT_RET          16
+#define S5P_FIMV_R2H_CMD_ERR_RET                32
+
+/* Error handling defines */
+#define S5P_FIMV_ERR_WARNINGS_START             145
+#define S5P_FIMV_ERR_DEC_MASK                   0xFFFF
+#define S5P_FIMV_ERR_DEC_SHIFT                  0
+#define S5P_FIMV_ERR_DSPL_MASK                  0xFFFF0000
+#define S5P_FIMV_ERR_DSPL_SHIFT                 16
+
+/* Shared memory registers' offsets */
+
+/* An offset of the start position in the stream when
+ * the start position is not aligned */
+#define S5P_FIMV_SHARED_CROP_INFO_H             0x0020
+#define S5P_FIMV_SHARED_CROP_LEFT_MASK          0xFFFF
+#define S5P_FIMV_SHARED_CROP_LEFT_SHIFT         0
+#define S5P_FIMV_SHARED_CROP_RIGHT_MASK         0xFFFF0000
+#define S5P_FIMV_SHARED_CROP_RIGHT_SHIFT        16
+#define S5P_FIMV_SHARED_CROP_INFO_V             0x0024
+#define S5P_FIMV_SHARED_CROP_TOP_MASK           0xFFFF
+#define S5P_FIMV_SHARED_CROP_TOP_SHIFT          0
+#define S5P_FIMV_SHARED_CROP_BOTTOM_MASK        0xFFFF0000
+#define S5P_FIMV_SHARED_CROP_BOTTOM_SHIFT       16
+#define S5P_FIMV_SHARED_SET_FRAME_TAG           0x0004
+#define S5P_FIMV_SHARED_GET_FRAME_TAG_TOP       0x0008
+#define S5P_FIMV_SHARED_GET_FRAME_TAG_BOT       0x000C
+#define S5P_FIMV_SHARED_START_BYTE_NUM          0x0018
+#define S5P_FIMV_SHARED_RC_VOP_TIMING           0x0030
+#define S5P_FIMV_SHARED_LUMA_DPB_SIZE           0x0064
+#define S5P_FIMV_SHARED_CHROMA_DPB_SIZE         0x0068
+#define S5P_FIMV_SHARED_MV_SIZE                 0x006C
+#define S5P_FIMV_SHARED_PIC_TIME_TOP            0x0010
+#define S5P_FIMV_SHARED_PIC_TIME_BOTTOM         0x0014
+#define S5P_FIMV_SHARED_EXT_ENC_CONTROL         0x0028
+#define S5P_FIMV_SHARED_P_B_FRAME_QP            0x0070
+#define S5P_FIMV_SHARED_ASPECT_RATIO_IDC        0x0074
+#define S5P_FIMV_SHARED_EXTENDED_SAR            0x0078
+#define S5P_FIMV_SHARED_H264_I_PERIOD           0x009C
+#define S5P_FIMV_SHARED_RC_CONTROL_CONFIG       0x00A0
+
+#endif /* _REGS_FIMV_H */
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc.c b/drivers/media/video/s5p-mfc/s5p_mfc.c
new file mode 100644
index 00000000000..7dc7eab58b3
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc.c
@@ -0,0 +1,1274 @@
+/*
+ * Samsung S5P Multi Format Codec v 5.1
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ * Kamil Debski, <k.debski@samsung.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/clk.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/version.h>
+#include <linux/videodev2.h>
+#include <linux/workqueue.h>
+#include <media/videobuf2-core.h>
+#include "regs-mfc.h"
+#include "s5p_mfc_ctrl.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_dec.h"
+#include "s5p_mfc_enc.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_opr.h"
+#include "s5p_mfc_pm.h"
+#include "s5p_mfc_shm.h"
+
+#define S5P_MFC_NAME            "s5p-mfc"
+#define S5P_MFC_DEC_NAME        "s5p-mfc-dec"
+#define S5P_MFC_ENC_NAME        "s5p-mfc-enc"
+
+int debug;
+module_param(debug, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "Debug level - higher value produces more verbose messages");
+
+/* Helper functions for interrupt processing */
+/* Remove from hw execution round robin */
+static void clear_work_bit(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        spin_lock(&dev->condlock);
+        clear_bit(ctx->num, &dev->ctx_work_bits);
+        spin_unlock(&dev->condlock);
+}
+
+/* Wake up context wait_queue */
+static void wake_up_ctx(struct s5p_mfc_ctx *ctx, unsigned int reason,
+                        unsigned int err)
+{
+        ctx->int_cond = 1;
+        ctx->int_type = reason;
+        ctx->int_err = err;
+        wake_up(&ctx->queue);
+}
+
+/* Wake up device wait_queue */
+static void wake_up_dev(struct s5p_mfc_dev *dev, unsigned int reason,
+                        unsigned int err)
+{
+        dev->int_cond = 1;
+        dev->int_type = reason;
+        dev->int_err = err;
+        wake_up(&dev->queue);
+}
+
+void s5p_mfc_watchdog(unsigned long arg)
+{
+        struct s5p_mfc_dev *dev = (struct s5p_mfc_dev *)arg;
+
+        if (test_bit(0, &dev->hw_lock))
+                atomic_inc(&dev->watchdog_cnt);
+        if (atomic_read(&dev->watchdog_cnt) >= MFC_WATCHDOG_CNT) {
+                /* This means that hw is busy and no interrupts were
+                 * generated by hw for the Nth time of running this
+                 * watchdog timer. This usually means a serious hw
+                 * error. Now it is time to kill all instances and
+                 * reset the MFC. */
+                mfc_err("Time out during waiting for HW\n");
+                queue_work(dev->watchdog_workqueue, &dev->watchdog_work);
+        }
+        dev->watchdog_timer.expires = jiffies +
+                                        msecs_to_jiffies(MFC_WATCHDOG_INTERVAL);
+        add_timer(&dev->watchdog_timer);
+}
+
+static void s5p_mfc_watchdog_worker(struct work_struct *work)
+{
+        struct s5p_mfc_dev *dev;
+        struct s5p_mfc_ctx *ctx;
+        unsigned long flags;
+        int mutex_locked;
+        int i, ret;
+
+        dev = container_of(work, struct s5p_mfc_dev, watchdog_work);
+
+        mfc_err("Driver timeout error handling\n");
+        /* Lock the mutex that protects open and release.
+         * This is necessary as they may load and unload firmware. */
+        mutex_locked = mutex_trylock(&dev->mfc_mutex);
+        if (!mutex_locked)
+                mfc_err("Error: some instance may be closing/opening\n");
+        spin_lock_irqsave(&dev->irqlock, flags);
+
+        s5p_mfc_clock_off();
+
+        for (i = 0; i < MFC_NUM_CONTEXTS; i++) {
+                ctx = dev->ctx[i];
+                if (!ctx)
+                        continue;
+                ctx->state = MFCINST_ERROR;
+                s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
+                s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
+                clear_work_bit(ctx);
+                wake_up_ctx(ctx, S5P_FIMV_R2H_CMD_ERR_RET, 0);
+        }
+        clear_bit(0, &dev->hw_lock);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        /* Double check if there is at least one instance running.
+         * If no instance is in memory than no firmware should be present */
+        if (dev->num_inst > 0) {
+                ret = s5p_mfc_reload_firmware(dev);
+                if (ret) {
+                        mfc_err("Failed to reload FW\n");
+                        goto unlock;
+                }
+                s5p_mfc_clock_on();
+                ret = s5p_mfc_init_hw(dev);
+                if (ret)
+                        mfc_err("Failed to reinit FW\n");
+        }
+unlock:
+        if (mutex_locked)
+                mutex_unlock(&dev->mfc_mutex);
+}
+
+static enum s5p_mfc_node_type s5p_mfc_get_node_type(struct file *file)
+{
+        struct video_device *vdev = video_devdata(file);
+
+        if (!vdev) {
+                mfc_err("failed to get video_device");
+                return MFCNODE_INVALID;
+        }
+        if (vdev->index == 0)
+                return MFCNODE_DECODER;
+        else if (vdev->index == 1)
+                return MFCNODE_ENCODER;
+        return MFCNODE_INVALID;
+}
+
+static void s5p_mfc_clear_int_flags(struct s5p_mfc_dev *dev)
+{
+        mfc_write(dev, 0, S5P_FIMV_RISC_HOST_INT);
+        mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD);
+        mfc_write(dev, 0xffff, S5P_FIMV_SI_RTN_CHID);
+}
+
+static void s5p_mfc_handle_frame_all_extracted(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_buf *dst_buf;
+
+        ctx->state = MFCINST_FINISHED;
+        ctx->sequence++;
+        while (!list_empty(&ctx->dst_queue)) {
+                dst_buf = list_entry(ctx->dst_queue.next,
+                                     struct s5p_mfc_buf, list);
+                mfc_debug(2, "Cleaning up buffer: %d\n",
+                                          dst_buf->b->v4l2_buf.index);
+                vb2_set_plane_payload(dst_buf->b, 0, 0);
+                vb2_set_plane_payload(dst_buf->b, 1, 0);
+                list_del(&dst_buf->list);
+                ctx->dst_queue_cnt--;
+                dst_buf->b->v4l2_buf.sequence = (ctx->sequence++);
+
+                if (s5p_mfc_read_shm(ctx, PIC_TIME_TOP) ==
+                        s5p_mfc_read_shm(ctx, PIC_TIME_BOT))
+                        dst_buf->b->v4l2_buf.field = V4L2_FIELD_NONE;
+                else
+                        dst_buf->b->v4l2_buf.field = V4L2_FIELD_INTERLACED;
+
+                ctx->dec_dst_flag &= ~(1 << dst_buf->b->v4l2_buf.index);
+                vb2_buffer_done(dst_buf->b, VB2_BUF_STATE_DONE);
+        }
+}
+
+static void s5p_mfc_handle_frame_copy_time(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf  *dst_buf, *src_buf;
+        size_t dec_y_addr = s5p_mfc_get_dec_y_adr();
+        unsigned int frame_type = s5p_mfc_get_frame_type();
+
+        /* Copy timestamp / timecode from decoded src to dst and set
+           appropraite flags */
+        src_buf = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        list_for_each_entry(dst_buf, &ctx->dst_queue, list) {
+                if (vb2_dma_contig_plane_paddr(dst_buf->b, 0) == dec_y_addr) {
+                        memcpy(&dst_buf->b->v4l2_buf.timecode,
+                                &src_buf->b->v4l2_buf.timecode,
+                                sizeof(struct v4l2_timecode));
+                        memcpy(&dst_buf->b->v4l2_buf.timestamp,
+                                &src_buf->b->v4l2_buf.timestamp,
+                                sizeof(struct timeval));
+                        switch (frame_type) {
+                        case S5P_FIMV_DECODE_FRAME_I_FRAME:
+                                dst_buf->b->v4l2_buf.flags |=
+                                                V4L2_BUF_FLAG_KEYFRAME;
+                                break;
+                        case S5P_FIMV_DECODE_FRAME_P_FRAME:
+                                dst_buf->b->v4l2_buf.flags |=
+                                                V4L2_BUF_FLAG_PFRAME;
+                                break;
+                        case S5P_FIMV_DECODE_FRAME_B_FRAME:
+                                dst_buf->b->v4l2_buf.flags |=
+                                                V4L2_BUF_FLAG_BFRAME;
+                                break;
+                        }
+                        break;
+                }
+        }
+}
+
+static void s5p_mfc_handle_frame_new(struct s5p_mfc_ctx *ctx, unsigned int err)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf  *dst_buf;
+        size_t dspl_y_addr = s5p_mfc_get_dspl_y_adr();
+        unsigned int frame_type = s5p_mfc_get_frame_type();
+        unsigned int index;
+
+        /* If frame is same as previous then skip and do not dequeue */
+        if (frame_type == S5P_FIMV_DECODE_FRAME_SKIPPED) {
+                if (!ctx->after_packed_pb)
+                        ctx->sequence++;
+                ctx->after_packed_pb = 0;
+                return;
+        }
+        ctx->sequence++;
+        /* The MFC returns address of the buffer, now we have to
+         * check which videobuf does it correspond to */
+        list_for_each_entry(dst_buf, &ctx->dst_queue, list) {
+                /* Check if this is the buffer we're looking for */
+                if (vb2_dma_contig_plane_paddr(dst_buf->b, 0) == dspl_y_addr) {
+                        list_del(&dst_buf->list);
+                        ctx->dst_queue_cnt--;
+                        dst_buf->b->v4l2_buf.sequence = ctx->sequence;
+                        if (s5p_mfc_read_shm(ctx, PIC_TIME_TOP) ==
+                                s5p_mfc_read_shm(ctx, PIC_TIME_BOT))
+                                dst_buf->b->v4l2_buf.field = V4L2_FIELD_NONE;
+                        else
+                                dst_buf->b->v4l2_buf.field =
+                                                        V4L2_FIELD_INTERLACED;
+                        vb2_set_plane_payload(dst_buf->b, 0, ctx->luma_size);
+                        vb2_set_plane_payload(dst_buf->b, 1, ctx->chroma_size);
+                        clear_bit(dst_buf->b->v4l2_buf.index,
+                                                        &ctx->dec_dst_flag);
+
+                        vb2_buffer_done(dst_buf->b,
+                                err ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+
+                        index = dst_buf->b->v4l2_buf.index;
+                        break;
+                }
+        }
+}
+
+/* Handle frame decoding interrupt */
+static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
+                                        unsigned int reason, unsigned int err)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned int dst_frame_status;
+        struct s5p_mfc_buf *src_buf;
+        unsigned long flags;
+        unsigned int res_change;
+
+        unsigned int index;
+
+        dst_frame_status = s5p_mfc_get_dspl_status()
+                                & S5P_FIMV_DEC_STATUS_DECODING_STATUS_MASK;
+        res_change = s5p_mfc_get_dspl_status()
+                                & S5P_FIMV_DEC_STATUS_RESOLUTION_MASK;
+        mfc_debug(2, "Frame Status: %x\n", dst_frame_status);
+        if (ctx->state == MFCINST_RES_CHANGE_INIT)
+                ctx->state = MFCINST_RES_CHANGE_FLUSH;
+        if (res_change) {
+                ctx->state = MFCINST_RES_CHANGE_INIT;
+                s5p_mfc_clear_int_flags(dev);
+                wake_up_ctx(ctx, reason, err);
+                if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+                        BUG();
+                s5p_mfc_clock_off();
+                s5p_mfc_try_run(dev);
+                return;
+        }
+        if (ctx->dpb_flush_flag)
+                ctx->dpb_flush_flag = 0;
+
+        spin_lock_irqsave(&dev->irqlock, flags);
+        /* All frames remaining in the buffer have been extracted  */
+        if (dst_frame_status == S5P_FIMV_DEC_STATUS_DECODING_EMPTY) {
+                if (ctx->state == MFCINST_RES_CHANGE_FLUSH) {
+                        s5p_mfc_handle_frame_all_extracted(ctx);
+                        ctx->state = MFCINST_RES_CHANGE_END;
+                        goto leave_handle_frame;
+                } else {
+                        s5p_mfc_handle_frame_all_extracted(ctx);
+                }
+        }
+
+        if (dst_frame_status == S5P_FIMV_DEC_STATUS_DECODING_DISPLAY ||
+                dst_frame_status == S5P_FIMV_DEC_STATUS_DECODING_ONLY)
+                s5p_mfc_handle_frame_copy_time(ctx);
+
+        /* A frame has been decoded and is in the buffer  */
+        if (dst_frame_status == S5P_FIMV_DEC_STATUS_DISPLAY_ONLY ||
+            dst_frame_status == S5P_FIMV_DEC_STATUS_DECODING_DISPLAY) {
+                s5p_mfc_handle_frame_new(ctx, err);
+        } else {
+                mfc_debug(2, "No frame decode\n");
+        }
+        /* Mark source buffer as complete */
+        if (dst_frame_status != S5P_FIMV_DEC_STATUS_DISPLAY_ONLY
+                && !list_empty(&ctx->src_queue)) {
+                src_buf = list_entry(ctx->src_queue.next, struct s5p_mfc_buf,
+                                                                list);
+                ctx->consumed_stream += s5p_mfc_get_consumed_stream();
+                if (ctx->codec_mode != S5P_FIMV_CODEC_H264_DEC &&
+                        s5p_mfc_get_frame_type() == S5P_FIMV_DECODE_FRAME_P_FRAME
+                                        && ctx->consumed_stream + STUFF_BYTE <
+                                        src_buf->b->v4l2_planes[0].bytesused) {
+                        /* Run MFC again on the same buffer */
+                        mfc_debug(2, "Running again the same buffer\n");
+                        ctx->after_packed_pb = 1;
+                } else {
+                        index = src_buf->b->v4l2_buf.index;
+                        mfc_debug(2, "MFC needs next buffer\n");
+                        ctx->consumed_stream = 0;
+                        list_del(&src_buf->list);
+                        ctx->src_queue_cnt--;
+                        if (s5p_mfc_err_dec(err) > 0)
+                                vb2_buffer_done(src_buf->b, VB2_BUF_STATE_ERROR);
+                        else
+                                vb2_buffer_done(src_buf->b, VB2_BUF_STATE_DONE);
+                }
+        }
+leave_handle_frame:
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        if ((ctx->src_queue_cnt == 0 && ctx->state != MFCINST_FINISHING)
+                                    || ctx->dst_queue_cnt < ctx->dpb_count)
+                clear_work_bit(ctx);
+        s5p_mfc_clear_int_flags(dev);
+        wake_up_ctx(ctx, reason, err);
+        if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+                BUG();
+        s5p_mfc_clock_off();
+        s5p_mfc_try_run(dev);
+}
+
+/* Error handling for interrupt */
+static void s5p_mfc_handle_error(struct s5p_mfc_ctx *ctx,
+                                 unsigned int reason, unsigned int err)
+{
+        struct s5p_mfc_dev *dev;
+        unsigned long flags;
+
+        /* If no context is available then all necessary
+         * processing has been done. */
+        if (ctx == 0)
+                return;
+
+        dev = ctx->dev;
+        mfc_err("Interrupt Error: %08x\n", err);
+        s5p_mfc_clear_int_flags(dev);
+        wake_up_dev(dev, reason, err);
+
+        /* Error recovery is dependent on the state of context */
+        switch (ctx->state) {
+        case MFCINST_INIT:
+                /* This error had to happen while acquireing instance */
+        case MFCINST_GOT_INST:
+                /* This error had to happen while parsing the header */
+        case MFCINST_HEAD_PARSED:
+                /* This error had to happen while setting dst buffers */
+        case MFCINST_RETURN_INST:
+                /* This error had to happen while releasing instance */
+                clear_work_bit(ctx);
+                wake_up_ctx(ctx, reason, err);
+                if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+                        BUG();
+                s5p_mfc_clock_off();
+                ctx->state = MFCINST_ERROR;
+                break;
+        case MFCINST_FINISHING:
+        case MFCINST_FINISHED:
+        case MFCINST_RUNNING:
+                /* It is higly probable that an error occured
+                 * while decoding a frame */
+                clear_work_bit(ctx);
+                ctx->state = MFCINST_ERROR;
+                /* Mark all dst buffers as having an error */
+                spin_lock_irqsave(&dev->irqlock, flags);
+                s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
+                /* Mark all src buffers as having an error */
+                s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+                        BUG();
+                s5p_mfc_clock_off();
+                break;
+        default:
+                mfc_err("Encountered an error interrupt which had not been handled\n");
+                break;
+        }
+        return;
+}
+
+/* Header parsing interrupt handling */
+static void s5p_mfc_handle_seq_done(struct s5p_mfc_ctx *ctx,
+                                 unsigned int reason, unsigned int err)
+{
+        struct s5p_mfc_dev *dev;
+        unsigned int guard_width, guard_height;
+
+        if (ctx == 0)
+                return;
+        dev = ctx->dev;
+        if (ctx->c_ops->post_seq_start) {
+                if (ctx->c_ops->post_seq_start(ctx))
+                        mfc_err("post_seq_start() failed\n");
+        } else {
+                ctx->img_width = s5p_mfc_get_img_width();
+                ctx->img_height = s5p_mfc_get_img_height();
+
+                ctx->buf_width = ALIGN(ctx->img_width,
+                                                S5P_FIMV_NV12MT_HALIGN);
+                ctx->buf_height = ALIGN(ctx->img_height,
+                                                S5P_FIMV_NV12MT_VALIGN);
+                mfc_debug(2, "SEQ Done: Movie dimensions %dx%d, "
+                        "buffer dimensions: %dx%d\n", ctx->img_width,
+                                ctx->img_height, ctx->buf_width,
+                                                ctx->buf_height);
+                if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC) {
+                        ctx->luma_size = ALIGN(ctx->buf_width *
+                                ctx->buf_height, S5P_FIMV_DEC_BUF_ALIGN);
+                        ctx->chroma_size = ALIGN(ctx->buf_width *
+                                         ALIGN((ctx->img_height >> 1),
+                                               S5P_FIMV_NV12MT_VALIGN),
+                                               S5P_FIMV_DEC_BUF_ALIGN);
+                        ctx->mv_size = ALIGN(ctx->buf_width *
+                                        ALIGN((ctx->buf_height >> 2),
+                                        S5P_FIMV_NV12MT_VALIGN),
+                                        S5P_FIMV_DEC_BUF_ALIGN);
+                } else {
+                        guard_width = ALIGN(ctx->img_width + 24,
+                                        S5P_FIMV_NV12MT_HALIGN);
+                        guard_height = ALIGN(ctx->img_height + 16,
+                                                S5P_FIMV_NV12MT_VALIGN);
+                        ctx->luma_size = ALIGN(guard_width *
+                                guard_height, S5P_FIMV_DEC_BUF_ALIGN);
+                        guard_width = ALIGN(ctx->img_width + 16,
+                                                S5P_FIMV_NV12MT_HALIGN);
+                        guard_height = ALIGN((ctx->img_height >> 1) + 4,
+                                                S5P_FIMV_NV12MT_VALIGN);
+                        ctx->chroma_size = ALIGN(guard_width *
+                                guard_height, S5P_FIMV_DEC_BUF_ALIGN);
+                        ctx->mv_size = 0;
+                }
+                ctx->dpb_count = s5p_mfc_get_dpb_count();
+                if (ctx->img_width == 0 || ctx->img_width == 0)
+                        ctx->state = MFCINST_ERROR;
+                else
+                        ctx->state = MFCINST_HEAD_PARSED;
+        }
+        s5p_mfc_clear_int_flags(dev);
+        clear_work_bit(ctx);
+        if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+                BUG();
+        s5p_mfc_clock_off();
+        s5p_mfc_try_run(dev);
+        wake_up_ctx(ctx, reason, err);
+}
+
+/* Header parsing interrupt handling */
+static void s5p_mfc_handle_init_buffers(struct s5p_mfc_ctx *ctx,
+                                 unsigned int reason, unsigned int err)
+{
+        struct s5p_mfc_buf *src_buf;
+        struct s5p_mfc_dev *dev;
+        unsigned long flags;
+
+        if (ctx == 0)
+                return;
+        dev = ctx->dev;
+        s5p_mfc_clear_int_flags(dev);
+        ctx->int_type = reason;
+        ctx->int_err = err;
+        ctx->int_cond = 1;
+        spin_lock(&dev->condlock);
+        clear_bit(ctx->num, &dev->ctx_work_bits);
+        spin_unlock(&dev->condlock);
+        if (err == 0) {
+                ctx->state = MFCINST_RUNNING;
+                if (!ctx->dpb_flush_flag) {
+                        spin_lock_irqsave(&dev->irqlock, flags);
+                        if (!list_empty(&ctx->src_queue)) {
+                                src_buf = list_entry(ctx->src_queue.next,
+                                             struct s5p_mfc_buf, list);
+                                list_del(&src_buf->list);
+                                ctx->src_queue_cnt--;
+                                vb2_buffer_done(src_buf->b,
+                                                VB2_BUF_STATE_DONE);
+                        }
+                        spin_unlock_irqrestore(&dev->irqlock, flags);
+                } else {
+                        ctx->dpb_flush_flag = 0;
+                }
+                if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+                        BUG();
+
+                s5p_mfc_clock_off();
+
+                wake_up(&ctx->queue);
+                s5p_mfc_try_run(dev);
+        } else {
+                if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+                        BUG();
+
+                s5p_mfc_clock_off();
+
+                wake_up(&ctx->queue);
+        }
+}
+
+/* Interrupt processing */
+static irqreturn_t s5p_mfc_irq(int irq, void *priv)
+{
+        struct s5p_mfc_dev *dev = priv;
+        struct s5p_mfc_ctx *ctx;
+        unsigned int reason;
+        unsigned int err;
+
+        mfc_debug_enter();
+        /* Reset the timeout watchdog */
+        atomic_set(&dev->watchdog_cnt, 0);
+        ctx = dev->ctx[dev->curr_ctx];
+        /* Get the reason of interrupt and the error code */
+        reason = s5p_mfc_get_int_reason();
+        err = s5p_mfc_get_int_err();
+        mfc_debug(1, "Int reason: %d (err: %08x)\n", reason, err);
+        switch (reason) {
+        case S5P_FIMV_R2H_CMD_ERR_RET:
+                /* An error has occured */
+                if (ctx->state == MFCINST_RUNNING &&
+                        s5p_mfc_err_dec(err) >= S5P_FIMV_ERR_WARNINGS_START)
+                        s5p_mfc_handle_frame(ctx, reason, err);
+                else
+                        s5p_mfc_handle_error(ctx, reason, err);
+                clear_bit(0, &dev->enter_suspend);
+                break;
+
+        case S5P_FIMV_R2H_CMD_SLICE_DONE_RET:
+        case S5P_FIMV_R2H_CMD_FRAME_DONE_RET:
+                if (ctx->c_ops->post_frame_start) {
+                        if (ctx->c_ops->post_frame_start(ctx))
+                                mfc_err("post_frame_start() failed\n");
+                        s5p_mfc_clear_int_flags(dev);
+                        wake_up_ctx(ctx, reason, err);
+                        if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+                                BUG();
+                        s5p_mfc_clock_off();
+                        s5p_mfc_try_run(dev);
+                } else {
+                        s5p_mfc_handle_frame(ctx, reason, err);
+                }
+                break;
+
+        case S5P_FIMV_R2H_CMD_SEQ_DONE_RET:
+                s5p_mfc_handle_seq_done(ctx, reason, err);
+                break;
+
+        case S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET:
+                ctx->inst_no = s5p_mfc_get_inst_no();
+                ctx->state = MFCINST_GOT_INST;
+                clear_work_bit(ctx);
+                wake_up(&ctx->queue);
+                goto irq_cleanup_hw;
+
+        case S5P_FIMV_R2H_CMD_CLOSE_INSTANCE_RET:
+                clear_work_bit(ctx);
+                ctx->state = MFCINST_FREE;
+                wake_up(&ctx->queue);
+                goto irq_cleanup_hw;
+
+        case S5P_FIMV_R2H_CMD_SYS_INIT_RET:
+        case S5P_FIMV_R2H_CMD_FW_STATUS_RET:
+        case S5P_FIMV_R2H_CMD_SLEEP_RET:
+        case S5P_FIMV_R2H_CMD_WAKEUP_RET:
+                if (ctx)
+                        clear_work_bit(ctx);
+                s5p_mfc_clear_int_flags(dev);
+                wake_up_dev(dev, reason, err);
+                clear_bit(0, &dev->hw_lock);
+                clear_bit(0, &dev->enter_suspend);
+                break;
+
+        case S5P_FIMV_R2H_CMD_INIT_BUFFERS_RET:
+                s5p_mfc_handle_init_buffers(ctx, reason, err);
+                break;
+        default:
+                mfc_debug(2, "Unknown int reason\n");
+                s5p_mfc_clear_int_flags(dev);
+        }
+        mfc_debug_leave();
+        return IRQ_HANDLED;
+irq_cleanup_hw:
+        s5p_mfc_clear_int_flags(dev);
+        ctx->int_type = reason;
+        ctx->int_err = err;
+        ctx->int_cond = 1;
+        if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+                mfc_err("Failed to unlock hw\n");
+
+        s5p_mfc_clock_off();
+
+        s5p_mfc_try_run(dev);
+        mfc_debug(2, "Exit via irq_cleanup_hw\n");
+        return IRQ_HANDLED;
+}
+
+/* Open an MFC node */
+static int s5p_mfc_open(struct file *file)
+{
+        struct s5p_mfc_dev *dev = video_drvdata(file);
+        struct s5p_mfc_ctx *ctx = NULL;
+        struct vb2_queue *q;
+        unsigned long flags;
+        int ret = 0;
+
+        mfc_debug_enter();
+        dev->num_inst++;        /* It is guarded by mfc_mutex in vfd */
+        /* Allocate memory for context */
+        ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
+        if (!ctx) {
+                mfc_err("Not enough memory\n");
+                ret = -ENOMEM;
+                goto err_alloc;
+        }
+        v4l2_fh_init(&ctx->fh, video_devdata(file));
+        file->private_data = &ctx->fh;
+        v4l2_fh_add(&ctx->fh);
+        ctx->dev = dev;
+        INIT_LIST_HEAD(&ctx->src_queue);
+        INIT_LIST_HEAD(&ctx->dst_queue);
+        ctx->src_queue_cnt = 0;
+        ctx->dst_queue_cnt = 0;
+        /* Get context number */
+        ctx->num = 0;
+        while (dev->ctx[ctx->num]) {
+                ctx->num++;
+                if (ctx->num >= MFC_NUM_CONTEXTS) {
+                        mfc_err("Too many open contexts\n");
+                        ret = -EBUSY;
+                        goto err_no_ctx;
+                }
+        }
+        /* Mark context as idle */
+        spin_lock_irqsave(&dev->condlock, flags);
+        clear_bit(ctx->num, &dev->ctx_work_bits);
+        spin_unlock_irqrestore(&dev->condlock, flags);
+        dev->ctx[ctx->num] = ctx;
+        if (s5p_mfc_get_node_type(file) == MFCNODE_DECODER) {
+                ctx->type = MFCINST_DECODER;
+                ctx->c_ops = get_dec_codec_ops();
+                /* Setup ctrl handler */
+                ret = s5p_mfc_dec_ctrls_setup(ctx);
+                if (ret) {
+                        mfc_err("Failed to setup mfc controls\n");
+                        goto err_ctrls_setup;
+                }
+        } else if (s5p_mfc_get_node_type(file) == MFCNODE_ENCODER) {
+                ctx->type = MFCINST_ENCODER;
+                ctx->c_ops = get_enc_codec_ops();
+                /* only for encoder */
+                INIT_LIST_HEAD(&ctx->ref_queue);
+                ctx->ref_queue_cnt = 0;
+                /* Setup ctrl handler */
+                ret = s5p_mfc_enc_ctrls_setup(ctx);
+                if (ret) {
+                        mfc_err("Failed to setup mfc controls\n");
+                        goto err_ctrls_setup;
+                }
+        } else {
+                ret = -ENOENT;
+                goto err_bad_node;
+        }
+        ctx->fh.ctrl_handler = &ctx->ctrl_handler;
+        ctx->inst_no = -1;
+        /* Load firmware if this is the first instance */
+        if (dev->num_inst == 1) {
+                dev->watchdog_timer.expires = jiffies +
+                                        msecs_to_jiffies(MFC_WATCHDOG_INTERVAL);
+                add_timer(&dev->watchdog_timer);
+                ret = s5p_mfc_power_on();
+                if (ret < 0) {
+                        mfc_err("power on failed\n");
+                        goto err_pwr_enable;
+                }
+                s5p_mfc_clock_on();
+                ret = s5p_mfc_alloc_and_load_firmware(dev);
+                if (ret)
+                        goto err_alloc_fw;
+                /* Init the FW */
+                ret = s5p_mfc_init_hw(dev);
+                if (ret)
+                        goto err_init_hw;
+                s5p_mfc_clock_off();
+        }
+        /* Init videobuf2 queue for CAPTURE */
+        q = &ctx->vq_dst;
+        q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+        q->drv_priv = &ctx->fh;
+        if (s5p_mfc_get_node_type(file) == MFCNODE_DECODER) {
+                q->io_modes = VB2_MMAP;
+                q->ops = get_dec_queue_ops();
+        } else if (s5p_mfc_get_node_type(file) == MFCNODE_ENCODER) {
+                q->io_modes = VB2_MMAP | VB2_USERPTR;
+                q->ops = get_enc_queue_ops();
+        } else {
+                ret = -ENOENT;
+                goto err_queue_init;
+        }
+        q->mem_ops = (struct vb2_mem_ops *)&vb2_dma_contig_memops;
+        ret = vb2_queue_init(q);
+        if (ret) {
+                mfc_err("Failed to initialize videobuf2 queue(capture)\n");
+                goto err_queue_init;
+        }
+        /* 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;
+        if (s5p_mfc_get_node_type(file) == MFCNODE_DECODER) {
+                q->io_modes = VB2_MMAP;
+                q->ops = get_dec_queue_ops();
+        } else if (s5p_mfc_get_node_type(file) == MFCNODE_ENCODER) {
+                q->io_modes = VB2_MMAP | VB2_USERPTR;
+                q->ops = get_enc_queue_ops();
+        } else {
+                ret = -ENOENT;
+                goto err_queue_init;
+        }
+        q->mem_ops = (struct vb2_mem_ops *)&vb2_dma_contig_memops;
+        ret = vb2_queue_init(q);
+        if (ret) {
+                mfc_err("Failed to initialize videobuf2 queue(output)\n");
+                goto err_queue_init;
+        }
+        init_waitqueue_head(&ctx->queue);
+        mfc_debug_leave();
+        return ret;
+        /* Deinit when failure occured */
+err_queue_init:
+err_init_hw:
+        s5p_mfc_release_firmware(dev);
+err_alloc_fw:
+        dev->ctx[ctx->num] = 0;
+        del_timer_sync(&dev->watchdog_timer);
+        s5p_mfc_clock_off();
+err_pwr_enable:
+        if (dev->num_inst == 1) {
+                if (s5p_mfc_power_off() < 0)
+                        mfc_err("power off failed\n");
+                s5p_mfc_release_firmware(dev);
+        }
+err_ctrls_setup:
+        s5p_mfc_dec_ctrls_delete(ctx);
+err_bad_node:
+err_no_ctx:
+        v4l2_fh_del(&ctx->fh);
+        v4l2_fh_exit(&ctx->fh);
+        kfree(ctx);
+err_alloc:
+        dev->num_inst--;
+        mfc_debug_leave();
+        return ret;
+}
+
+/* Release MFC context */
+static int s5p_mfc_release(struct file *file)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(file->private_data);
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+
+        mfc_debug_enter();
+        s5p_mfc_clock_on();
+        vb2_queue_release(&ctx->vq_src);
+        vb2_queue_release(&ctx->vq_dst);
+        /* Mark context as idle */
+        spin_lock_irqsave(&dev->condlock, flags);
+        clear_bit(ctx->num, &dev->ctx_work_bits);
+        spin_unlock_irqrestore(&dev->condlock, flags);
+        /* If instance was initialised then
+         * return instance and free reosurces */
+        if (ctx->inst_no != MFC_NO_INSTANCE_SET) {
+                mfc_debug(2, "Has to free instance\n");
+                ctx->state = MFCINST_RETURN_INST;
+                spin_lock_irqsave(&dev->condlock, flags);
+                set_bit(ctx->num, &dev->ctx_work_bits);
+                spin_unlock_irqrestore(&dev->condlock, flags);
+                s5p_mfc_clean_ctx_int_flags(ctx);
+                s5p_mfc_try_run(dev);
+                /* Wait until instance is returned or timeout occured */
+                if (s5p_mfc_wait_for_done_ctx
+                    (ctx, S5P_FIMV_R2H_CMD_CLOSE_INSTANCE_RET, 0)) {
+                        s5p_mfc_clock_off();
+                        mfc_err("Err returning instance\n");
+                }
+                mfc_debug(2, "After free instance\n");
+                /* Free resources */
+                s5p_mfc_release_codec_buffers(ctx);
+                s5p_mfc_release_instance_buffer(ctx);
+                if (ctx->type == MFCINST_DECODER)
+                        s5p_mfc_release_dec_desc_buffer(ctx);
+
+                ctx->inst_no = MFC_NO_INSTANCE_SET;
+        }
+        /* hardware locking scheme */
+        if (dev->curr_ctx == ctx->num)
+                clear_bit(0, &dev->hw_lock);
+        dev->num_inst--;
+        if (dev->num_inst == 0) {
+                mfc_debug(2, "Last instance - release firmware\n");
+                /* reset <-> F/W release */
+                s5p_mfc_reset(dev);
+                s5p_mfc_release_firmware(dev);
+                del_timer_sync(&dev->watchdog_timer);
+                if (s5p_mfc_power_off() < 0)
+                        mfc_err("Power off failed\n");
+        }
+        mfc_debug(2, "Shutting down clock\n");
+        s5p_mfc_clock_off();
+        dev->ctx[ctx->num] = 0;
+        s5p_mfc_dec_ctrls_delete(ctx);
+        v4l2_fh_del(&ctx->fh);
+        v4l2_fh_exit(&ctx->fh);
+        kfree(ctx);
+        mfc_debug_leave();
+        return 0;
+}
+
+/* Poll */
+static unsigned int s5p_mfc_poll(struct file *file,
+                                 struct poll_table_struct *wait)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(file->private_data);
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct vb2_queue *src_q, *dst_q;
+        struct vb2_buffer *src_vb = NULL, *dst_vb = NULL;
+        unsigned int rc = 0;
+        unsigned long flags;
+
+        src_q = &ctx->vq_src;
+        dst_q = &ctx->vq_dst;
+        /*
+         * There has to be at least one buffer queued on each queued_list, which
+         * means either in driver already or waiting for driver to claim it
+         * and start processing.
+         */
+        if ((!src_q->streaming || list_empty(&src_q->queued_list))
+                && (!dst_q->streaming || list_empty(&dst_q->queued_list))) {
+                rc = POLLERR;
+                goto end;
+        }
+        mutex_unlock(&dev->mfc_mutex);
+        poll_wait(file, &src_q->done_wq, wait);
+        poll_wait(file, &dst_q->done_wq, wait);
+        mutex_lock(&dev->mfc_mutex);
+        spin_lock_irqsave(&src_q->done_lock, flags);
+        if (!list_empty(&src_q->done_list))
+                src_vb = list_first_entry(&src_q->done_list, struct vb2_buffer,
+                                                                done_entry);
+        if (src_vb && (src_vb->state == VB2_BUF_STATE_DONE
+                                || src_vb->state == VB2_BUF_STATE_ERROR))
+                rc |= POLLOUT | POLLWRNORM;
+        spin_unlock_irqrestore(&src_q->done_lock, flags);
+        spin_lock_irqsave(&dst_q->done_lock, flags);
+        if (!list_empty(&dst_q->done_list))
+                dst_vb = list_first_entry(&dst_q->done_list, struct vb2_buffer,
+                                                                done_entry);
+        if (dst_vb && (dst_vb->state == VB2_BUF_STATE_DONE
+                                || dst_vb->state == VB2_BUF_STATE_ERROR))
+                rc |= POLLIN | POLLRDNORM;
+        spin_unlock_irqrestore(&dst_q->done_lock, flags);
+end:
+        return rc;
+}
+
+/* Mmap */
+static int s5p_mfc_mmap(struct file *file, struct vm_area_struct *vma)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(file->private_data);
+        unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
+        int ret;
+        if (offset < DST_QUEUE_OFF_BASE) {
+                mfc_debug(2, "mmaping source\n");
+                ret = vb2_mmap(&ctx->vq_src, vma);
+        } else {                /* capture */
+                mfc_debug(2, "mmaping destination\n");
+                vma->vm_pgoff -= (DST_QUEUE_OFF_BASE >> PAGE_SHIFT);
+                ret = vb2_mmap(&ctx->vq_dst, vma);
+        }
+        return ret;
+}
+
+/* v4l2 ops */
+static const struct v4l2_file_operations s5p_mfc_fops = {
+        .owner = THIS_MODULE,
+        .open = s5p_mfc_open,
+        .release = s5p_mfc_release,
+        .poll = s5p_mfc_poll,
+        .unlocked_ioctl = video_ioctl2,
+        .mmap = s5p_mfc_mmap,
+};
+
+static int match_child(struct device *dev, void *data)
+{
+        if (!dev_name(dev))
+                return 0;
+        return !strcmp(dev_name(dev), (char *)data);
+}
+
+
+/* MFC probe function */
+static int __devinit s5p_mfc_probe(struct platform_device *pdev)
+{
+        struct s5p_mfc_dev *dev;
+        struct video_device *vfd;
+        struct resource *res;
+        int ret;
+
+        pr_debug("%s++\n", __func__);
+        dev = kzalloc(sizeof *dev, GFP_KERNEL);
+        if (!dev) {
+                dev_err(&pdev->dev, "Not enough memory for MFC device\n");
+                return -ENOMEM;
+        }
+
+        spin_lock_init(&dev->irqlock);
+        spin_lock_init(&dev->condlock);
+        dev->plat_dev = pdev;
+        if (!dev->plat_dev) {
+                dev_err(&pdev->dev, "No platform data specified\n");
+                ret = -ENODEV;
+                goto err_dev;
+        }
+
+        ret = s5p_mfc_init_pm(dev);
+        if (ret < 0) {
+                dev_err(&pdev->dev, "failed to get mfc clock source\n");
+                goto err_clk;
+        }
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (res == NULL) {
+                dev_err(&pdev->dev, "failed to get memory region resource\n");
+                ret = -ENOENT;
+                goto err_res;
+        }
+
+        dev->mfc_mem = request_mem_region(res->start, resource_size(res),
+                                          pdev->name);
+        if (dev->mfc_mem == NULL) {
+                dev_err(&pdev->dev, "failed to get memory region\n");
+                ret = -ENOENT;
+                goto err_mem_reg;
+        }
+        dev->regs_base = ioremap(dev->mfc_mem->start, resource_size(dev->mfc_mem));
+        if (dev->regs_base == NULL) {
+                dev_err(&pdev->dev, "failed to ioremap address region\n");
+                ret = -ENOENT;
+                goto err_ioremap;
+        }
+
+        res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+        if (res == NULL) {
+                dev_err(&pdev->dev, "failed to get irq resource\n");
+                ret = -ENOENT;
+                goto err_get_res;
+        }
+        dev->irq = res->start;
+        ret = request_irq(dev->irq, s5p_mfc_irq, IRQF_DISABLED, pdev->name,
+                                                                        dev);
+        if (ret) {
+                dev_err(&pdev->dev, "Failed to install irq (%d)\n", ret);
+                goto err_req_irq;
+        }
+
+        dev->mem_dev_l = device_find_child(&dev->plat_dev->dev, "s5p-mfc-l",
+                                           match_child);
+        if (!dev->mem_dev_l) {
+                mfc_err("Mem child (L) device get failed\n");
+                ret = -ENODEV;
+                goto err_find_child;
+        }
+        dev->mem_dev_r = device_find_child(&dev->plat_dev->dev, "s5p-mfc-r",
+                                           match_child);
+        if (!dev->mem_dev_r) {
+                mfc_err("Mem child (R) device get failed\n");
+                ret = -ENODEV;
+                goto err_find_child;
+        }
+
+        dev->alloc_ctx[0] = vb2_dma_contig_init_ctx(dev->mem_dev_l);
+        if (IS_ERR_OR_NULL(dev->alloc_ctx[0])) {
+                ret = PTR_ERR(dev->alloc_ctx[0]);
+                goto err_mem_init_ctx_0;
+        }
+        dev->alloc_ctx[1] = vb2_dma_contig_init_ctx(dev->mem_dev_r);
+        if (IS_ERR_OR_NULL(dev->alloc_ctx[1])) {
+                ret = PTR_ERR(dev->alloc_ctx[1]);
+                goto err_mem_init_ctx_1;
+        }
+
+        mutex_init(&dev->mfc_mutex);
+
+        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();
+        if (!vfd) {
+                v4l2_err(&dev->v4l2_dev, "Failed to allocate video device\n");
+                ret = -ENOMEM;
+                goto err_dec_alloc;
+        }
+        vfd->fops       = &s5p_mfc_fops,
+        vfd->ioctl_ops  = get_dec_v4l2_ioctl_ops();
+        vfd->release    = video_device_release,
+        vfd->lock       = &dev->mfc_mutex;
+        vfd->v4l2_dev   = &dev->v4l2_dev;
+        snprintf(vfd->name, sizeof(vfd->name), "%s", S5P_MFC_DEC_NAME);
+        dev->vfd_dec    = vfd;
+        ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
+        if (ret) {
+                v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
+                video_device_release(vfd);
+                goto err_dec_reg;
+        }
+        v4l2_info(&dev->v4l2_dev,
+                  "decoder registered as /dev/video%d\n", vfd->num);
+        video_set_drvdata(vfd, dev);
+
+        /* encoder */
+        vfd = video_device_alloc();
+        if (!vfd) {
+                v4l2_err(&dev->v4l2_dev, "Failed to allocate video device\n");
+                ret = -ENOMEM;
+                goto err_enc_alloc;
+        }
+        vfd->fops       = &s5p_mfc_fops,
+        vfd->ioctl_ops  = get_enc_v4l2_ioctl_ops();
+        vfd->release    = video_device_release,
+        vfd->lock       = &dev->mfc_mutex;
+        vfd->v4l2_dev   = &dev->v4l2_dev;
+        snprintf(vfd->name, sizeof(vfd->name), "%s", S5P_MFC_ENC_NAME);
+        dev->vfd_enc    = vfd;
+        ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
+        if (ret) {
+                v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
+                video_device_release(vfd);
+                goto err_enc_reg;
+        }
+        v4l2_info(&dev->v4l2_dev,
+                  "encoder registered as /dev/video%d\n", vfd->num);
+        video_set_drvdata(vfd, dev);
+        platform_set_drvdata(pdev, dev);
+
+        dev->hw_lock = 0;
+        dev->watchdog_workqueue = create_singlethread_workqueue(S5P_MFC_NAME);
+        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;
+
+        pr_debug("%s--\n", __func__);
+        return 0;
+
+/* Deinit MFC if probe had failed */
+err_enc_reg:
+        video_device_release(dev->vfd_enc);
+err_enc_alloc:
+        video_unregister_device(dev->vfd_dec);
+err_dec_reg:
+        video_device_release(dev->vfd_dec);
+err_dec_alloc:
+        v4l2_device_unregister(&dev->v4l2_dev);
+err_v4l2_dev_reg:
+        vb2_dma_contig_cleanup_ctx(dev->alloc_ctx[1]);
+err_mem_init_ctx_1:
+        vb2_dma_contig_cleanup_ctx(dev->alloc_ctx[0]);
+err_mem_init_ctx_0:
+err_find_child:
+        free_irq(dev->irq, dev);
+err_req_irq:
+err_get_res:
+        iounmap(dev->regs_base);
+        dev->regs_base = NULL;
+err_ioremap:
+        release_resource(dev->mfc_mem);
+        kfree(dev->mfc_mem);
+err_mem_reg:
+err_res:
+        s5p_mfc_final_pm(dev);
+err_clk:
+err_dev:
+        kfree(dev);
+        pr_debug("%s-- with error\n", __func__);
+        return ret;
+
+}
+
+/* Remove the driver */
+static int __devexit s5p_mfc_remove(struct platform_device *pdev)
+{
+        struct s5p_mfc_dev *dev = platform_get_drvdata(pdev);
+
+        v4l2_info(&dev->v4l2_dev, "Removing %s\n", pdev->name);
+
+        del_timer_sync(&dev->watchdog_timer);
+        flush_workqueue(dev->watchdog_workqueue);
+        destroy_workqueue(dev->watchdog_workqueue);
+
+        video_unregister_device(dev->vfd_enc);
+        video_unregister_device(dev->vfd_dec);
+        v4l2_device_unregister(&dev->v4l2_dev);
+        vb2_dma_contig_cleanup_ctx(dev->alloc_ctx[0]);
+        vb2_dma_contig_cleanup_ctx(dev->alloc_ctx[1]);
+
+        free_irq(dev->irq, dev);
+        iounmap(dev->regs_base);
+        if (dev->mfc_mem) {
+                release_resource(dev->mfc_mem);
+                kfree(dev->mfc_mem);
+                dev->mfc_mem = NULL;
+        }
+        s5p_mfc_final_pm(dev);
+        kfree(dev);
+        return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+
+static int s5p_mfc_suspend(struct device *dev)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct s5p_mfc_dev *m_dev = platform_get_drvdata(pdev);
+        int ret;
+
+        if (m_dev->num_inst == 0)
+                return 0;
+        return s5p_mfc_sleep(m_dev);
+        if (test_and_set_bit(0, &m_dev->enter_suspend) != 0) {
+                mfc_err("Error: going to suspend for a second time\n");
+                return -EIO;
+        }
+
+        /* Check if we're processing then wait if it necessary. */
+        while (test_and_set_bit(0, &m_dev->hw_lock) != 0) {
+                /* Try and lock the HW */
+                /* Wait on the interrupt waitqueue */
+                ret = wait_event_interruptible_timeout(m_dev->queue,
+                        m_dev->int_cond || m_dev->ctx[m_dev->curr_ctx]->int_cond,
+                        msecs_to_jiffies(MFC_INT_TIMEOUT));
+
+                if (ret == 0) {
+                        mfc_err("Waiting for hardware to finish timed out\n");
+                        return -EIO;
+                }
+        }
+        return 0;
+}
+
+static int s5p_mfc_resume(struct device *dev)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct s5p_mfc_dev *m_dev = platform_get_drvdata(pdev);
+
+        if (m_dev->num_inst == 0)
+                return 0;
+        return s5p_mfc_wakeup(m_dev);
+}
+#endif
+
+#ifdef CONFIG_PM_RUNTIME
+static int s5p_mfc_runtime_suspend(struct device *dev)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct s5p_mfc_dev *m_dev = platform_get_drvdata(pdev);
+
+        atomic_set(&m_dev->pm.power, 0);
+        return 0;
+}
+
+static int s5p_mfc_runtime_resume(struct device *dev)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct s5p_mfc_dev *m_dev = platform_get_drvdata(pdev);
+        int pre_power;
+
+        if (!m_dev->alloc_ctx)
+                return 0;
+        pre_power = atomic_read(&m_dev->pm.power);
+        atomic_set(&m_dev->pm.power, 1);
+        return 0;
+}
+#endif
+
+/* Power management */
+static const struct dev_pm_ops s5p_mfc_pm_ops = {
+        SET_SYSTEM_SLEEP_PM_OPS(s5p_mfc_suspend, s5p_mfc_resume)
+        SET_RUNTIME_PM_OPS(s5p_mfc_runtime_suspend, s5p_mfc_runtime_resume,
+                           NULL)
+};
+
+static struct platform_driver s5p_mfc_pdrv = {
+        .probe  = s5p_mfc_probe,
+        .remove = __devexit_p(s5p_mfc_remove),
+        .driver = {
+                .name   = S5P_MFC_NAME,
+                .owner  = THIS_MODULE,
+                .pm     = &s5p_mfc_pm_ops
+        },
+};
+
+static char banner[] __initdata =
+                        "S5P MFC V4L2 Driver, (C) 2011 Samsung Electronics\n";
+
+static int __init s5p_mfc_init(void)
+{
+        int ret;
+
+        pr_info("%s", banner);
+        ret = platform_driver_register(&s5p_mfc_pdrv);
+        if (ret)
+                pr_err("Platform device registration failed.\n");
+        return ret;
+}
+
+static void __devexit s5p_mfc_exit(void)
+{
+        platform_driver_unregister(&s5p_mfc_pdrv);
+}
+
+module_init(s5p_mfc_init);
+module_exit(s5p_mfc_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Kamil Debski <k.debski@samsung.com>");
+MODULE_DESCRIPTION("Samsung S5P Multi Format Codec V4L2 driver");
+
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_cmd.c b/drivers/media/video/s5p-mfc/s5p_mfc_cmd.c
new file mode 100644
index 00000000000..f0665ed1a52
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_cmd.c
@@ -0,0 +1,120 @@
+/*
+ * linux/drivers/media/video/s5p-mfc/s5p_mfc_cmd.c
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.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 "regs-mfc.h"
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_debug.h"
+
+/* This function is used to send a command to the MFC */
+static int s5p_mfc_cmd_host2risc(struct s5p_mfc_dev *dev, int cmd,
+                                                struct s5p_mfc_cmd_args *args)
+{
+        int cur_cmd;
+        unsigned long timeout;
+
+        timeout = jiffies + msecs_to_jiffies(MFC_BW_TIMEOUT);
+        /* wait until host to risc command register becomes 'H2R_CMD_EMPTY' */
+        do {
+                if (time_after(jiffies, timeout)) {
+                        mfc_err("Timeout while waiting for hardware\n");
+                        return -EIO;
+                }
+                cur_cmd = mfc_read(dev, S5P_FIMV_HOST2RISC_CMD);
+        } while (cur_cmd != S5P_FIMV_H2R_CMD_EMPTY);
+        mfc_write(dev, args->arg[0], S5P_FIMV_HOST2RISC_ARG1);
+        mfc_write(dev, args->arg[1], S5P_FIMV_HOST2RISC_ARG2);
+        mfc_write(dev, args->arg[2], S5P_FIMV_HOST2RISC_ARG3);
+        mfc_write(dev, args->arg[3], S5P_FIMV_HOST2RISC_ARG4);
+        /* Issue the command */
+        mfc_write(dev, cmd, S5P_FIMV_HOST2RISC_CMD);
+        return 0;
+}
+
+/* Initialize the MFC */
+int s5p_mfc_sys_init_cmd(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;
+        return s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_SYS_INIT, &h2r_args);
+}
+
+/* Suspend the MFC hardware */
+int s5p_mfc_sleep_cmd(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_cmd_args h2r_args;
+
+        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+        return s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_SLEEP, &h2r_args);
+}
+
+/* Wake up the MFC hardware */
+int s5p_mfc_wakeup_cmd(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_cmd_args h2r_args;
+
+        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+        return s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_WAKEUP, &h2r_args);
+}
+
+
+int s5p_mfc_open_inst_cmd(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_cmd_args h2r_args;
+        int ret;
+
+        /* Preparing decoding - getting instance number */
+        mfc_debug(2, "Getting instance number (codec: %d)\n", ctx->codec_mode);
+        dev->curr_ctx = ctx->num;
+        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+        h2r_args.arg[0] = ctx->codec_mode;
+        h2r_args.arg[1] = 0; /* no crc & no pixelcache */
+        h2r_args.arg[2] = ctx->ctx_ofs;
+        h2r_args.arg[3] = ctx->ctx_size;
+        ret = s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_OPEN_INSTANCE,
+                                                                &h2r_args);
+        if (ret) {
+                mfc_err("Failed to create a new instance\n");
+                ctx->state = MFCINST_ERROR;
+        }
+        return ret;
+}
+
+int s5p_mfc_close_inst_cmd(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_cmd_args h2r_args;
+        int ret;
+
+        if (ctx->state == MFCINST_FREE) {
+                mfc_err("Instance already returned\n");
+                ctx->state = MFCINST_ERROR;
+                return -EINVAL;
+        }
+        /* Closing decoding instance  */
+        mfc_debug(2, "Returning instance number %d\n", ctx->inst_no);
+        dev->curr_ctx = ctx->num;
+        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+        h2r_args.arg[0] = ctx->inst_no;
+        ret = s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_CLOSE_INSTANCE,
+                                                                &h2r_args);
+        if (ret) {
+                mfc_err("Failed to return an instance\n");
+                ctx->state = MFCINST_ERROR;
+                return -EINVAL;
+        }
+        return 0;
+}
+
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_cmd.h b/drivers/media/video/s5p-mfc/s5p_mfc_cmd.h
new file mode 100644
index 00000000000..5ceebfe6131
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_cmd.h
@@ -0,0 +1,30 @@
+/*
+ * linux/drivers/media/video/s5p-mfc/s5p_mfc_cmd.h
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.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 S5P_MFC_CMD_H_
+#define S5P_MFC_CMD_H_
+
+#include "s5p_mfc_common.h"
+
+#define MAX_H2R_ARG     4
+
+struct s5p_mfc_cmd_args {
+        unsigned int    arg[MAX_H2R_ARG];
+};
+
+int s5p_mfc_sys_init_cmd(struct s5p_mfc_dev *dev);
+int s5p_mfc_sleep_cmd(struct s5p_mfc_dev *dev);
+int s5p_mfc_wakeup_cmd(struct s5p_mfc_dev *dev);
+int s5p_mfc_open_inst_cmd(struct s5p_mfc_ctx *ctx);
+int s5p_mfc_close_inst_cmd(struct s5p_mfc_ctx *ctx);
+
+#endif /* S5P_MFC_CMD_H_ */
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_common.h b/drivers/media/video/s5p-mfc/s5p_mfc_common.h
new file mode 100644
index 00000000000..91146fa622e
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_common.h
@@ -0,0 +1,572 @@
+/*
+ * Samsung S5P Multi Format Codec v 5.0
+ *
+ * This file contains definitions of enums and structs used by the codec
+ * driver.
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ * Kamil Debski, <k.debski@samsung.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 S5P_MFC_COMMON_H_
+#define S5P_MFC_COMMON_H_
+
+#include "regs-mfc.h"
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-core.h>
+
+/* Definitions related to MFC memory */
+
+/* Offset base used to differentiate between CAPTURE and OUTPUT
+*  while mmaping */
+#define DST_QUEUE_OFF_BASE      (TASK_SIZE / 2)
+
+/* Offset used by the hardware to store addresses */
+#define MFC_OFFSET_SHIFT        11
+
+#define FIRMWARE_ALIGN          0x20000         /* 128KB */
+#define MFC_H264_CTX_BUF_SIZE   0x96000         /* 600KB per H264 instance */
+#define MFC_CTX_BUF_SIZE        0x2800          /* 10KB per instance */
+#define DESC_BUF_SIZE           0x20000         /* 128KB for DESC buffer */
+#define SHARED_BUF_SIZE         0x2000          /* 8KB for shared buffer */
+
+#define DEF_CPB_SIZE            0x40000         /* 512KB */
+
+#define MFC_BANK1_ALLOC_CTX     0
+#define MFC_BANK2_ALLOC_CTX     1
+
+#define MFC_BANK1_ALIGN_ORDER   13
+#define MFC_BANK2_ALIGN_ORDER   13
+#define MFC_BASE_ALIGN_ORDER    17
+
+#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
+#define MFC_NUM_CONTEXTS        4
+/* Interrupt timeout */
+#define MFC_INT_TIMEOUT         2000
+/* Busy wait timeout */
+#define MFC_BW_TIMEOUT          500
+/* Watchdog interval */
+#define MFC_WATCHDOG_INTERVAL   1000
+/* After how many executions watchdog should assume lock up */
+#define MFC_WATCHDOG_CNT        10
+#define MFC_NO_INSTANCE_SET     -1
+#define MFC_ENC_CAP_PLANE_COUNT 1
+#define MFC_ENC_OUT_PLANE_COUNT 2
+#define STUFF_BYTE              4
+#define MFC_MAX_CTRLS           64
+
+#define mfc_read(dev, offset)           readl(dev->regs_base + (offset))
+#define mfc_write(dev, data, offset)    writel((data), dev->regs_base + \
+                                                                (offset))
+
+/**
+ * enum s5p_mfc_fmt_type - type of the pixelformat
+ */
+enum s5p_mfc_fmt_type {
+        MFC_FMT_DEC,
+        MFC_FMT_ENC,
+        MFC_FMT_RAW,
+};
+
+/**
+ * enum s5p_mfc_node_type - The type of an MFC device node.
+ */
+enum s5p_mfc_node_type {
+        MFCNODE_INVALID = -1,
+        MFCNODE_DECODER = 0,
+        MFCNODE_ENCODER = 1,
+};
+
+/**
+ * enum s5p_mfc_inst_type - The type of an MFC instance.
+ */
+enum s5p_mfc_inst_type {
+        MFCINST_INVALID,
+        MFCINST_DECODER,
+        MFCINST_ENCODER,
+};
+
+/**
+ * enum s5p_mfc_inst_state - The state of an MFC instance.
+ */
+enum s5p_mfc_inst_state {
+        MFCINST_FREE = 0,
+        MFCINST_INIT = 100,
+        MFCINST_GOT_INST,
+        MFCINST_HEAD_PARSED,
+        MFCINST_BUFS_SET,
+        MFCINST_RUNNING,
+        MFCINST_FINISHING,
+        MFCINST_FINISHED,
+        MFCINST_RETURN_INST,
+        MFCINST_ERROR,
+        MFCINST_ABORT,
+        MFCINST_RES_CHANGE_INIT,
+        MFCINST_RES_CHANGE_FLUSH,
+        MFCINST_RES_CHANGE_END,
+};
+
+/**
+ * enum s5p_mfc_queue_state - The state of buffer queue.
+ */
+enum s5p_mfc_queue_state {
+        QUEUE_FREE,
+        QUEUE_BUFS_REQUESTED,
+        QUEUE_BUFS_QUERIED,
+        QUEUE_BUFS_MMAPED,
+};
+
+/**
+ * enum s5p_mfc_decode_arg - type of frame decoding
+ */
+enum s5p_mfc_decode_arg {
+        MFC_DEC_FRAME,
+        MFC_DEC_LAST_FRAME,
+        MFC_DEC_RES_CHANGE,
+};
+
+struct s5p_mfc_ctx;
+
+/**
+ * struct s5p_mfc_buf - MFC buffer
+ */
+struct s5p_mfc_buf {
+        struct list_head list;
+        struct vb2_buffer *b;
+        union {
+                struct {
+                        size_t luma;
+                        size_t chroma;
+                } raw;
+                size_t stream;
+        } cookie;
+        int used;
+};
+
+/**
+ * struct s5p_mfc_pm - power management data structure
+ */
+struct s5p_mfc_pm {
+        struct clk      *clock;
+        struct clk      *clock_gate;
+        atomic_t        power;
+        struct device   *device;
+};
+
+/**
+ * struct s5p_mfc_dev - The struct containing driver internal parameters.
+ *
+ * @v4l2_dev:           v4l2_device
+ * @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)
+ * @regs_base:          base address of the MFC hw registers
+ * @irq:                irq resource
+ * @mfc_mem:            MFC registers memory resource
+ * @dec_ctrl_handler:   control framework handler for decoding
+ * @enc_ctrl_handler:   control framework handler for encoding
+ * @pm:                 power management control
+ * @num_inst:           couter of active MFC instances
+ * @irqlock:            lock for operations on videobuf2 queues
+ * @condlock:           lock for changing/checking if a context is ready to be
+ *                      processed
+ * @mfc_mutex:          lock for video_device
+ * @int_cond:           variable used by the waitqueue
+ * @int_type:           type of last interrupt
+ * @int_err:            error number for last interrupt
+ * @queue:              waitqueue for waiting for completion of device commands
+ * @fw_size:            size of firmware
+ * @bank1:              address of the beggining of bank 1 memory
+ * @bank2:              address of the beggining of bank 2 memory
+ * @hw_lock:            used for hardware locking
+ * @ctx:                array of driver contexts
+ * @curr_ctx:           number of the currently running context
+ * @ctx_work_bits:      used to mark which contexts are waiting for hardware
+ * @watchdog_cnt:       counter for the watchdog
+ * @watchdog_workqueue: workqueue for the watchdog
+ * @watchdog_work:      worker for the watchdog
+ * @alloc_ctx:          videobuf2 allocator contexts for two memory banks
+ * @enter_suspend:      flag set when entering suspend
+ *
+ */
+struct s5p_mfc_dev {
+        struct v4l2_device      v4l2_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;
+        void __iomem            *regs_base;
+        int                     irq;
+        struct resource         *mfc_mem;
+        struct v4l2_ctrl_handler dec_ctrl_handler;
+        struct v4l2_ctrl_handler enc_ctrl_handler;
+        struct s5p_mfc_pm       pm;
+        int num_inst;
+        spinlock_t irqlock;     /* lock when operating on videobuf2 queues */
+        spinlock_t condlock;    /* lock when changing/checking if a context is
+                                        ready to be processed */
+        struct mutex mfc_mutex; /* video_device lock */
+        int int_cond;
+        int int_type;
+        unsigned int int_err;
+        wait_queue_head_t queue;
+        size_t fw_size;
+        size_t bank1;
+        size_t bank2;
+        unsigned long hw_lock;
+        struct s5p_mfc_ctx *ctx[MFC_NUM_CONTEXTS];
+        int curr_ctx;
+        unsigned long ctx_work_bits;
+        atomic_t watchdog_cnt;
+        struct timer_list watchdog_timer;
+        struct workqueue_struct *watchdog_workqueue;
+        struct work_struct watchdog_work;
+        void *alloc_ctx[2];
+        unsigned long enter_suspend;
+};
+
+/**
+ * struct s5p_mfc_h264_enc_params - encoding parameters for h264
+ */
+struct s5p_mfc_h264_enc_params {
+        enum v4l2_mpeg_video_h264_profile profile;
+        enum v4l2_mpeg_video_h264_loop_filter_mode loop_filter_mode;
+        s8 loop_filter_alpha;
+        s8 loop_filter_beta;
+        enum v4l2_mpeg_video_h264_entropy_mode entropy_mode;
+        u8 max_ref_pic;
+        u8 num_ref_pic_4p;
+        int _8x8_transform;
+        int rc_mb;
+        int rc_mb_dark;
+        int rc_mb_smooth;
+        int rc_mb_static;
+        int rc_mb_activity;
+        int vui_sar;
+        u8 vui_sar_idc;
+        u16 vui_ext_sar_width;
+        u16 vui_ext_sar_height;
+        int open_gop;
+        u16 open_gop_size;
+        u8 rc_frame_qp;
+        u8 rc_min_qp;
+        u8 rc_max_qp;
+        u8 rc_p_frame_qp;
+        u8 rc_b_frame_qp;
+        enum v4l2_mpeg_video_h264_level level_v4l2;
+        int level;
+        u16 cpb_size;
+};
+
+/**
+ * struct s5p_mfc_mpeg4_enc_params - encoding parameters for h263 and mpeg4
+ */
+struct s5p_mfc_mpeg4_enc_params {
+        /* MPEG4 Only */
+        enum v4l2_mpeg_video_mpeg4_profile profile;
+        int quarter_pixel;
+        /* Common for MPEG4, H263 */
+        u16 vop_time_res;
+        u16 vop_frm_delta;
+        u8 rc_frame_qp;
+        u8 rc_min_qp;
+        u8 rc_max_qp;
+        u8 rc_p_frame_qp;
+        u8 rc_b_frame_qp;
+        enum v4l2_mpeg_video_mpeg4_level level_v4l2;
+        int level;
+};
+
+/**
+ * struct s5p_mfc_enc_params - general encoding parameters
+ */
+struct s5p_mfc_enc_params {
+        u16 width;
+        u16 height;
+
+        u16 gop_size;
+        enum v4l2_mpeg_video_multi_slice_mode slice_mode;
+        u16 slice_mb;
+        u32 slice_bit;
+        u16 intra_refresh_mb;
+        int pad;
+        u8 pad_luma;
+        u8 pad_cb;
+        u8 pad_cr;
+        int rc_frame;
+        u32 rc_bitrate;
+        u16 rc_reaction_coeff;
+        u16 vbv_size;
+
+        enum v4l2_mpeg_video_header_mode seq_hdr_mode;
+        enum v4l2_mpeg_mfc51_video_frame_skip_mode frame_skip_mode;
+        int fixed_target_bit;
+
+        u8 num_b_frame;
+        u32 rc_framerate_num;
+        u32 rc_framerate_denom;
+        int interlace;
+
+        union {
+                struct s5p_mfc_h264_enc_params h264;
+                struct s5p_mfc_mpeg4_enc_params mpeg4;
+        } codec;
+
+};
+
+/**
+ * struct s5p_mfc_codec_ops - codec ops, used by encoding
+ */
+struct s5p_mfc_codec_ops {
+        /* initialization routines */
+        int (*pre_seq_start) (struct s5p_mfc_ctx *ctx);
+        int (*post_seq_start) (struct s5p_mfc_ctx *ctx);
+        /* execution routines */
+        int (*pre_frame_start) (struct s5p_mfc_ctx *ctx);
+        int (*post_frame_start) (struct s5p_mfc_ctx *ctx);
+};
+
+#define call_cop(c, op, args...)                                \
+        (((c)->c_ops->op) ?                                     \
+                ((c)->c_ops->op(args)) : 0)
+
+/**
+ * struct s5p_mfc_ctx - This struct contains the instance context
+ *
+ * @dev:                pointer to the s5p_mfc_dev of the device
+ * @fh:                 struct v4l2_fh
+ * @num:                number of the context that this structure describes
+ * @int_cond:           variable used by the waitqueue
+ * @int_type:           type of the last interrupt
+ * @int_err:            error number received from MFC hw in the interrupt
+ * @queue:              waitqueue that can be used to wait for this context to
+ *                      finish
+ * @src_fmt:            source pixelformat information
+ * @dst_fmt:            destination pixelformat information
+ * @vq_src:             vb2 queue for source buffers
+ * @vq_dst:             vb2 queue for destination buffers
+ * @src_queue:          driver internal queue for source buffers
+ * @dst_queue:          driver internal queue for destination buffers
+ * @src_queue_cnt:      number of buffers queued on the source internal queue
+ * @dst_queue_cnt:      number of buffers queued on the dest internal queue
+ * @type:               type of the instance - decoder or encoder
+ * @state:              state of the context
+ * @inst_no:            number of hw instance associated with the context
+ * @img_width:          width of the image that is decoded or encoded
+ * @img_height:         height of the image that is decoded or encoded
+ * @buf_width:          width of the buffer for processed image
+ * @buf_height:         height of the buffer for processed image
+ * @luma_size:          size of a luma plane
+ * @chroma_size:        size of a chroma plane
+ * @mv_size:            size of a motion vectors buffer
+ * @consumed_stream:    number of bytes that have been used so far from the
+ *                      decoding buffer
+ * @dpb_flush_flag:     flag used to indicate that a DPB buffers are being
+ *                      flushed
+ * @bank1_buf:          handle to memory allocated for temporary buffers from
+ *                      memory bank 1
+ * @bank1_phys:         address of the temporary buffers from memory bank 1
+ * @bank1_size:         size of the memory allocated for temporary buffers from
+ *                      memory bank 1
+ * @bank2_buf:          handle to memory allocated for temporary buffers from
+ *                      memory bank 2
+ * @bank2_phys:         address of the temporary buffers from memory bank 2
+ * @bank2_size:         size of the memory allocated for temporary buffers from
+ *                      memory bank 2
+ * @capture_state:      state of the capture buffers queue
+ * @output_state:       state of the output buffers queue
+ * @src_bufs:           information on allocated source buffers
+ * @dst_bufs:           information on allocated destination buffers
+ * @sequence:           counter for the sequence number for v4l2
+ * @dec_dst_flag:       flags for buffers queued in the hardware
+ * @dec_src_buf_size:   size of the buffer for source buffers in decoding
+ * @codec_mode:         number of codec mode used by MFC hw
+ * @slice_interface:    slice interface flag
+ * @loop_filter_mpeg4:  loop filter for MPEG4 flag
+ * @display_delay:      value of the display delay for H264
+ * @display_delay_enable:       display delay for H264 enable flag
+ * @after_packed_pb:    flag used to track buffer when stream is in
+ *                      Packed PB format
+ * @dpb_count:          count of the DPB buffers required by MFC hw
+ * @total_dpb_count:    count of DPB buffers with additional buffers
+ *                      requested by the application
+ * @ctx_buf:            handle to the memory associated with this context
+ * @ctx_phys:           address of the memory associated with this context
+ * @ctx_size:           size of the memory associated with this context
+ * @desc_buf:           description buffer for decoding handle
+ * @desc_phys:          description buffer for decoding address
+ * @shm_alloc:          handle for the shared memory buffer
+ * @shm:                virtual address for the shared memory buffer
+ * @shm_ofs:            address offset for shared memory
+ * @enc_params:         encoding parameters for MFC
+ * @enc_dst_buf_size:   size of the buffers for encoder output
+ * @frame_type:         used to force the type of the next encoded frame
+ * @ref_queue:          list of the reference buffers for encoding
+ * @ref_queue_cnt:      number of the buffers in the reference list
+ * @c_ops:              ops for encoding
+ * @ctrls:              array of controls, used when adding controls to the
+ *                      v4l2 control framework
+ * @ctrl_handler:       handler for v4l2 framework
+ */
+struct s5p_mfc_ctx {
+        struct s5p_mfc_dev *dev;
+        struct v4l2_fh fh;
+
+        int num;
+
+        int int_cond;
+        int int_type;
+        unsigned int int_err;
+        wait_queue_head_t queue;
+
+        struct s5p_mfc_fmt *src_fmt;
+        struct s5p_mfc_fmt *dst_fmt;
+
+        struct vb2_queue vq_src;
+        struct vb2_queue vq_dst;
+
+        struct list_head src_queue;
+        struct list_head dst_queue;
+
+        unsigned int src_queue_cnt;
+        unsigned int dst_queue_cnt;
+
+        enum s5p_mfc_inst_type type;
+        enum s5p_mfc_inst_state state;
+        int inst_no;
+
+        /* Image parameters */
+        int img_width;
+        int img_height;
+        int buf_width;
+        int buf_height;
+
+        int luma_size;
+        int chroma_size;
+        int mv_size;
+
+        unsigned long consumed_stream;
+
+        unsigned int dpb_flush_flag;
+
+        /* Buffers */
+        void *bank1_buf;
+        size_t bank1_phys;
+        size_t bank1_size;
+
+        void *bank2_buf;
+        size_t bank2_phys;
+        size_t bank2_size;
+
+        enum s5p_mfc_queue_state capture_state;
+        enum s5p_mfc_queue_state output_state;
+
+        struct s5p_mfc_buf src_bufs[MFC_MAX_BUFFERS];
+        int src_bufs_cnt;
+        struct s5p_mfc_buf dst_bufs[MFC_MAX_BUFFERS];
+        int dst_bufs_cnt;
+
+        unsigned int sequence;
+        unsigned long dec_dst_flag;
+        size_t dec_src_buf_size;
+
+        /* Control values */
+        int codec_mode;
+        int slice_interface;
+        int loop_filter_mpeg4;
+        int display_delay;
+        int display_delay_enable;
+        int after_packed_pb;
+
+        int dpb_count;
+        int total_dpb_count;
+
+        /* Buffers */
+        void *ctx_buf;
+        size_t ctx_phys;
+        size_t ctx_ofs;
+        size_t ctx_size;
+
+        void *desc_buf;
+        size_t desc_phys;
+
+
+        void *shm_alloc;
+        void *shm;
+        size_t shm_ofs;
+
+        struct s5p_mfc_enc_params enc_params;
+
+        size_t enc_dst_buf_size;
+
+        enum v4l2_mpeg_mfc51_video_force_frame_type force_frame_type;
+
+        struct list_head ref_queue;
+        unsigned int ref_queue_cnt;
+
+        struct s5p_mfc_codec_ops *c_ops;
+
+        struct v4l2_ctrl *ctrls[MFC_MAX_CTRLS];
+        struct v4l2_ctrl_handler ctrl_handler;
+};
+
+/*
+ * struct s5p_mfc_fmt - structure used to store information about pixelformats
+ *                      used by the MFC
+ */
+struct s5p_mfc_fmt {
+        char *name;
+        u32 fourcc;
+        u32 codec_mode;
+        enum s5p_mfc_fmt_type type;
+        u32 num_planes;
+};
+
+/**
+ * struct mfc_control - structure used to store information about MFC controls
+ *                      it is used to initialize the control framework.
+ */
+struct mfc_control {
+        __u32                   id;
+        enum v4l2_ctrl_type     type;
+        __u8                    name[32];  /* Whatever */
+        __s32                   minimum;   /* Note signedness */
+        __s32                   maximum;
+        __s32                   step;
+        __u32                   menu_skip_mask;
+        __s32                   default_value;
+        __u32                   flags;
+        __u32                   reserved[2];
+        __u8                    is_volatile;
+};
+
+
+#define fh_to_ctx(__fh) container_of(__fh, struct s5p_mfc_ctx, fh)
+#define ctrl_to_ctx(__ctrl) \
+        container_of((__ctrl)->handler, struct s5p_mfc_ctx, ctrl_handler)
+
+#endif /* S5P_MFC_COMMON_H_ */
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.c b/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.c
new file mode 100644
index 00000000000..5f4da80051b
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.c
@@ -0,0 +1,343 @@
+/*
+ * linux/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.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/delay.h>
+#include <linux/err.h>
+#include <linux/firmware.h>
+#include <linux/jiffies.h>
+#include <linux/sched.h>
+#include "regs-mfc.h"
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_pm.h"
+
+static void *s5p_mfc_bitproc_buf;
+static size_t s5p_mfc_bitproc_phys;
+static unsigned char *s5p_mfc_bitproc_virt;
+
+/* Allocate and load firmware */
+int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev)
+{
+        struct firmware *fw_blob;
+        size_t bank2_base_phys;
+        void *b_base;
+        int err;
+
+        /* Firmare has to be present as a separate file or compiled
+         * into kernel. */
+        mfc_debug_enter();
+        err = request_firmware((const struct firmware **)&fw_blob,
+                                     "s5pc110-mfc.fw", dev->v4l2_dev.dev);
+        if (err != 0) {
+                mfc_err("Firmware is not present in the /lib/firmware directory nor compiled in kernel\n");
+                return -EINVAL;
+        }
+        dev->fw_size = ALIGN(fw_blob->size, FIRMWARE_ALIGN);
+        if (s5p_mfc_bitproc_buf) {
+                mfc_err("Attempting to allocate firmware when it seems that it is already loaded\n");
+                release_firmware(fw_blob);
+                return -ENOMEM;
+        }
+        s5p_mfc_bitproc_buf = vb2_dma_contig_memops.alloc(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], dev->fw_size);
+        if (IS_ERR(s5p_mfc_bitproc_buf)) {
+                s5p_mfc_bitproc_buf = 0;
+                mfc_err("Allocating bitprocessor buffer failed\n");
+                release_firmware(fw_blob);
+                return -ENOMEM;
+        }
+        s5p_mfc_bitproc_phys = s5p_mfc_mem_cookie(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], s5p_mfc_bitproc_buf);
+        if (s5p_mfc_bitproc_phys & ((1 << MFC_BASE_ALIGN_ORDER) - 1)) {
+                mfc_err("The base memory for bank 1 is not aligned to 128KB\n");
+                vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
+                s5p_mfc_bitproc_phys = 0;
+                s5p_mfc_bitproc_buf = 0;
+                release_firmware(fw_blob);
+                return -EIO;
+        }
+        s5p_mfc_bitproc_virt = vb2_dma_contig_memops.vaddr(s5p_mfc_bitproc_buf);
+        if (!s5p_mfc_bitproc_virt) {
+                mfc_err("Bitprocessor memory remap failed\n");
+                vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
+                s5p_mfc_bitproc_phys = 0;
+                s5p_mfc_bitproc_buf = 0;
+                release_firmware(fw_blob);
+                return -EIO;
+        }
+        dev->bank1 = s5p_mfc_bitproc_phys;
+        b_base = vb2_dma_contig_memops.alloc(
+                dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], 1 << MFC_BANK2_ALIGN_ORDER);
+        if (IS_ERR(b_base)) {
+                vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
+                s5p_mfc_bitproc_phys = 0;
+                s5p_mfc_bitproc_buf = 0;
+                mfc_err("Allocating bank2 base failed\n");
+        release_firmware(fw_blob);
+                return -ENOMEM;
+        }
+        bank2_base_phys = s5p_mfc_mem_cookie(
+                dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], b_base);
+        vb2_dma_contig_memops.put(b_base);
+        if (bank2_base_phys & ((1 << MFC_BASE_ALIGN_ORDER) - 1)) {
+                mfc_err("The base memory for bank 2 is not aligned to 128KB\n");
+                vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
+                s5p_mfc_bitproc_phys = 0;
+                s5p_mfc_bitproc_buf = 0;
+                release_firmware(fw_blob);
+                return -EIO;
+        }
+        dev->bank2 = bank2_base_phys;
+        memcpy(s5p_mfc_bitproc_virt, fw_blob->data, fw_blob->size);
+        wmb();
+        release_firmware(fw_blob);
+        mfc_debug_leave();
+        return 0;
+}
+
+/* Reload firmware to MFC */
+int s5p_mfc_reload_firmware(struct s5p_mfc_dev *dev)
+{
+        struct firmware *fw_blob;
+        int err;
+
+        /* Firmare has to be present as a separate file or compiled
+         * into kernel. */
+        mfc_debug_enter();
+        err = request_firmware((const struct firmware **)&fw_blob,
+                                     "s5pc110-mfc.fw", dev->v4l2_dev.dev);
+        if (err != 0) {
+                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) {
+                mfc_err("MFC firmware is too big to be loaded\n");
+                release_firmware(fw_blob);
+                return -ENOMEM;
+        }
+        if (s5p_mfc_bitproc_buf == 0 || s5p_mfc_bitproc_phys == 0) {
+                mfc_err("MFC firmware is not allocated or was not mapped correctly\n");
+                release_firmware(fw_blob);
+                return -EINVAL;
+        }
+        memcpy(s5p_mfc_bitproc_virt, fw_blob->data, fw_blob->size);
+        wmb();
+        release_firmware(fw_blob);
+        mfc_debug_leave();
+        return 0;
+}
+
+/* Release firmware memory */
+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 (!s5p_mfc_bitproc_buf)
+                return -EINVAL;
+        vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
+        s5p_mfc_bitproc_virt =  0;
+        s5p_mfc_bitproc_phys = 0;
+        s5p_mfc_bitproc_buf = 0;
+        return 0;
+}
+
+/* Reset the device */
+int s5p_mfc_reset(struct s5p_mfc_dev *dev)
+{
+        unsigned int mc_status;
+        unsigned long timeout;
+
+        mfc_debug_enter();
+        /* Stop procedure */
+        /*  reset RISC */
+        mfc_write(dev, 0x3f6, S5P_FIMV_SW_RESET);
+        /*  All reset except for MC */
+        mfc_write(dev, 0x3e2, S5P_FIMV_SW_RESET);
+        mdelay(10);
+
+        timeout = jiffies + msecs_to_jiffies(MFC_BW_TIMEOUT);
+        /* Check MC status */
+        do {
+                if (time_after(jiffies, timeout)) {
+                        mfc_err("Timeout while resetting MFC\n");
+                        return -EIO;
+                }
+
+                mc_status = mfc_read(dev, S5P_FIMV_MC_STATUS);
+
+        } while (mc_status & 0x3);
+
+        mfc_write(dev, 0x0, S5P_FIMV_SW_RESET);
+        mfc_write(dev, 0x3fe, S5P_FIMV_SW_RESET);
+        mfc_debug_leave();
+        return 0;
+}
+
+static inline void s5p_mfc_init_memctrl(struct s5p_mfc_dev *dev)
+{
+        mfc_write(dev, dev->bank1, S5P_FIMV_MC_DRAMBASE_ADR_A);
+        mfc_write(dev, dev->bank2, S5P_FIMV_MC_DRAMBASE_ADR_B);
+        mfc_debug(2, "Bank1: %08x, Bank2: %08x\n", dev->bank1, dev->bank2);
+}
+
+static inline void s5p_mfc_clear_cmds(struct s5p_mfc_dev *dev)
+{
+        mfc_write(dev, 0xffffffff, S5P_FIMV_SI_CH0_INST_ID);
+        mfc_write(dev, 0xffffffff, S5P_FIMV_SI_CH1_INST_ID);
+        mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD);
+        mfc_write(dev, 0, S5P_FIMV_HOST2RISC_CMD);
+}
+
+/* Initialize hardware */
+int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
+{
+        unsigned int ver;
+        int ret;
+
+        mfc_debug_enter();
+        if (!s5p_mfc_bitproc_buf)
+                return -EINVAL;
+
+        /* 0. MFC reset */
+        mfc_debug(2, "MFC reset..\n");
+        s5p_mfc_clock_on();
+        ret = s5p_mfc_reset(dev);
+        if (ret) {
+                mfc_err("Failed to reset MFC - timeout\n");
+                return ret;
+        }
+        mfc_debug(2, "Done MFC reset..\n");
+        /* 1. Set DRAM base Addr */
+        s5p_mfc_init_memctrl(dev);
+        /* 2. Initialize registers of channel I/F */
+        s5p_mfc_clear_cmds(dev);
+        /* 3. Release reset signal to the RISC */
+        s5p_mfc_clean_dev_int_flags(dev);
+        mfc_write(dev, 0x3ff, S5P_FIMV_SW_RESET);
+        mfc_debug(2, "Will now wait for completion of firmware transfer\n");
+        if (s5p_mfc_wait_for_done_dev(dev, S5P_FIMV_R2H_CMD_FW_STATUS_RET)) {
+                mfc_err("Failed to load firmware\n");
+                s5p_mfc_reset(dev);
+                s5p_mfc_clock_off();
+                return -EIO;
+        }
+        s5p_mfc_clean_dev_int_flags(dev);
+        /* 4. Initialize firmware */
+        ret = s5p_mfc_sys_init_cmd(dev);
+        if (ret) {
+                mfc_err("Failed to send command to MFC - timeout\n");
+                s5p_mfc_reset(dev);
+                s5p_mfc_clock_off();
+                return ret;
+        }
+        mfc_debug(2, "Ok, now will write a command to init the system\n");
+        if (s5p_mfc_wait_for_done_dev(dev, S5P_FIMV_R2H_CMD_SYS_INIT_RET)) {
+                mfc_err("Failed to load firmware\n");
+                s5p_mfc_reset(dev);
+                s5p_mfc_clock_off();
+                return -EIO;
+        }
+        dev->int_cond = 0;
+        if (dev->int_err != 0 || dev->int_type !=
+                                        S5P_FIMV_R2H_CMD_SYS_INIT_RET) {
+                /* Failure. */
+                mfc_err("Failed to init firmware - error: %d int: %d\n",
+                                                dev->int_err, dev->int_type);
+                s5p_mfc_reset(dev);
+                s5p_mfc_clock_off();
+                return -EIO;
+        }
+        ver = mfc_read(dev, S5P_FIMV_FW_VERSION);
+        mfc_debug(2, "MFC F/W version : %02xyy, %02xmm, %02xdd\n",
+                (ver >> 16) & 0xFF, (ver >> 8) & 0xFF, ver & 0xFF);
+        s5p_mfc_clock_off();
+        mfc_debug_leave();
+        return 0;
+}
+
+
+int s5p_mfc_sleep(struct s5p_mfc_dev *dev)
+{
+        int ret;
+
+        mfc_debug_enter();
+        s5p_mfc_clock_on();
+        s5p_mfc_clean_dev_int_flags(dev);
+        ret = s5p_mfc_sleep_cmd(dev);
+        if (ret) {
+                mfc_err("Failed to send command to MFC - timeout\n");
+                return ret;
+        }
+        if (s5p_mfc_wait_for_done_dev(dev, S5P_FIMV_R2H_CMD_SLEEP_RET)) {
+                mfc_err("Failed to sleep\n");
+                return -EIO;
+        }
+        s5p_mfc_clock_off();
+        dev->int_cond = 0;
+        if (dev->int_err != 0 || dev->int_type !=
+                                                S5P_FIMV_R2H_CMD_SLEEP_RET) {
+                /* Failure. */
+                mfc_err("Failed to sleep - error: %d int: %d\n", dev->int_err,
+                                                                dev->int_type);
+                return -EIO;
+        }
+        mfc_debug_leave();
+        return ret;
+}
+
+int s5p_mfc_wakeup(struct s5p_mfc_dev *dev)
+{
+        int ret;
+
+        mfc_debug_enter();
+        /* 0. MFC reset */
+        mfc_debug(2, "MFC reset..\n");
+        s5p_mfc_clock_on();
+        ret = s5p_mfc_reset(dev);
+        if (ret) {
+                mfc_err("Failed to reset MFC - timeout\n");
+                return ret;
+        }
+        mfc_debug(2, "Done MFC reset..\n");
+        /* 1. Set DRAM base Addr */
+        s5p_mfc_init_memctrl(dev);
+        /* 2. Initialize registers of channel I/F */
+        s5p_mfc_clear_cmds(dev);
+        s5p_mfc_clean_dev_int_flags(dev);
+        /* 3. Initialize firmware */
+        ret = s5p_mfc_wakeup_cmd(dev);
+        if (ret) {
+                mfc_err("Failed to send command to MFC - timeout\n");
+                return ret;
+        }
+        /* 4. Release reset signal to the RISC */
+        mfc_write(dev, 0x3ff, S5P_FIMV_SW_RESET);
+        mfc_debug(2, "Ok, now will write a command to wakeup the system\n");
+        if (s5p_mfc_wait_for_done_dev(dev, S5P_FIMV_R2H_CMD_WAKEUP_RET)) {
+                mfc_err("Failed to load firmware\n");
+                return -EIO;
+        }
+        s5p_mfc_clock_off();
+        dev->int_cond = 0;
+        if (dev->int_err != 0 || dev->int_type !=
+                                                S5P_FIMV_R2H_CMD_WAKEUP_RET) {
+                /* Failure. */
+                mfc_err("Failed to wakeup - error: %d int: %d\n", dev->int_err,
+                                                                dev->int_type);
+                return -EIO;
+        }
+        mfc_debug_leave();
+        return 0;
+}
+
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.h b/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.h
new file mode 100644
index 00000000000..61dc23b7ee5
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.h
@@ -0,0 +1,29 @@
+/*
+ * linux/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.h
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.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 S5P_MFC_CTRL_H
+#define S5P_MFC_CTRL_H
+
+#include "s5p_mfc_common.h"
+
+int s5p_mfc_release_firmware(struct s5p_mfc_dev *dev);
+int s5p_mfc_alloc_and_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);
+
+int s5p_mfc_sleep(struct s5p_mfc_dev *dev);
+int s5p_mfc_wakeup(struct s5p_mfc_dev *dev);
+
+int s5p_mfc_reset(struct s5p_mfc_dev *dev);
+
+#endif /* S5P_MFC_CTRL_H */
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_debug.h b/drivers/media/video/s5p-mfc/s5p_mfc_debug.h
new file mode 100644
index 00000000000..ecb8616a492
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_debug.h
@@ -0,0 +1,48 @@
+/*
+ * drivers/media/video/samsung/mfc5/s5p_mfc_debug.h
+ *
+ * Header file for Samsung MFC (Multi Function Codec - FIMV) driver
+ * This file contains debug macros
+ *
+ * Kamil Debski, Copyright (c) 2011 Samsung Electronics
+ * http://www.samsung.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.
+ */
+
+#ifndef S5P_MFC_DEBUG_H_
+#define S5P_MFC_DEBUG_H_
+
+#define DEBUG
+
+#ifdef DEBUG
+extern int debug;
+
+#define mfc_debug(level, fmt, args...)                          \
+        do {                                                    \
+                if (debug >= level)                             \
+                        printk(KERN_DEBUG "%s:%d: " fmt,        \
+                                __func__, __LINE__, ##args);    \
+        } while (0)
+#else
+#define mfc_debug(level, fmt, args...)
+#endif
+
+#define mfc_debug_enter() mfc_debug(5, "enter")
+#define mfc_debug_leave() mfc_debug(5, "leave")
+
+#define mfc_err(fmt, args...)                           \
+        do {                                            \
+                printk(KERN_ERR "%s:%d: " fmt,          \
+                       __func__, __LINE__, ##args);     \
+        } while (0)
+
+#define mfc_info(fmt, args...)                          \
+        do {                                            \
+                printk(KERN_INFO "%s:%d: " fmt,         \
+                       __func__, __LINE__, ##args);     \
+        } while (0)
+
+#endif /* S5P_MFC_DEBUG_H_ */
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_dec.c b/drivers/media/video/s5p-mfc/s5p_mfc_dec.c
new file mode 100644
index 00000000000..b2c5052a9c4
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_dec.c
@@ -0,0 +1,1036 @@
+/*
+ * linux/drivers/media/video/s5p-mfc/s5p_mfc_dec.c
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ * Kamil Debski, <k.debski@samsung.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/clk.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/version.h>
+#include <linux/videodev2.h>
+#include <linux/workqueue.h>
+#include <media/v4l2-ctrls.h>
+#include <media/videobuf2-core.h>
+#include "regs-mfc.h"
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_dec.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_opr.h"
+#include "s5p_mfc_pm.h"
+#include "s5p_mfc_shm.h"
+
+static struct s5p_mfc_fmt formats[] = {
+        {
+                .name           = "4:2:0 2 Planes 64x32 Tiles",
+                .fourcc         = V4L2_PIX_FMT_NV12MT,
+                .codec_mode     = S5P_FIMV_CODEC_NONE,
+                .type           = MFC_FMT_RAW,
+                .num_planes     = 2,
+         },
+        {
+                .name = "4:2:0 2 Planes",
+                .fourcc = V4L2_PIX_FMT_NV12M,
+                .codec_mode = S5P_FIMV_CODEC_NONE,
+                .type = MFC_FMT_RAW,
+                .num_planes = 2,
+        },
+        {
+                .name = "H264 Encoded Stream",
+                .fourcc = V4L2_PIX_FMT_H264,
+                .codec_mode = S5P_FIMV_CODEC_H264_DEC,
+                .type = MFC_FMT_DEC,
+                .num_planes = 1,
+        },
+        {
+                .name = "H263 Encoded Stream",
+                .fourcc = V4L2_PIX_FMT_H263,
+                .codec_mode = S5P_FIMV_CODEC_H263_DEC,
+                .type = MFC_FMT_DEC,
+                .num_planes = 1,
+        },
+        {
+                .name = "MPEG1 Encoded Stream",
+                .fourcc = V4L2_PIX_FMT_MPEG1,
+                .codec_mode = S5P_FIMV_CODEC_MPEG2_DEC,
+                .type = MFC_FMT_DEC,
+                .num_planes = 1,
+        },
+        {
+                .name = "MPEG2 Encoded Stream",
+                .fourcc = V4L2_PIX_FMT_MPEG2,
+                .codec_mode = S5P_FIMV_CODEC_MPEG2_DEC,
+                .type = MFC_FMT_DEC,
+                .num_planes = 1,
+        },
+        {
+                .name = "MPEG4 Encoded Stream",
+                .fourcc = V4L2_PIX_FMT_MPEG4,
+                .codec_mode = S5P_FIMV_CODEC_MPEG4_DEC,
+                .type = MFC_FMT_DEC,
+                .num_planes = 1,
+        },
+        {
+                .name = "XviD Encoded Stream",
+                .fourcc = V4L2_PIX_FMT_XVID,
+                .codec_mode = S5P_FIMV_CODEC_MPEG4_DEC,
+                .type = MFC_FMT_DEC,
+                .num_planes = 1,
+        },
+        {
+                .name = "VC1 Encoded Stream",
+                .fourcc = V4L2_PIX_FMT_VC1_ANNEX_G,
+                .codec_mode = S5P_FIMV_CODEC_VC1_DEC,
+                .type = MFC_FMT_DEC,
+                .num_planes = 1,
+        },
+        {
+                .name = "VC1 RCV Encoded Stream",
+                .fourcc = V4L2_PIX_FMT_VC1_ANNEX_L,
+                .codec_mode = S5P_FIMV_CODEC_VC1RCV_DEC,
+                .type = MFC_FMT_DEC,
+                .num_planes = 1,
+        },
+};
+
+#define NUM_FORMATS ARRAY_SIZE(formats)
+
+/* Find selected format description */
+static struct s5p_mfc_fmt *find_format(struct v4l2_format *f, unsigned int t)
+{
+        unsigned int i;
+
+        for (i = 0; i < NUM_FORMATS; i++) {
+                if (formats[i].fourcc == f->fmt.pix_mp.pixelformat &&
+                    formats[i].type == t)
+                        return &formats[i];
+        }
+        return NULL;
+}
+
+static struct mfc_control controls[] = {
+        {
+                .id = V4L2_CID_MPEG_MFC51_VIDEO_DECODER_H264_DISPLAY_DELAY,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .name = "H264 Display Delay",
+                .minimum = 0,
+                .maximum = 16383,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_MFC51_VIDEO_DECODER_H264_DISPLAY_DELAY_ENABLE,
+                .type = V4L2_CTRL_TYPE_BOOLEAN,
+                .name = "H264 Display Delay Enable",
+                .minimum = 0,
+                .maximum = 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_DECODER_MPEG4_DEBLOCK_FILTER,
+                .type = V4L2_CTRL_TYPE_BOOLEAN,
+                .name = "Mpeg4 Loop Filter Enable",
+                .minimum = 0,
+                .maximum = 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_DECODER_SLICE_INTERFACE,
+                .type = V4L2_CTRL_TYPE_BOOLEAN,
+                .name = "Slice Interface Enable",
+                .minimum = 0,
+                .maximum = 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MIN_BUFFERS_FOR_CAPTURE,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .name = "Minimum number of cap bufs",
+                .minimum = 1,
+                .maximum = 32,
+                .step = 1,
+                .default_value = 1,
+                .is_volatile = 1,
+        },
+};
+
+#define NUM_CTRLS ARRAY_SIZE(controls)
+
+/* Check whether a context should be run on hardware */
+static int s5p_mfc_ctx_ready(struct s5p_mfc_ctx *ctx)
+{
+        /* Context is to parse header */
+        if (ctx->src_queue_cnt >= 1 && ctx->state == MFCINST_GOT_INST)
+                return 1;
+        /* Context is to decode a frame */
+        if (ctx->src_queue_cnt >= 1 &&
+            ctx->state == MFCINST_RUNNING &&
+            ctx->dst_queue_cnt >= ctx->dpb_count)
+                return 1;
+        /* Context is to return last frame */
+        if (ctx->state == MFCINST_FINISHING &&
+            ctx->dst_queue_cnt >= ctx->dpb_count)
+                return 1;
+        /* Context is to set buffers */
+        if (ctx->src_queue_cnt >= 1 &&
+            ctx->state == MFCINST_HEAD_PARSED &&
+            ctx->capture_state == QUEUE_BUFS_MMAPED)
+                return 1;
+        /* Resolution change */
+        if ((ctx->state == MFCINST_RES_CHANGE_INIT ||
+                ctx->state == MFCINST_RES_CHANGE_FLUSH) &&
+                ctx->dst_queue_cnt >= ctx->dpb_count)
+                return 1;
+        if (ctx->state == MFCINST_RES_CHANGE_END &&
+                ctx->src_queue_cnt >= 1)
+                return 1;
+        mfc_debug(2, "ctx is not ready\n");
+        return 0;
+}
+
+static struct s5p_mfc_codec_ops decoder_codec_ops = {
+        .pre_seq_start          = NULL,
+        .post_seq_start         = NULL,
+        .pre_frame_start        = NULL,
+        .post_frame_start       = NULL,
+};
+
+/* Query capabilities of the device */
+static int vidioc_querycap(struct file *file, void *priv,
+                           struct v4l2_capability *cap)
+{
+        struct s5p_mfc_dev *dev = video_drvdata(file);
+
+        strncpy(cap->driver, dev->plat_dev->name, sizeof(cap->driver) - 1);
+        strncpy(cap->card, dev->plat_dev->name, sizeof(cap->card) - 1);
+        cap->bus_info[0] = 0;
+        cap->version = KERNEL_VERSION(1, 0, 0);
+        cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT
+                                                    | V4L2_CAP_STREAMING;
+        return 0;
+}
+
+/* Enumerate format */
+static int vidioc_enum_fmt(struct v4l2_fmtdesc *f, bool mplane, bool out)
+{
+        struct s5p_mfc_fmt *fmt;
+        int i, j = 0;
+
+        for (i = 0; i < ARRAY_SIZE(formats); ++i) {
+                if (mplane && formats[i].num_planes == 1)
+                        continue;
+                else if (!mplane && formats[i].num_planes > 1)
+                        continue;
+                if (out && formats[i].type != MFC_FMT_DEC)
+                        continue;
+                else if (!out && formats[i].type != MFC_FMT_RAW)
+                        continue;
+
+                if (j == f->index)
+                        break;
+                ++j;
+        }
+        if (i == ARRAY_SIZE(formats))
+                return -EINVAL;
+        fmt = &formats[i];
+        strlcpy(f->description, fmt->name, sizeof(f->description));
+        f->pixelformat = fmt->fourcc;
+        return 0;
+}
+
+static int vidioc_enum_fmt_vid_cap(struct file *file, void *pirv,
+                                                        struct v4l2_fmtdesc *f)
+{
+        return vidioc_enum_fmt(f, false, false);
+}
+
+static int vidioc_enum_fmt_vid_cap_mplane(struct file *file, void *pirv,
+                                                        struct v4l2_fmtdesc *f)
+{
+        return vidioc_enum_fmt(f, true, false);
+}
+
+static int vidioc_enum_fmt_vid_out(struct file *file, void *prov,
+                                                        struct v4l2_fmtdesc *f)
+{
+        return vidioc_enum_fmt(f, false, true);
+}
+
+static int vidioc_enum_fmt_vid_out_mplane(struct file *file, void *prov,
+                                                        struct v4l2_fmtdesc *f)
+{
+        return vidioc_enum_fmt(f, true, true);
+}
+
+/* Get format */
+static int vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+        struct v4l2_pix_format_mplane *pix_mp;
+
+        mfc_debug_enter();
+        pix_mp = &f->fmt.pix_mp;
+        if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
+            (ctx->state == MFCINST_GOT_INST || ctx->state ==
+                                                MFCINST_RES_CHANGE_END)) {
+                /* If the MFC is parsing the header,
+                 * so wait until it is finished */
+                s5p_mfc_clean_ctx_int_flags(ctx);
+                s5p_mfc_wait_for_done_ctx(ctx, S5P_FIMV_R2H_CMD_SEQ_DONE_RET,
+                                                                        0);
+        }
+        if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
+            ctx->state >= MFCINST_HEAD_PARSED &&
+            ctx->state < MFCINST_ABORT) {
+                /* This is run on CAPTURE (decode output) */
+                /* Width and height are set to the dimensions
+                   of the movie, the buffer is bigger and
+                   further processing stages should crop to this
+                   rectangle. */
+                pix_mp->width = ctx->buf_width;
+                pix_mp->height = ctx->buf_height;
+                pix_mp->field = V4L2_FIELD_NONE;
+                pix_mp->num_planes = 2;
+                /* Set pixelformat to the format in which MFC
+                   outputs the decoded frame */
+                pix_mp->pixelformat = V4L2_PIX_FMT_NV12MT;
+                pix_mp->plane_fmt[0].bytesperline = ctx->buf_width;
+                pix_mp->plane_fmt[0].sizeimage = ctx->luma_size;
+                pix_mp->plane_fmt[1].bytesperline = ctx->buf_width;
+                pix_mp->plane_fmt[1].sizeimage = ctx->chroma_size;
+        } else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                /* This is run on OUTPUT
+                   The buffer contains compressed image
+                   so width and height have no meaning */
+                pix_mp->width = 0;
+                pix_mp->height = 0;
+                pix_mp->field = V4L2_FIELD_NONE;
+                pix_mp->plane_fmt[0].bytesperline = ctx->dec_src_buf_size;
+                pix_mp->plane_fmt[0].sizeimage = ctx->dec_src_buf_size;
+                pix_mp->pixelformat = ctx->src_fmt->fourcc;
+                pix_mp->num_planes = ctx->src_fmt->num_planes;
+        } else {
+                mfc_err("Format could not be read\n");
+                mfc_debug(2, "%s-- with error\n", __func__);
+                return -EINVAL;
+        }
+        mfc_debug_leave();
+        return 0;
+}
+
+/* Try format */
+static int vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+        struct s5p_mfc_fmt *fmt;
+
+        if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                mfc_err("This node supports decoding only\n");
+                return -EINVAL;
+        }
+        fmt = find_format(f, MFC_FMT_DEC);
+        if (!fmt) {
+                mfc_err("Unsupported format\n");
+                return -EINVAL;
+        }
+        if (fmt->type != MFC_FMT_DEC) {
+                mfc_err("\n");
+                return -EINVAL;
+        }
+        return 0;
+}
+
+/* Set format */
+static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+        struct s5p_mfc_dev *dev = video_drvdata(file);
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+        int ret = 0;
+        struct s5p_mfc_fmt *fmt;
+        struct v4l2_pix_format_mplane *pix_mp;
+
+        mfc_debug_enter();
+        ret = vidioc_try_fmt(file, priv, f);
+        pix_mp = &f->fmt.pix_mp;
+        if (ret)
+                return ret;
+        if (ctx->vq_src.streaming || ctx->vq_dst.streaming) {
+                v4l2_err(&dev->v4l2_dev, "%s queue busy\n", __func__);
+                ret = -EBUSY;
+                goto out;
+        }
+        fmt = find_format(f, MFC_FMT_DEC);
+        if (!fmt || fmt->codec_mode == S5P_FIMV_CODEC_NONE) {
+                mfc_err("Unknown codec\n");
+                ret = -EINVAL;
+                goto out;
+        }
+        if (fmt->type != MFC_FMT_DEC) {
+                mfc_err("Wrong format selected, you should choose "
+                                        "format for decoding\n");
+                ret = -EINVAL;
+                goto out;
+        }
+        ctx->src_fmt = fmt;
+        ctx->codec_mode = fmt->codec_mode;
+        mfc_debug(2, "The codec number is: %d\n", ctx->codec_mode);
+        pix_mp->height = 0;
+        pix_mp->width = 0;
+        if (pix_mp->plane_fmt[0].sizeimage)
+                ctx->dec_src_buf_size = pix_mp->plane_fmt[0].sizeimage;
+        else
+                pix_mp->plane_fmt[0].sizeimage = ctx->dec_src_buf_size =
+                                                                DEF_CPB_SIZE;
+        pix_mp->plane_fmt[0].bytesperline = 0;
+        ctx->state = MFCINST_INIT;
+out:
+        mfc_debug_leave();
+        return ret;
+}
+
+/* Reqeust buffers */
+static int vidioc_reqbufs(struct file *file, void *priv,
+                                          struct v4l2_requestbuffers *reqbufs)
+{
+        struct s5p_mfc_dev *dev = video_drvdata(file);
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+        int ret = 0;
+        unsigned long flags;
+
+        if (reqbufs->memory != V4L2_MEMORY_MMAP) {
+                mfc_err("Only V4L2_MEMORY_MAP is supported\n");
+                return -EINVAL;
+        }
+        if (reqbufs->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                /* Can only request buffers after an instance has been opened.*/
+                if (ctx->state == MFCINST_INIT) {
+                        ctx->src_bufs_cnt = 0;
+                        if (reqbufs->count == 0) {
+                                mfc_debug(2, "Freeing buffers\n");
+                                s5p_mfc_clock_on();
+                                ret = vb2_reqbufs(&ctx->vq_src, reqbufs);
+                                s5p_mfc_clock_off();
+                                return ret;
+                        }
+                        /* Decoding */
+                        if (ctx->output_state != QUEUE_FREE) {
+                                mfc_err("Bufs have already been requested\n");
+                                return -EINVAL;
+                        }
+                        s5p_mfc_clock_on();
+                        ret = vb2_reqbufs(&ctx->vq_src, reqbufs);
+                        s5p_mfc_clock_off();
+                        if (ret) {
+                                mfc_err("vb2_reqbufs on output failed\n");
+                                return ret;
+                        }
+                        mfc_debug(2, "vb2_reqbufs: %d\n", ret);
+                        ctx->output_state = QUEUE_BUFS_REQUESTED;
+                }
+        } else if (reqbufs->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                ctx->dst_bufs_cnt = 0;
+                if (reqbufs->count == 0) {
+                        mfc_debug(2, "Freeing buffers\n");
+                        s5p_mfc_clock_on();
+                        ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
+                        s5p_mfc_clock_off();
+                        return ret;
+                }
+                if (ctx->capture_state != QUEUE_FREE) {
+                        mfc_err("Bufs have already been requested\n");
+                        return -EINVAL;
+                }
+                ctx->capture_state = QUEUE_BUFS_REQUESTED;
+                s5p_mfc_clock_on();
+                ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
+                s5p_mfc_clock_off();
+                if (ret) {
+                        mfc_err("vb2_reqbufs on capture failed\n");
+                        return ret;
+                }
+                if (reqbufs->count < ctx->dpb_count) {
+                        mfc_err("Not enough buffers allocated\n");
+                        reqbufs->count = 0;
+                        s5p_mfc_clock_on();
+                        ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
+                        s5p_mfc_clock_off();
+                        return -ENOMEM;
+                }
+                ctx->total_dpb_count = reqbufs->count;
+                ret = s5p_mfc_alloc_codec_buffers(ctx);
+                if (ret) {
+                        mfc_err("Failed to allocate decoding buffers\n");
+                        reqbufs->count = 0;
+                        s5p_mfc_clock_on();
+                        ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
+                        s5p_mfc_clock_off();
+                        return -ENOMEM;
+                }
+                if (ctx->dst_bufs_cnt == ctx->total_dpb_count) {
+                        ctx->capture_state = QUEUE_BUFS_MMAPED;
+                } else {
+                        mfc_err("Not all buffers passed to buf_init\n");
+                        reqbufs->count = 0;
+                        s5p_mfc_clock_on();
+                        ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
+                        s5p_mfc_release_codec_buffers(ctx);
+                        s5p_mfc_clock_off();
+                        return -ENOMEM;
+                }
+                if (s5p_mfc_ctx_ready(ctx)) {
+                        spin_lock_irqsave(&dev->condlock, flags);
+                        set_bit(ctx->num, &dev->ctx_work_bits);
+                        spin_unlock_irqrestore(&dev->condlock, flags);
+                }
+                s5p_mfc_try_run(dev);
+                s5p_mfc_wait_for_done_ctx(ctx,
+                                         S5P_FIMV_R2H_CMD_INIT_BUFFERS_RET, 0);
+        }
+        return ret;
+}
+
+/* Query buffer */
+static int vidioc_querybuf(struct file *file, void *priv,
+                                                   struct v4l2_buffer *buf)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+        int ret;
+        int i;
+
+        if (buf->memory != V4L2_MEMORY_MMAP) {
+                mfc_err("Only mmaped buffers can be used\n");
+                return -EINVAL;
+        }
+        mfc_debug(2, "State: %d, buf->type: %d\n", ctx->state, buf->type);
+        if (ctx->state == MFCINST_INIT &&
+                        buf->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                ret = vb2_querybuf(&ctx->vq_src, buf);
+        } else if (ctx->state == MFCINST_RUNNING &&
+                        buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                ret = vb2_querybuf(&ctx->vq_dst, buf);
+                for (i = 0; i < buf->length; i++)
+                        buf->m.planes[i].m.mem_offset += DST_QUEUE_OFF_BASE;
+        } else {
+                mfc_err("vidioc_querybuf called in an inappropriate state\n");
+                ret = -EINVAL;
+        }
+        mfc_debug_leave();
+        return ret;
+}
+
+/* Queue a buffer */
+static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+        if (ctx->state == MFCINST_ERROR) {
+                mfc_err("Call on QBUF after unrecoverable error\n");
+                return -EIO;
+        }
+        if (buf->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+                return vb2_qbuf(&ctx->vq_src, buf);
+        else if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+                return vb2_qbuf(&ctx->vq_dst, buf);
+        return -EINVAL;
+}
+
+/* Dequeue a buffer */
+static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+        if (ctx->state == MFCINST_ERROR) {
+                mfc_err("Call on DQBUF after unrecoverable error\n");
+                return -EIO;
+        }
+        if (buf->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+                return vb2_dqbuf(&ctx->vq_src, buf, file->f_flags & O_NONBLOCK);
+        else if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+                return vb2_dqbuf(&ctx->vq_dst, buf, file->f_flags & O_NONBLOCK);
+        return -EINVAL;
+}
+
+/* Stream on */
+static int vidioc_streamon(struct file *file, void *priv,
+                           enum v4l2_buf_type type)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        int ret = -EINVAL;
+
+        mfc_debug_enter();
+        if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+
+                if (ctx->state == MFCINST_INIT) {
+                        ctx->dst_bufs_cnt = 0;
+                        ctx->src_bufs_cnt = 0;
+                        ctx->capture_state = QUEUE_FREE;
+                        ctx->output_state = QUEUE_FREE;
+                        s5p_mfc_alloc_instance_buffer(ctx);
+                        s5p_mfc_alloc_dec_temp_buffers(ctx);
+                        spin_lock_irqsave(&dev->condlock, flags);
+                        set_bit(ctx->num, &dev->ctx_work_bits);
+                        spin_unlock_irqrestore(&dev->condlock, flags);
+                        s5p_mfc_clean_ctx_int_flags(ctx);
+                        s5p_mfc_try_run(dev);
+
+                        if (s5p_mfc_wait_for_done_ctx(ctx,
+                                S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET, 0)) {
+                                /* Error or timeout */
+                                mfc_err("Error getting instance from hardware\n");
+                                s5p_mfc_release_instance_buffer(ctx);
+                                s5p_mfc_release_dec_desc_buffer(ctx);
+                                return -EIO;
+                        }
+                        mfc_debug(2, "Got instance number: %d\n", ctx->inst_no);
+                }
+                ret = vb2_streamon(&ctx->vq_src, type);
+                }
+        else if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+                ret = vb2_streamon(&ctx->vq_dst, type);
+        mfc_debug_leave();
+        return ret;
+}
+
+/* Stream off, which equals to a pause */
+static int vidioc_streamoff(struct file *file, void *priv,
+                            enum v4l2_buf_type type)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+        if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+                return vb2_streamoff(&ctx->vq_src, type);
+        else if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+                return vb2_streamoff(&ctx->vq_dst, type);
+        return -EINVAL;
+}
+
+/* Set controls - v4l2 control framework */
+static int s5p_mfc_dec_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct s5p_mfc_ctx *ctx = ctrl_to_ctx(ctrl);
+
+        switch (ctrl->id) {
+        case V4L2_CID_MPEG_MFC51_VIDEO_DECODER_H264_DISPLAY_DELAY:
+                ctx->loop_filter_mpeg4 = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_MFC51_VIDEO_DECODER_H264_DISPLAY_DELAY_ENABLE:
+                ctx->display_delay_enable = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_DECODER_MPEG4_DEBLOCK_FILTER:
+                ctx->display_delay = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_DECODER_SLICE_INTERFACE:
+                ctx->slice_interface = ctrl->val;
+                break;
+        default:
+                mfc_err("Invalid control 0x%08x\n", ctrl->id);
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static int s5p_mfc_dec_g_v_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct s5p_mfc_ctx *ctx = ctrl_to_ctx(ctrl);
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        switch (ctrl->id) {
+        case V4L2_CID_MIN_BUFFERS_FOR_CAPTURE:
+                if (ctx->state >= MFCINST_HEAD_PARSED &&
+                    ctx->state < MFCINST_ABORT) {
+                        ctrl->val = ctx->dpb_count;
+                        break;
+                } else if (ctx->state != MFCINST_INIT) {
+                        v4l2_err(&dev->v4l2_dev, "Decoding not initialised\n");
+                        return -EINVAL;
+                }
+                /* Should wait for the header to be parsed */
+                s5p_mfc_clean_ctx_int_flags(ctx);
+                s5p_mfc_wait_for_done_ctx(ctx,
+                                S5P_FIMV_R2H_CMD_SEQ_DONE_RET, 0);
+                if (ctx->state >= MFCINST_HEAD_PARSED &&
+                    ctx->state < MFCINST_ABORT) {
+                        ctrl->val = ctx->dpb_count;
+                } else {
+                        v4l2_err(&dev->v4l2_dev, "Decoding not initialised\n");
+                        return -EINVAL;
+                }
+                break;
+        }
+        return 0;
+}
+
+
+static const struct v4l2_ctrl_ops s5p_mfc_dec_ctrl_ops = {
+        .s_ctrl = s5p_mfc_dec_s_ctrl,
+        .g_volatile_ctrl = s5p_mfc_dec_g_v_ctrl,
+};
+
+/* Get cropping information */
+static int vidioc_g_crop(struct file *file, void *priv,
+                struct v4l2_crop *cr)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+        u32 left, right, top, bottom;
+
+        if (ctx->state != MFCINST_HEAD_PARSED &&
+        ctx->state != MFCINST_RUNNING && ctx->state != MFCINST_FINISHING
+                                        && ctx->state != MFCINST_FINISHED) {
+                        mfc_err("Cannont set crop\n");
+                        return -EINVAL;
+                }
+        if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_H264) {
+                left = s5p_mfc_read_shm(ctx, CROP_INFO_H);
+                right = left >> S5P_FIMV_SHARED_CROP_RIGHT_SHIFT;
+                left = left & S5P_FIMV_SHARED_CROP_LEFT_MASK;
+                top = s5p_mfc_read_shm(ctx, CROP_INFO_V);
+                bottom = top >> S5P_FIMV_SHARED_CROP_BOTTOM_SHIFT;
+                top = top & S5P_FIMV_SHARED_CROP_TOP_MASK;
+                cr->c.left = left;
+                cr->c.top = top;
+                cr->c.width = ctx->img_width - left - right;
+                cr->c.height = ctx->img_height - top - bottom;
+                mfc_debug(2, "Cropping info [h264]: l=%d t=%d "
+                        "w=%d h=%d (r=%d b=%d fw=%d fh=%d\n", left, top,
+                        cr->c.width, cr->c.height, right, bottom,
+                        ctx->buf_width, ctx->buf_height);
+        } else {
+                cr->c.left = 0;
+                cr->c.top = 0;
+                cr->c.width = ctx->img_width;
+                cr->c.height = ctx->img_height;
+                mfc_debug(2, "Cropping info: w=%d h=%d fw=%d "
+                        "fh=%d\n", cr->c.width, cr->c.height, ctx->buf_width,
+                                                        ctx->buf_height);
+        }
+        return 0;
+}
+
+/* v4l2_ioctl_ops */
+static const struct v4l2_ioctl_ops s5p_mfc_dec_ioctl_ops = {
+        .vidioc_querycap = vidioc_querycap,
+        .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
+        .vidioc_enum_fmt_vid_cap_mplane = vidioc_enum_fmt_vid_cap_mplane,
+        .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
+        .vidioc_enum_fmt_vid_out_mplane = vidioc_enum_fmt_vid_out_mplane,
+        .vidioc_g_fmt_vid_cap_mplane = vidioc_g_fmt,
+        .vidioc_g_fmt_vid_out_mplane = vidioc_g_fmt,
+        .vidioc_try_fmt_vid_cap_mplane = vidioc_try_fmt,
+        .vidioc_try_fmt_vid_out_mplane = vidioc_try_fmt,
+        .vidioc_s_fmt_vid_cap_mplane = vidioc_s_fmt,
+        .vidioc_s_fmt_vid_out_mplane = vidioc_s_fmt,
+        .vidioc_reqbufs = vidioc_reqbufs,
+        .vidioc_querybuf = vidioc_querybuf,
+        .vidioc_qbuf = vidioc_qbuf,
+        .vidioc_dqbuf = vidioc_dqbuf,
+        .vidioc_streamon = vidioc_streamon,
+        .vidioc_streamoff = vidioc_streamoff,
+        .vidioc_g_crop = vidioc_g_crop,
+};
+
+static int s5p_mfc_queue_setup(struct vb2_queue *vq, unsigned int *buf_count,
+                               unsigned int *plane_count, unsigned long psize[],
+                               void *allocators[])
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+
+        /* Video output for decoding (source)
+         * this can be set after getting an instance */
+        if (ctx->state == MFCINST_INIT &&
+            vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                /* A single plane is required for input */
+                *plane_count = 1;
+                if (*buf_count < 1)
+                        *buf_count = 1;
+                if (*buf_count > MFC_MAX_BUFFERS)
+                        *buf_count = MFC_MAX_BUFFERS;
+        /* Video capture for decoding (destination)
+         * this can be set after the header was parsed */
+        } else if (ctx->state == MFCINST_HEAD_PARSED &&
+                   vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                /* Output plane count is 2 - one for Y and one for CbCr */
+                *plane_count = 2;
+                /* Setup buffer count */
+                if (*buf_count < ctx->dpb_count)
+                        *buf_count = ctx->dpb_count;
+                if (*buf_count > ctx->dpb_count + MFC_MAX_EXTRA_DPB)
+                        *buf_count = ctx->dpb_count + MFC_MAX_EXTRA_DPB;
+                if (*buf_count > MFC_MAX_BUFFERS)
+                        *buf_count = MFC_MAX_BUFFERS;
+        } else {
+                mfc_err("State seems invalid. State = %d, vq->type = %d\n",
+                                                        ctx->state, vq->type);
+                return -EINVAL;
+        }
+        mfc_debug(2, "Buffer count=%d, plane count=%d\n",
+                                                *buf_count, *plane_count);
+        if (ctx->state == MFCINST_HEAD_PARSED &&
+            vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                psize[0] = ctx->luma_size;
+                psize[1] = ctx->chroma_size;
+                allocators[0] = ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
+                allocators[1] = ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+        } else if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE &&
+                   ctx->state == MFCINST_INIT) {
+                psize[0] = ctx->dec_src_buf_size;
+                allocators[0] = ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+        } else {
+                mfc_err("This video node is dedicated to decoding. Decoding not initalised\n");
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static void s5p_mfc_unlock(struct vb2_queue *q)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(q->drv_priv);
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mutex_unlock(&dev->mfc_mutex);
+}
+
+static void s5p_mfc_lock(struct vb2_queue *q)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(q->drv_priv);
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mutex_lock(&dev->mfc_mutex);
+}
+
+static int s5p_mfc_buf_init(struct vb2_buffer *vb)
+{
+        struct vb2_queue *vq = vb->vb2_queue;
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+        unsigned int i;
+
+        if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                if (ctx->capture_state == QUEUE_BUFS_MMAPED)
+                        return 0;
+                for (i = 0; i <= ctx->src_fmt->num_planes ; i++) {
+                        if (IS_ERR_OR_NULL(ERR_PTR(
+                                        vb2_dma_contig_plane_paddr(vb, i)))) {
+                                mfc_err("Plane mem not allocated\n");
+                                return -EINVAL;
+                        }
+                }
+                if (vb2_plane_size(vb, 0) < ctx->luma_size ||
+                        vb2_plane_size(vb, 1) < ctx->chroma_size) {
+                        mfc_err("Plane buffer (CAPTURE) is too small\n");
+                        return -EINVAL;
+                }
+                i = vb->v4l2_buf.index;
+                ctx->dst_bufs[i].b = vb;
+                ctx->dst_bufs[i].cookie.raw.luma =
+                                        vb2_dma_contig_plane_paddr(vb, 0);
+                ctx->dst_bufs[i].cookie.raw.chroma =
+                                        vb2_dma_contig_plane_paddr(vb, 1);
+                ctx->dst_bufs_cnt++;
+        } else if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                if (IS_ERR_OR_NULL(ERR_PTR(
+                                        vb2_dma_contig_plane_paddr(vb, 0)))) {
+                        mfc_err("Plane memory not allocated\n");
+                        return -EINVAL;
+                }
+                if (vb2_plane_size(vb, 0) < ctx->dec_src_buf_size) {
+                        mfc_err("Plane buffer (OUTPUT) is too small\n");
+                        return -EINVAL;
+                }
+
+                i = vb->v4l2_buf.index;
+                ctx->src_bufs[i].b = vb;
+                ctx->src_bufs[i].cookie.stream =
+                                        vb2_dma_contig_plane_paddr(vb, 0);
+                ctx->src_bufs_cnt++;
+        } else {
+                mfc_err("s5p_mfc_buf_init: unknown queue type\n");
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static int s5p_mfc_start_streaming(struct vb2_queue *q)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(q->drv_priv);
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+
+        v4l2_ctrl_handler_setup(&ctx->ctrl_handler);
+        if (ctx->state == MFCINST_FINISHING ||
+                ctx->state == MFCINST_FINISHED)
+                ctx->state = MFCINST_RUNNING;
+        /* If context is ready then dev = work->data;schedule it to run */
+        if (s5p_mfc_ctx_ready(ctx)) {
+                spin_lock_irqsave(&dev->condlock, flags);
+                set_bit(ctx->num, &dev->ctx_work_bits);
+                spin_unlock_irqrestore(&dev->condlock, flags);
+        }
+        s5p_mfc_try_run(dev);
+        return 0;
+}
+
+static int s5p_mfc_stop_streaming(struct vb2_queue *q)
+{
+        unsigned long flags;
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(q->drv_priv);
+        struct s5p_mfc_dev *dev = ctx->dev;
+        int aborted = 0;
+
+        if ((ctx->state == MFCINST_FINISHING ||
+                ctx->state ==  MFCINST_RUNNING) &&
+                dev->curr_ctx == ctx->num && dev->hw_lock) {
+                ctx->state = MFCINST_ABORT;
+                s5p_mfc_wait_for_done_ctx(ctx,
+                                        S5P_FIMV_R2H_CMD_FRAME_DONE_RET, 0);
+                aborted = 1;
+        }
+        spin_lock_irqsave(&dev->irqlock, flags);
+        if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
+                INIT_LIST_HEAD(&ctx->dst_queue);
+                ctx->dst_queue_cnt = 0;
+                ctx->dpb_flush_flag = 1;
+                ctx->dec_dst_flag = 0;
+        }
+        if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
+                INIT_LIST_HEAD(&ctx->src_queue);
+                ctx->src_queue_cnt = 0;
+        }
+        if (aborted)
+                ctx->state = MFCINST_RUNNING;
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        return 0;
+}
+
+
+static void s5p_mfc_buf_queue(struct vb2_buffer *vb)
+{
+        struct vb2_queue *vq = vb->vb2_queue;
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *mfc_buf;
+
+        if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                mfc_buf = &ctx->src_bufs[vb->v4l2_buf.index];
+                mfc_buf->used = 0;
+                spin_lock_irqsave(&dev->irqlock, flags);
+                list_add_tail(&mfc_buf->list, &ctx->src_queue);
+                ctx->src_queue_cnt++;
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+        } else if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                mfc_buf = &ctx->dst_bufs[vb->v4l2_buf.index];
+                mfc_buf->used = 0;
+                /* Mark destination as available for use by MFC */
+                spin_lock_irqsave(&dev->irqlock, flags);
+                set_bit(vb->v4l2_buf.index, &ctx->dec_dst_flag);
+                list_add_tail(&mfc_buf->list, &ctx->dst_queue);
+                ctx->dst_queue_cnt++;
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+        } else {
+                mfc_err("Unsupported buffer type (%d)\n", vq->type);
+        }
+        if (s5p_mfc_ctx_ready(ctx)) {
+                spin_lock_irqsave(&dev->condlock, flags);
+                set_bit(ctx->num, &dev->ctx_work_bits);
+                spin_unlock_irqrestore(&dev->condlock, flags);
+        }
+        s5p_mfc_try_run(dev);
+}
+
+static struct vb2_ops s5p_mfc_dec_qops = {
+        .queue_setup            = s5p_mfc_queue_setup,
+        .wait_prepare           = s5p_mfc_unlock,
+        .wait_finish            = s5p_mfc_lock,
+        .buf_init               = s5p_mfc_buf_init,
+        .start_streaming        = s5p_mfc_start_streaming,
+        .stop_streaming         = s5p_mfc_stop_streaming,
+        .buf_queue              = s5p_mfc_buf_queue,
+};
+
+struct s5p_mfc_codec_ops *get_dec_codec_ops(void)
+{
+        return &decoder_codec_ops;
+}
+
+struct vb2_ops *get_dec_queue_ops(void)
+{
+        return &s5p_mfc_dec_qops;
+}
+
+const struct v4l2_ioctl_ops *get_dec_v4l2_ioctl_ops(void)
+{
+        return &s5p_mfc_dec_ioctl_ops;
+}
+
+#define IS_MFC51_PRIV(x) ((V4L2_CTRL_ID2CLASS(x) == V4L2_CTRL_CLASS_MPEG) \
+                                                && V4L2_CTRL_DRIVER_PRIV(x))
+
+int s5p_mfc_dec_ctrls_setup(struct s5p_mfc_ctx *ctx)
+{
+        struct v4l2_ctrl_config cfg;
+        int i;
+
+        v4l2_ctrl_handler_init(&ctx->ctrl_handler, NUM_CTRLS);
+        if (ctx->ctrl_handler.error) {
+                mfc_err("v4l2_ctrl_handler_init failed\n");
+                return ctx->ctrl_handler.error;
+        }
+
+        for (i = 0; i < NUM_CTRLS; i++) {
+                if (IS_MFC51_PRIV(controls[i].id)) {
+                        cfg.ops = &s5p_mfc_dec_ctrl_ops;
+                        cfg.id = controls[i].id;
+                        cfg.min = controls[i].minimum;
+                        cfg.max = controls[i].maximum;
+                        cfg.def = controls[i].default_value;
+                        cfg.name = controls[i].name;
+                        cfg.type = controls[i].type;
+
+                        cfg.step = controls[i].step;
+                        cfg.menu_skip_mask = 0;
+
+                        ctx->ctrls[i] = v4l2_ctrl_new_custom(&ctx->ctrl_handler,
+                                        &cfg, NULL);
+                } else {
+                        ctx->ctrls[i] = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+                                        &s5p_mfc_dec_ctrl_ops,
+                                        controls[i].id, controls[i].minimum,
+                                        controls[i].maximum, controls[i].step,
+                                        controls[i].default_value);
+                }
+                if (ctx->ctrl_handler.error) {
+                        mfc_err("Adding control (%d) failed\n", i);
+                        return ctx->ctrl_handler.error;
+                }
+                if (controls[i].is_volatile && ctx->ctrls[i])
+                        ctx->ctrls[i]->is_volatile = 1;
+        }
+        return 0;
+}
+
+void s5p_mfc_dec_ctrls_delete(struct s5p_mfc_ctx *ctx)
+{
+        int i;
+
+        v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+        for (i = 0; i < NUM_CTRLS; i++)
+                ctx->ctrls[i] = NULL;
+}
+
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_dec.h b/drivers/media/video/s5p-mfc/s5p_mfc_dec.h
new file mode 100644
index 00000000000..fb8b215db0e
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_dec.h
@@ -0,0 +1,23 @@
+/*
+ * linux/drivers/media/video/s5p-mfc/s5p_mfc_dec.h
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.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 S5P_MFC_DEC_H_
+#define S5P_MFC_DEC_H_
+
+struct s5p_mfc_codec_ops *get_dec_codec_ops(void);
+struct vb2_ops *get_dec_queue_ops(void);
+const struct v4l2_ioctl_ops *get_dec_v4l2_ioctl_ops(void);
+struct s5p_mfc_fmt *get_dec_def_fmt(bool src);
+int s5p_mfc_dec_ctrls_setup(struct s5p_mfc_ctx *ctx);
+void s5p_mfc_dec_ctrls_delete(struct s5p_mfc_ctx *ctx);
+
+#endif /* S5P_MFC_DEC_H_ */
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_enc.c b/drivers/media/video/s5p-mfc/s5p_mfc_enc.c
new file mode 100644
index 00000000000..fee094a14f4
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_enc.c
@@ -0,0 +1,1829 @@
+/*
+ * linux/drivers/media/video/s5p-mfc/s5p_mfc_enc.c
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * Jeongtae Park        <jtp.park@samsung.com>
+ * Kamil Debski         <k.debski@samsung.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/clk.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/version.h>
+#include <linux/videodev2.h>
+#include <linux/workqueue.h>
+#include <media/v4l2-ctrls.h>
+#include <media/videobuf2-core.h>
+#include "regs-mfc.h"
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_enc.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_opr.h"
+
+static struct s5p_mfc_fmt formats[] = {
+        {
+                .name = "4:2:0 2 Planes 64x32 Tiles",
+                .fourcc = V4L2_PIX_FMT_NV12MT,
+                .codec_mode = S5P_FIMV_CODEC_NONE,
+                .type = MFC_FMT_RAW,
+                .num_planes = 2,
+        },
+        {
+                .name = "4:2:0 2 Planes",
+                .fourcc = V4L2_PIX_FMT_NV12M,
+                .codec_mode = S5P_FIMV_CODEC_NONE,
+                .type = MFC_FMT_RAW,
+                .num_planes = 2,
+        },
+        {
+                .name = "H264 Encoded Stream",
+                .fourcc = V4L2_PIX_FMT_H264,
+                .codec_mode = S5P_FIMV_CODEC_H264_ENC,
+                .type = MFC_FMT_ENC,
+                .num_planes = 1,
+        },
+        {
+                .name = "MPEG4 Encoded Stream",
+                .fourcc = V4L2_PIX_FMT_MPEG4,
+                .codec_mode = S5P_FIMV_CODEC_MPEG4_ENC,
+                .type = MFC_FMT_ENC,
+                .num_planes = 1,
+        },
+        {
+                .name = "H264 Encoded Stream",
+                .fourcc = V4L2_PIX_FMT_H263,
+                .codec_mode = S5P_FIMV_CODEC_H263_ENC,
+                .type = MFC_FMT_ENC,
+                .num_planes = 1,
+        },
+};
+
+#define NUM_FORMATS ARRAY_SIZE(formats)
+static struct s5p_mfc_fmt *find_format(struct v4l2_format *f, unsigned int t)
+{
+        unsigned int i;
+
+        for (i = 0; i < NUM_FORMATS; i++) {
+                if (formats[i].fourcc == f->fmt.pix_mp.pixelformat &&
+                    formats[i].type == t)
+                        return &formats[i];
+        }
+        return NULL;
+}
+
+static struct mfc_control controls[] = {
+        {
+                .id = V4L2_CID_MPEG_VIDEO_GOP_SIZE,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .minimum = 0,
+                .maximum = (1 << 16) - 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE,
+                .type = V4L2_CTRL_TYPE_MENU,
+                .minimum = V4L2_MPEG_VIDEO_MULTI_SLICE_MODE_SINGLE,
+                .maximum = V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES,
+                .default_value = V4L2_MPEG_VIDEO_MULTI_SLICE_MODE_SINGLE,
+                .menu_skip_mask = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_MB,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .minimum = 1,
+                .maximum = (1 << 16) - 1,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_BYTES,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .minimum = 1900,
+                .maximum = (1 << 30) - 1,
+                .step = 1,
+                .default_value = 1900,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_CYCLIC_INTRA_REFRESH_MB,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .minimum = 0,
+                .maximum = (1 << 16) - 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_MFC51_VIDEO_PADDING,
+                .type = V4L2_CTRL_TYPE_BOOLEAN,
+                .name = "Padding Control Enable",
+                .minimum = 0,
+                .maximum = 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_MFC51_VIDEO_PADDING_YUV,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .name = "Padding Color YUV Value",
+                .minimum = 0,
+                .maximum = (1 << 25) - 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_FRAME_RC_ENABLE,
+                .type = V4L2_CTRL_TYPE_BOOLEAN,
+                .minimum = 0,
+                .maximum = 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_BITRATE,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .minimum = 1,
+                .maximum = (1 << 30) - 1,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_MFC51_VIDEO_RC_REACTION_COEFF,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .name = "Rate Control Reaction Coeff.",
+                .minimum = 1,
+                .maximum = (1 << 16) - 1,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_MFC51_VIDEO_FORCE_FRAME_TYPE,
+                .type = V4L2_CTRL_TYPE_MENU,
+                .name = "Force frame type",
+                .minimum = V4L2_MPEG_MFC51_VIDEO_FORCE_FRAME_TYPE_DISABLED,
+                .maximum = V4L2_MPEG_MFC51_VIDEO_FORCE_FRAME_TYPE_NOT_CODED,
+                .default_value = V4L2_MPEG_MFC51_VIDEO_FORCE_FRAME_TYPE_DISABLED,
+                .menu_skip_mask = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_VBV_SIZE,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .minimum = 0,
+                .maximum = (1 << 16) - 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H264_CPB_SIZE,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .minimum = 0,
+                .maximum = (1 << 16) - 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_HEADER_MODE,
+                .type = V4L2_CTRL_TYPE_MENU,
+                .minimum = V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE,
+                .maximum = V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_1ST_FRAME,
+                .default_value = V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE,
+                .menu_skip_mask = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE,
+                .type = V4L2_CTRL_TYPE_MENU,
+                .name = "Frame Skip Enable",
+                .minimum = V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_DISABLED,
+                .maximum = V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT,
+                .menu_skip_mask = 0,
+                .default_value = V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_DISABLED,
+        },
+        {
+                .id = V4L2_CID_MPEG_MFC51_VIDEO_RC_FIXED_TARGET_BIT,
+                .type = V4L2_CTRL_TYPE_BOOLEAN,
+                .name = "Fixed Target Bit Enable",
+                .minimum = 0,
+                .maximum = 1,
+                .default_value = 0,
+                .menu_skip_mask = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_B_FRAMES,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .minimum = 0,
+                .maximum = 2,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H264_PROFILE,
+                .type = V4L2_CTRL_TYPE_MENU,
+                .minimum = V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE,
+                .maximum = V4L2_MPEG_VIDEO_H264_PROFILE_MULTIVIEW_HIGH,
+                .default_value = V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE,
+                .menu_skip_mask = ~(
+                                (1 << V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE) |
+                                (1 << V4L2_MPEG_VIDEO_H264_PROFILE_MAIN) |
+                                (1 << V4L2_MPEG_VIDEO_H264_PROFILE_HIGH)
+                                ),
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H264_LEVEL,
+                .type = V4L2_CTRL_TYPE_MENU,
+                .minimum = V4L2_MPEG_VIDEO_H264_LEVEL_1_0,
+                .maximum = V4L2_MPEG_VIDEO_H264_LEVEL_4_0,
+                .default_value = V4L2_MPEG_VIDEO_H264_LEVEL_1_0,
+                .menu_skip_mask = ~(
+                                (1 << V4L2_MPEG_VIDEO_H264_LEVEL_4_1) |
+                                (1 << V4L2_MPEG_VIDEO_H264_LEVEL_4_2) |
+                                (1 << V4L2_MPEG_VIDEO_H264_LEVEL_5_0) |
+                                (1 << V4L2_MPEG_VIDEO_H264_LEVEL_5_1)
+                                ),
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL,
+                .type = V4L2_CTRL_TYPE_MENU,
+                .minimum = V4L2_MPEG_VIDEO_MPEG4_LEVEL_0,
+                .maximum = V4L2_MPEG_VIDEO_MPEG4_LEVEL_5,
+                .default_value = V4L2_MPEG_VIDEO_MPEG4_LEVEL_0,
+                .menu_skip_mask = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE,
+                .type = V4L2_CTRL_TYPE_MENU,
+                .minimum = V4L2_MPEG_VIDEO_H264_LOOP_FILTER_MODE_ENABLED,
+                .maximum = V4L2_MPEG_VIDEO_H264_LOOP_FILTER_MODE_DISABLED_AT_SLICE_BOUNDARY,
+                .default_value = V4L2_MPEG_VIDEO_H264_LOOP_FILTER_MODE_ENABLED,
+                .menu_skip_mask = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .minimum = -6,
+                .maximum = 6,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .minimum = -6,
+                .maximum = 6,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE,
+                .type = V4L2_CTRL_TYPE_MENU,
+                .minimum = V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CAVLC,
+                .maximum = V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC,
+                .default_value = V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CAVLC,
+                .menu_skip_mask = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_MFC51_VIDEO_H264_NUM_REF_PIC_FOR_P,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .name = "The Number of Ref. Pic for P",
+                .minimum = 1,
+                .maximum = 2,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H264_8X8_TRANSFORM,
+                .type = V4L2_CTRL_TYPE_BOOLEAN,
+                .minimum = 0,
+                .maximum = 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_MB_RC_ENABLE,
+                .type = V4L2_CTRL_TYPE_BOOLEAN,
+                .minimum = 0,
+                .maximum = 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .minimum = 0,
+                .maximum = 51,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H264_MIN_QP,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .minimum = 0,
+                .maximum = 51,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H264_MAX_QP,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .minimum = 0,
+                .maximum = 51,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H264_P_FRAME_QP,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .minimum = 0,
+                .maximum = 51,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H264_B_FRAME_QP,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .minimum = 0,
+                .maximum = 51,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H263_I_FRAME_QP,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .name = "H263 I-Frame QP value",
+                .minimum = 1,
+                .maximum = 31,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H263_MIN_QP,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .name = "H263 Minimum QP value",
+                .minimum = 1,
+                .maximum = 31,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H263_MAX_QP,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .name = "H263 Maximum QP value",
+                .minimum = 1,
+                .maximum = 31,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H263_P_FRAME_QP,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .name = "H263 P frame QP value",
+                .minimum = 1,
+                .maximum = 31,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H263_B_FRAME_QP,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .name = "H263 B frame QP value",
+                .minimum = 1,
+                .maximum = 31,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_MPEG4_I_FRAME_QP,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .name = "MPEG4 I-Frame QP value",
+                .minimum = 1,
+                .maximum = 31,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_MPEG4_MIN_QP,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .name = "MPEG4 Minimum QP value",
+                .minimum = 1,
+                .maximum = 31,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_MPEG4_MAX_QP,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .name = "MPEG4 Maximum QP value",
+                .minimum = 0,
+                .maximum = 51,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_MPEG4_P_FRAME_QP,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .name = "MPEG4 P frame QP value",
+                .minimum = 1,
+                .maximum = 31,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_MPEG4_B_FRAME_QP,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .name = "MPEG4 B frame QP value",
+                .minimum = 1,
+                .maximum = 31,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_MFC51_VIDEO_H264_ADAPTIVE_RC_DARK,
+                .type = V4L2_CTRL_TYPE_BOOLEAN,
+                .name = "H264 Dark Reg Adaptive RC",
+                .minimum = 0,
+                .maximum = 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_MFC51_VIDEO_H264_ADAPTIVE_RC_SMOOTH,
+                .type = V4L2_CTRL_TYPE_BOOLEAN,
+                .name = "H264 Smooth Reg Adaptive RC",
+                .minimum = 0,
+                .maximum = 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_MFC51_VIDEO_H264_ADAPTIVE_RC_STATIC,
+                .type = V4L2_CTRL_TYPE_BOOLEAN,
+                .name = "H264 Static Reg Adaptive RC",
+                .minimum = 0,
+                .maximum = 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_MFC51_VIDEO_H264_ADAPTIVE_RC_ACTIVITY,
+                .type = V4L2_CTRL_TYPE_BOOLEAN,
+                .name = "H264 Activity Reg Adaptive RC",
+                .minimum = 0,
+                .maximum = 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_ENABLE,
+                .type = V4L2_CTRL_TYPE_BOOLEAN,
+                .minimum = 0,
+                .maximum = 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_IDC,
+                .type = V4L2_CTRL_TYPE_MENU,
+                .minimum = V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_UNSPECIFIED,
+                .maximum = V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_EXTENDED,
+                .default_value = 0,
+                .menu_skip_mask = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H264_VUI_EXT_SAR_WIDTH,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .minimum = 0,
+                .maximum = (1 << 16) - 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H264_VUI_EXT_SAR_HEIGHT,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .minimum = 0,
+                .maximum = (1 << 16) - 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_GOP_CLOSURE,
+                .type = V4L2_CTRL_TYPE_BOOLEAN,
+                .minimum = 0,
+                .maximum = 1,
+                .step = 1,
+                .default_value = 1,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD,
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .minimum = 0,
+                .maximum = (1 << 16) - 1,
+                .step = 1,
+                .default_value = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE,
+                .type = V4L2_CTRL_TYPE_MENU,
+                .minimum = V4L2_MPEG_VIDEO_MPEG4_PROFILE_SIMPLE,
+                .maximum = V4L2_MPEG_VIDEO_MPEG4_PROFILE_ADVANCED_SIMPLE,
+                .default_value = 0,
+                .menu_skip_mask = 0,
+        },
+        {
+                .id = V4L2_CID_MPEG_VIDEO_MPEG4_QPEL,
+                .type = V4L2_CTRL_TYPE_BOOLEAN,
+                .minimum = 0,
+                .maximum = 1,
+                .step = 1,
+                .default_value = 0,
+        },
+};
+
+#define NUM_CTRLS ARRAY_SIZE(controls)
+static const char * const *mfc51_get_menu(u32 id)
+{
+        static const char * const mfc51_video_frame_skip[] = {
+                "Disabled",
+                "Level Limit",
+                "VBV/CPB Limit",
+                NULL,
+        };
+        static const char * const mfc51_video_force_frame[] = {
+                "Disabled",
+                "I Frame",
+                "Not Coded",
+                NULL,
+        };
+        switch (id) {
+        case V4L2_CID_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE:
+                return mfc51_video_frame_skip;
+        case V4L2_CID_MPEG_MFC51_VIDEO_FORCE_FRAME_TYPE:
+                return mfc51_video_force_frame;
+        }
+        return NULL;
+}
+
+static int s5p_mfc_ctx_ready(struct s5p_mfc_ctx *ctx)
+{
+        mfc_debug(2, "src=%d, dst=%d, state=%d\n",
+                  ctx->src_queue_cnt, ctx->dst_queue_cnt, ctx->state);
+        /* context is ready to make header */
+        if (ctx->state == MFCINST_GOT_INST && ctx->dst_queue_cnt >= 1)
+                return 1;
+        /* context is ready to encode a frame */
+        if (ctx->state == MFCINST_RUNNING &&
+                ctx->src_queue_cnt >= 1 && ctx->dst_queue_cnt >= 1)
+                return 1;
+        /* context is ready to encode remain frames */
+        if (ctx->state == MFCINST_FINISHING &&
+                ctx->src_queue_cnt >= 1 && ctx->dst_queue_cnt >= 1)
+                return 1;
+        mfc_debug(2, "ctx is not ready\n");
+        return 0;
+}
+
+static void cleanup_ref_queue(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_buf *mb_entry;
+        unsigned long mb_y_addr, mb_c_addr;
+
+        /* move buffers in ref queue to src queue */
+        while (!list_empty(&ctx->ref_queue)) {
+                mb_entry = list_entry((&ctx->ref_queue)->next,
+                                                struct s5p_mfc_buf, list);
+                mb_y_addr = vb2_dma_contig_plane_paddr(mb_entry->b, 0);
+                mb_c_addr = vb2_dma_contig_plane_paddr(mb_entry->b, 1);
+                list_del(&mb_entry->list);
+                ctx->ref_queue_cnt--;
+                list_add_tail(&mb_entry->list, &ctx->src_queue);
+                ctx->src_queue_cnt++;
+        }
+        mfc_debug(2, "enc src count: %d, enc ref count: %d\n",
+                  ctx->src_queue_cnt, ctx->ref_queue_cnt);
+        INIT_LIST_HEAD(&ctx->ref_queue);
+        ctx->ref_queue_cnt = 0;
+}
+
+static int enc_pre_seq_start(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf *dst_mb;
+        unsigned long dst_addr;
+        unsigned int dst_size;
+        unsigned long flags;
+
+        spin_lock_irqsave(&dev->irqlock, flags);
+        dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+        dst_addr = vb2_dma_contig_plane_paddr(dst_mb->b, 0);
+        dst_size = vb2_plane_size(dst_mb->b, 0);
+        s5p_mfc_set_enc_stream_buffer(ctx, dst_addr, dst_size);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        return 0;
+}
+
+static int enc_post_seq_start(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        struct s5p_mfc_buf *dst_mb;
+        unsigned long flags;
+
+        if (p->seq_hdr_mode == V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE) {
+                spin_lock_irqsave(&dev->irqlock, flags);
+                dst_mb = list_entry(ctx->dst_queue.next,
+                                struct s5p_mfc_buf, list);
+                list_del(&dst_mb->list);
+                ctx->dst_queue_cnt--;
+                vb2_set_plane_payload(dst_mb->b, 0,
+                                                s5p_mfc_get_enc_strm_size());
+                vb2_buffer_done(dst_mb->b, VB2_BUF_STATE_DONE);
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+        }
+        ctx->state = MFCINST_RUNNING;
+        if (s5p_mfc_ctx_ready(ctx)) {
+                spin_lock_irqsave(&dev->condlock, flags);
+                set_bit(ctx->num, &dev->ctx_work_bits);
+                spin_unlock_irqrestore(&dev->condlock, flags);
+        }
+        s5p_mfc_try_run(dev);
+        return 0;
+}
+
+static int enc_pre_frame_start(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf *dst_mb;
+        struct s5p_mfc_buf *src_mb;
+        unsigned long flags;
+        unsigned long src_y_addr, src_c_addr, dst_addr;
+        unsigned int dst_size;
+
+        spin_lock_irqsave(&dev->irqlock, flags);
+        src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        src_y_addr = vb2_dma_contig_plane_paddr(src_mb->b, 0);
+        src_c_addr = vb2_dma_contig_plane_paddr(src_mb->b, 1);
+        s5p_mfc_set_enc_frame_buffer(ctx, src_y_addr, src_c_addr);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+
+        spin_lock_irqsave(&dev->irqlock, flags);
+        dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+        dst_addr = vb2_dma_contig_plane_paddr(dst_mb->b, 0);
+        dst_size = vb2_plane_size(dst_mb->b, 0);
+        s5p_mfc_set_enc_stream_buffer(ctx, dst_addr, dst_size);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+
+        return 0;
+}
+
+static int enc_post_frame_start(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf *mb_entry;
+        unsigned long enc_y_addr, enc_c_addr;
+        unsigned long mb_y_addr, mb_c_addr;
+        int slice_type;
+        unsigned int strm_size;
+        unsigned long flags;
+
+        slice_type = s5p_mfc_get_enc_slice_type();
+        strm_size = s5p_mfc_get_enc_strm_size();
+        mfc_debug(2, "Encoded slice type: %d", slice_type);
+        mfc_debug(2, "Encoded stream size: %d", strm_size);
+        mfc_debug(2, "Display order: %d",
+                  mfc_read(dev, S5P_FIMV_ENC_SI_PIC_CNT));
+        spin_lock_irqsave(&dev->irqlock, flags);
+        if (slice_type >= 0) {
+                s5p_mfc_get_enc_frame_buffer(ctx, &enc_y_addr, &enc_c_addr);
+                list_for_each_entry(mb_entry, &ctx->src_queue, list) {
+                        mb_y_addr = vb2_dma_contig_plane_paddr(mb_entry->b, 0);
+                        mb_c_addr = vb2_dma_contig_plane_paddr(mb_entry->b, 1);
+                        if ((enc_y_addr == mb_y_addr) &&
+                                                (enc_c_addr == mb_c_addr)) {
+                                list_del(&mb_entry->list);
+                                ctx->src_queue_cnt--;
+                                vb2_buffer_done(mb_entry->b,
+                                                        VB2_BUF_STATE_DONE);
+                                break;
+                        }
+                }
+                list_for_each_entry(mb_entry, &ctx->ref_queue, list) {
+                        mb_y_addr = vb2_dma_contig_plane_paddr(mb_entry->b, 0);
+                        mb_c_addr = vb2_dma_contig_plane_paddr(mb_entry->b, 1);
+                        if ((enc_y_addr == mb_y_addr) &&
+                                                (enc_c_addr == mb_c_addr)) {
+                                list_del(&mb_entry->list);
+                                ctx->ref_queue_cnt--;
+                                vb2_buffer_done(mb_entry->b,
+                                                        VB2_BUF_STATE_DONE);
+                                break;
+                        }
+                }
+        }
+        if ((ctx->src_queue_cnt > 0) && (ctx->state == MFCINST_RUNNING)) {
+                mb_entry = list_entry(ctx->src_queue.next, struct s5p_mfc_buf,
+                                                                        list);
+                if (mb_entry->used) {
+                        list_del(&mb_entry->list);
+                        ctx->src_queue_cnt--;
+                        list_add_tail(&mb_entry->list, &ctx->ref_queue);
+                        ctx->ref_queue_cnt++;
+                }
+                mfc_debug(2, "enc src count: %d, enc ref count: %d\n",
+                          ctx->src_queue_cnt, ctx->ref_queue_cnt);
+        }
+        if (strm_size > 0) {
+                /* at least one more dest. buffers exist always  */
+                mb_entry = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf,
+                                                                        list);
+                list_del(&mb_entry->list);
+                ctx->dst_queue_cnt--;
+                switch (slice_type) {
+                case S5P_FIMV_ENC_SI_SLICE_TYPE_I:
+                        mb_entry->b->v4l2_buf.flags |= V4L2_BUF_FLAG_KEYFRAME;
+                        break;
+                case S5P_FIMV_ENC_SI_SLICE_TYPE_P:
+                        mb_entry->b->v4l2_buf.flags |= V4L2_BUF_FLAG_PFRAME;
+                        break;
+                case S5P_FIMV_ENC_SI_SLICE_TYPE_B:
+                        mb_entry->b->v4l2_buf.flags |= V4L2_BUF_FLAG_BFRAME;
+                        break;
+                }
+                vb2_set_plane_payload(mb_entry->b, 0, strm_size);
+                vb2_buffer_done(mb_entry->b, VB2_BUF_STATE_DONE);
+        }
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        if ((ctx->src_queue_cnt == 0) || (ctx->dst_queue_cnt == 0)) {
+                spin_lock(&dev->condlock);
+                clear_bit(ctx->num, &dev->ctx_work_bits);
+                spin_unlock(&dev->condlock);
+        }
+        return 0;
+}
+
+static struct s5p_mfc_codec_ops encoder_codec_ops = {
+        .pre_seq_start          = enc_pre_seq_start,
+        .post_seq_start         = enc_post_seq_start,
+        .pre_frame_start        = enc_pre_frame_start,
+        .post_frame_start       = enc_post_frame_start,
+};
+
+/* Query capabilities of the device */
+static int vidioc_querycap(struct file *file, void *priv,
+                           struct v4l2_capability *cap)
+{
+        struct s5p_mfc_dev *dev = video_drvdata(file);
+
+        strncpy(cap->driver, dev->plat_dev->name, sizeof(cap->driver) - 1);
+        strncpy(cap->card, dev->plat_dev->name, sizeof(cap->card) - 1);
+        cap->bus_info[0] = 0;
+        cap->version = KERNEL_VERSION(1, 0, 0);
+        cap->capabilities = V4L2_CAP_VIDEO_CAPTURE
+                          | V4L2_CAP_VIDEO_OUTPUT
+                          | V4L2_CAP_STREAMING;
+        return 0;
+}
+
+static int vidioc_enum_fmt(struct v4l2_fmtdesc *f, bool mplane, bool out)
+{
+        struct s5p_mfc_fmt *fmt;
+        int i, j = 0;
+
+        for (i = 0; i < ARRAY_SIZE(formats); ++i) {
+                if (mplane && formats[i].num_planes == 1)
+                        continue;
+                else if (!mplane && formats[i].num_planes > 1)
+                        continue;
+                if (out && formats[i].type != MFC_FMT_RAW)
+                        continue;
+                else if (!out && formats[i].type != MFC_FMT_ENC)
+                        continue;
+                if (j == f->index) {
+                        fmt = &formats[i];
+                        strlcpy(f->description, fmt->name,
+                                sizeof(f->description));
+                        f->pixelformat = fmt->fourcc;
+                        return 0;
+                }
+                ++j;
+        }
+        return -EINVAL;
+}
+
+static int vidioc_enum_fmt_vid_cap(struct file *file, void *pirv,
+                                   struct v4l2_fmtdesc *f)
+{
+        return vidioc_enum_fmt(f, false, false);
+}
+
+static int vidioc_enum_fmt_vid_cap_mplane(struct file *file, void *pirv,
+                                          struct v4l2_fmtdesc *f)
+{
+        return vidioc_enum_fmt(f, true, false);
+}
+
+static int vidioc_enum_fmt_vid_out(struct file *file, void *prov,
+                                   struct v4l2_fmtdesc *f)
+{
+        return vidioc_enum_fmt(f, false, true);
+}
+
+static int vidioc_enum_fmt_vid_out_mplane(struct file *file, void *prov,
+                                          struct v4l2_fmtdesc *f)
+{
+        return vidioc_enum_fmt(f, true, true);
+}
+
+static int vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+        struct v4l2_pix_format_mplane *pix_fmt_mp = &f->fmt.pix_mp;
+
+        mfc_debug(2, "f->type = %d ctx->state = %d\n", f->type, ctx->state);
+        if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                /* This is run on output (encoder dest) */
+                pix_fmt_mp->width = 0;
+                pix_fmt_mp->height = 0;
+                pix_fmt_mp->field = V4L2_FIELD_NONE;
+                pix_fmt_mp->pixelformat = ctx->dst_fmt->fourcc;
+                pix_fmt_mp->num_planes = ctx->dst_fmt->num_planes;
+
+                pix_fmt_mp->plane_fmt[0].bytesperline = ctx->enc_dst_buf_size;
+                pix_fmt_mp->plane_fmt[0].sizeimage = ctx->enc_dst_buf_size;
+        } else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                /* This is run on capture (encoder src) */
+                pix_fmt_mp->width = ctx->img_width;
+                pix_fmt_mp->height = ctx->img_height;
+
+                pix_fmt_mp->field = V4L2_FIELD_NONE;
+                pix_fmt_mp->pixelformat = ctx->src_fmt->fourcc;
+                pix_fmt_mp->num_planes = ctx->src_fmt->num_planes;
+
+                pix_fmt_mp->plane_fmt[0].bytesperline = ctx->buf_width;
+                pix_fmt_mp->plane_fmt[0].sizeimage = ctx->luma_size;
+                pix_fmt_mp->plane_fmt[1].bytesperline = ctx->buf_width;
+                pix_fmt_mp->plane_fmt[1].sizeimage = ctx->chroma_size;
+        } else {
+                mfc_err("invalid buf type\n");
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static int vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+        struct s5p_mfc_fmt *fmt;
+        struct v4l2_pix_format_mplane *pix_fmt_mp = &f->fmt.pix_mp;
+
+        if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                fmt = find_format(f, MFC_FMT_ENC);
+                if (!fmt) {
+                        mfc_err("failed to try output format\n");
+                        return -EINVAL;
+                }
+
+                if (pix_fmt_mp->plane_fmt[0].sizeimage == 0) {
+                        mfc_err("must be set encoding output size\n");
+                        return -EINVAL;
+                }
+
+                pix_fmt_mp->plane_fmt[0].bytesperline =
+                        pix_fmt_mp->plane_fmt[0].sizeimage;
+        } else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                fmt = find_format(f, MFC_FMT_RAW);
+                if (!fmt) {
+                        mfc_err("failed to try output format\n");
+                        return -EINVAL;
+                }
+
+                if (fmt->num_planes != pix_fmt_mp->num_planes) {
+                        mfc_err("failed to try output format\n");
+                        return -EINVAL;
+                }
+        } else {
+                mfc_err("invalid buf type\n");
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+        struct s5p_mfc_dev *dev = video_drvdata(file);
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+        struct s5p_mfc_fmt *fmt;
+        struct v4l2_pix_format_mplane *pix_fmt_mp = &f->fmt.pix_mp;
+        unsigned long flags;
+        int ret = 0;
+
+        ret = vidioc_try_fmt(file, priv, f);
+        if (ret)
+                return ret;
+        if (ctx->vq_src.streaming || ctx->vq_dst.streaming) {
+                v4l2_err(&dev->v4l2_dev, "%s queue busy\n", __func__);
+                ret = -EBUSY;
+                goto out;
+        }
+        if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                fmt = find_format(f, MFC_FMT_ENC);
+                if (!fmt) {
+                        mfc_err("failed to set capture format\n");
+                        return -EINVAL;
+                }
+                ctx->state = MFCINST_INIT;
+                ctx->dst_fmt = fmt;
+                ctx->codec_mode = ctx->dst_fmt->codec_mode;
+                ctx->enc_dst_buf_size = pix_fmt_mp->plane_fmt[0].sizeimage;
+                pix_fmt_mp->plane_fmt[0].bytesperline = 0;
+                ctx->dst_bufs_cnt = 0;
+                ctx->capture_state = QUEUE_FREE;
+                s5p_mfc_alloc_instance_buffer(ctx);
+                spin_lock_irqsave(&dev->condlock, flags);
+                set_bit(ctx->num, &dev->ctx_work_bits);
+                spin_unlock_irqrestore(&dev->condlock, flags);
+                s5p_mfc_clean_ctx_int_flags(ctx);
+                s5p_mfc_try_run(dev);
+                if (s5p_mfc_wait_for_done_ctx(ctx, \
+                                S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET, 1)) {
+                                /* Error or timeout */
+                        mfc_err("Error getting instance from hardware\n");
+                        s5p_mfc_release_instance_buffer(ctx);
+                        ret = -EIO;
+                        goto out;
+                }
+                mfc_debug(2, "Got instance number: %d\n", ctx->inst_no);
+        } else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                fmt = find_format(f, MFC_FMT_RAW);
+                if (!fmt) {
+                        mfc_err("failed to set output format\n");
+                        return -EINVAL;
+                }
+                if (fmt->num_planes != pix_fmt_mp->num_planes) {
+                        mfc_err("failed to set output format\n");
+                        ret = -EINVAL;
+                        goto out;
+                }
+                ctx->src_fmt = fmt;
+                ctx->img_width = pix_fmt_mp->width;
+                ctx->img_height = pix_fmt_mp->height;
+                mfc_debug(2, "codec number: %d\n", ctx->src_fmt->codec_mode);
+                mfc_debug(2, "fmt - w: %d, h: %d, ctx - w: %d, h: %d\n",
+                        pix_fmt_mp->width, pix_fmt_mp->height,
+                        ctx->img_width, ctx->img_height);
+                if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M) {
+                        ctx->buf_width = ALIGN(ctx->img_width,
+                                                        S5P_FIMV_NV12M_HALIGN);
+                        ctx->luma_size = ALIGN(ctx->img_width,
+                                S5P_FIMV_NV12M_HALIGN) * ALIGN(ctx->img_height,
+                                S5P_FIMV_NV12M_LVALIGN);
+                        ctx->chroma_size = ALIGN(ctx->img_width,
+                                S5P_FIMV_NV12M_HALIGN) * ALIGN((ctx->img_height
+                                >> 1), S5P_FIMV_NV12M_CVALIGN);
+
+                        ctx->luma_size = ALIGN(ctx->luma_size,
+                                                        S5P_FIMV_NV12M_SALIGN);
+                        ctx->chroma_size = ALIGN(ctx->chroma_size,
+                                                        S5P_FIMV_NV12M_SALIGN);
+
+                        pix_fmt_mp->plane_fmt[0].sizeimage = ctx->luma_size;
+                        pix_fmt_mp->plane_fmt[0].bytesperline = ctx->buf_width;
+                        pix_fmt_mp->plane_fmt[1].sizeimage = ctx->chroma_size;
+                        pix_fmt_mp->plane_fmt[1].bytesperline = ctx->buf_width;
+
+                } else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT) {
+                        ctx->buf_width = ALIGN(ctx->img_width,
+                                                        S5P_FIMV_NV12MT_HALIGN);
+                        ctx->luma_size = ALIGN(ctx->img_width,
+                                S5P_FIMV_NV12MT_HALIGN) * ALIGN(ctx->img_height,
+                                S5P_FIMV_NV12MT_VALIGN);
+                        ctx->chroma_size = ALIGN(ctx->img_width,
+                                S5P_FIMV_NV12MT_HALIGN) * ALIGN((ctx->img_height
+                                >> 1), S5P_FIMV_NV12MT_VALIGN);
+                        ctx->luma_size = ALIGN(ctx->luma_size,
+                                                        S5P_FIMV_NV12MT_SALIGN);
+                        ctx->chroma_size = ALIGN(ctx->chroma_size,
+                                                        S5P_FIMV_NV12MT_SALIGN);
+
+                        pix_fmt_mp->plane_fmt[0].sizeimage = ctx->luma_size;
+                        pix_fmt_mp->plane_fmt[0].bytesperline = ctx->buf_width;
+                        pix_fmt_mp->plane_fmt[1].sizeimage = ctx->chroma_size;
+                        pix_fmt_mp->plane_fmt[1].bytesperline = ctx->buf_width;
+                }
+                ctx->src_bufs_cnt = 0;
+                ctx->output_state = QUEUE_FREE;
+        } else {
+                mfc_err("invalid buf type\n");
+                return -EINVAL;
+        }
+out:
+        mfc_debug_leave();
+        return ret;
+}
+
+static int vidioc_reqbufs(struct file *file, void *priv,
+                                          struct v4l2_requestbuffers *reqbufs)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+        int ret = 0;
+
+        /* if memory is not mmp or userptr return error */
+        if ((reqbufs->memory != V4L2_MEMORY_MMAP) &&
+                (reqbufs->memory != V4L2_MEMORY_USERPTR))
+                return -EINVAL;
+        if (reqbufs->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                if (ctx->capture_state != QUEUE_FREE) {
+                        mfc_err("invalid capture state: %d\n",
+                                                        ctx->capture_state);
+                        return -EINVAL;
+                }
+                ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
+                if (ret != 0) {
+                        mfc_err("error in vb2_reqbufs() for E(D)\n");
+                        return ret;
+                }
+                ctx->capture_state = QUEUE_BUFS_REQUESTED;
+                ret = s5p_mfc_alloc_codec_buffers(ctx);
+                if (ret) {
+                        mfc_err("Failed to allocate encoding buffers\n");
+                        reqbufs->count = 0;
+                        ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
+                        return -ENOMEM;
+                }
+        } else if (reqbufs->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                if (ctx->output_state != QUEUE_FREE) {
+                        mfc_err("invalid output state: %d\n",
+                                                        ctx->output_state);
+                        return -EINVAL;
+                }
+                ret = vb2_reqbufs(&ctx->vq_src, reqbufs);
+                if (ret != 0) {
+                        mfc_err("error in vb2_reqbufs() for E(S)\n");
+                        return ret;
+                }
+                ctx->output_state = QUEUE_BUFS_REQUESTED;
+        } else {
+                mfc_err("invalid buf type\n");
+                return -EINVAL;
+        }
+        return ret;
+}
+
+static int vidioc_querybuf(struct file *file, void *priv,
+                                                   struct v4l2_buffer *buf)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+        int ret = 0;
+
+        /* if memory is not mmp or userptr return error */
+        if ((buf->memory != V4L2_MEMORY_MMAP) &&
+                (buf->memory != V4L2_MEMORY_USERPTR))
+                return -EINVAL;
+        if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                if (ctx->state != MFCINST_GOT_INST) {
+                        mfc_err("invalid context state: %d\n", ctx->state);
+                        return -EINVAL;
+                }
+                ret = vb2_querybuf(&ctx->vq_dst, buf);
+                if (ret != 0) {
+                        mfc_err("error in vb2_querybuf() for E(D)\n");
+                        return ret;
+                }
+                buf->m.planes[0].m.mem_offset += DST_QUEUE_OFF_BASE;
+        } else if (buf->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                ret = vb2_querybuf(&ctx->vq_src, buf);
+                if (ret != 0) {
+                        mfc_err("error in vb2_querybuf() for E(S)\n");
+                        return ret;
+                }
+        } else {
+                mfc_err("invalid buf type\n");
+                return -EINVAL;
+        }
+        return ret;
+}
+
+/* Queue a buffer */
+static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+        if (ctx->state == MFCINST_ERROR) {
+                mfc_err("Call on QBUF after unrecoverable error\n");
+                return -EIO;
+        }
+        if (buf->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+                return vb2_qbuf(&ctx->vq_src, buf);
+        else if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+                return vb2_qbuf(&ctx->vq_dst, buf);
+        return -EINVAL;
+}
+
+/* Dequeue a buffer */
+static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+        if (ctx->state == MFCINST_ERROR) {
+                mfc_err("Call on DQBUF after unrecoverable error\n");
+                return -EIO;
+        }
+        if (buf->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+                return vb2_dqbuf(&ctx->vq_src, buf, file->f_flags & O_NONBLOCK);
+        else if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+                return vb2_dqbuf(&ctx->vq_dst, buf, file->f_flags & O_NONBLOCK);
+        return -EINVAL;
+}
+
+/* Stream on */
+static int vidioc_streamon(struct file *file, void *priv,
+                           enum v4l2_buf_type type)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+        if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+                return vb2_streamon(&ctx->vq_src, type);
+        else if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+                return vb2_streamon(&ctx->vq_dst, type);
+        return -EINVAL;
+}
+
+/* Stream off, which equals to a pause */
+static int vidioc_streamoff(struct file *file, void *priv,
+                            enum v4l2_buf_type type)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+        if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+                return vb2_streamoff(&ctx->vq_src, type);
+        else if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+                return vb2_streamoff(&ctx->vq_dst, type);
+        return -EINVAL;
+}
+
+static inline int h264_level(enum v4l2_mpeg_video_h264_level lvl)
+{
+        static unsigned int t[V4L2_MPEG_VIDEO_H264_LEVEL_4_0 + 1] = {
+                /* V4L2_MPEG_VIDEO_H264_LEVEL_1_0   */ 10,
+                /* V4L2_MPEG_VIDEO_H264_LEVEL_1B    */ 9,
+                /* V4L2_MPEG_VIDEO_H264_LEVEL_1_1   */ 11,
+                /* V4L2_MPEG_VIDEO_H264_LEVEL_1_2   */ 12,
+                /* V4L2_MPEG_VIDEO_H264_LEVEL_1_3   */ 13,
+                /* V4L2_MPEG_VIDEO_H264_LEVEL_2_0   */ 20,
+                /* V4L2_MPEG_VIDEO_H264_LEVEL_2_1   */ 21,
+                /* V4L2_MPEG_VIDEO_H264_LEVEL_2_2   */ 22,
+                /* V4L2_MPEG_VIDEO_H264_LEVEL_3_0   */ 30,
+                /* V4L2_MPEG_VIDEO_H264_LEVEL_3_1   */ 31,
+                /* V4L2_MPEG_VIDEO_H264_LEVEL_3_2   */ 32,
+                /* V4L2_MPEG_VIDEO_H264_LEVEL_4_0   */ 40,
+        };
+        return t[lvl];
+}
+
+static inline int mpeg4_level(enum v4l2_mpeg_video_mpeg4_level lvl)
+{
+        static unsigned int t[V4L2_MPEG_VIDEO_MPEG4_LEVEL_5 + 1] = {
+                /* V4L2_MPEG_VIDEO_MPEG4_LEVEL_0    */ 0,
+                /* V4L2_MPEG_VIDEO_MPEG4_LEVEL_0B   */ 9,
+                /* V4L2_MPEG_VIDEO_MPEG4_LEVEL_1    */ 1,
+                /* V4L2_MPEG_VIDEO_MPEG4_LEVEL_2    */ 2,
+                /* V4L2_MPEG_VIDEO_MPEG4_LEVEL_3    */ 3,
+                /* V4L2_MPEG_VIDEO_MPEG4_LEVEL_3B   */ 7,
+                /* V4L2_MPEG_VIDEO_MPEG4_LEVEL_4    */ 4,
+                /* V4L2_MPEG_VIDEO_MPEG4_LEVEL_5    */ 5,
+        };
+        return t[lvl];
+}
+
+static inline int vui_sar_idc(enum v4l2_mpeg_video_h264_vui_sar_idc sar)
+{
+        static unsigned int t[V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_EXTENDED + 1] = {
+                /* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_UNSPECIFIED     */ 0,
+                /* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_1x1             */ 1,
+                /* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_12x11           */ 2,
+                /* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_10x11           */ 3,
+                /* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_16x11           */ 4,
+                /* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_40x33           */ 5,
+                /* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_24x11           */ 6,
+                /* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_20x11           */ 7,
+                /* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_32x11           */ 8,
+                /* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_80x33           */ 9,
+                /* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_18x11           */ 10,
+                /* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_15x11           */ 11,
+                /* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_64x33           */ 12,
+                /* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_160x99          */ 13,
+                /* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_4x3             */ 14,
+                /* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_3x2             */ 15,
+                /* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_2x1             */ 16,
+                /* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_EXTENDED        */ 255,
+        };
+        return t[sar];
+}
+
+static int s5p_mfc_enc_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct s5p_mfc_ctx *ctx = ctrl_to_ctx(ctrl);
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        int ret = 0;
+
+        switch (ctrl->id) {
+        case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+                p->gop_size = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE:
+                p->slice_mode = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_MB:
+                p->slice_mb = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_BYTES:
+                p->slice_bit = ctrl->val * 8;
+                break;
+        case V4L2_CID_MPEG_VIDEO_CYCLIC_INTRA_REFRESH_MB:
+                p->intra_refresh_mb = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_MFC51_VIDEO_PADDING:
+                p->pad = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_MFC51_VIDEO_PADDING_YUV:
+                p->pad_luma = (ctrl->val >> 16) & 0xff;
+                p->pad_cb = (ctrl->val >> 8) & 0xff;
+                p->pad_cr = (ctrl->val >> 0) & 0xff;
+                break;
+        case V4L2_CID_MPEG_VIDEO_FRAME_RC_ENABLE:
+                p->rc_frame = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_BITRATE:
+                p->rc_bitrate = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_MFC51_VIDEO_RC_REACTION_COEFF:
+                p->rc_reaction_coeff = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_MFC51_VIDEO_FORCE_FRAME_TYPE:
+                ctx->force_frame_type = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_VBV_SIZE:
+                p->vbv_size = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_H264_CPB_SIZE:
+                p->codec.h264.cpb_size = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_HEADER_MODE:
+                p->seq_hdr_mode = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE:
+                p->frame_skip_mode = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_MFC51_VIDEO_RC_FIXED_TARGET_BIT:
+                p->fixed_target_bit = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_B_FRAMES:
+                p->num_b_frame = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
+                switch (ctrl->val) {
+                case V4L2_MPEG_VIDEO_H264_PROFILE_MAIN:
+                        p->codec.h264.profile =
+                                        S5P_FIMV_ENC_PROFILE_H264_MAIN;
+                        break;
+                case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH:
+                        p->codec.h264.profile =
+                                        S5P_FIMV_ENC_PROFILE_H264_HIGH;
+                        break;
+                case V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE:
+                        p->codec.h264.profile =
+                                S5P_FIMV_ENC_PROFILE_H264_BASELINE;
+                        break;
+                default:
+                        ret = -EINVAL;
+                }
+                break;
+        case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
+                p->codec.h264.level_v4l2 = ctrl->val;
+                p->codec.h264.level = h264_level(ctrl->val);
+                if (p->codec.h264.level < 0) {
+                        mfc_err("Level number is wrong\n");
+                        ret = p->codec.h264.level;
+                }
+                break;
+        case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:
+                p->codec.mpeg4.level_v4l2 = ctrl->val;
+                p->codec.mpeg4.level = mpeg4_level(ctrl->val);
+                if (p->codec.mpeg4.level < 0) {
+                        mfc_err("Level number is wrong\n");
+                        ret = p->codec.mpeg4.level;
+                }
+                break;
+        case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE:
+                p->codec.h264.loop_filter_mode = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA:
+                p->codec.h264.loop_filter_alpha = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA:
+                p->codec.h264.loop_filter_beta = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE:
+                p->codec.h264.entropy_mode = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_MFC51_VIDEO_H264_NUM_REF_PIC_FOR_P:
+                p->codec.h264.num_ref_pic_4p = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_H264_8X8_TRANSFORM:
+                p->codec.h264._8x8_transform = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_MB_RC_ENABLE:
+                p->codec.h264.rc_mb = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP:
+                p->codec.h264.rc_frame_qp = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_H264_MIN_QP:
+                p->codec.h264.rc_min_qp = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_H264_MAX_QP:
+                p->codec.h264.rc_max_qp = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_H264_P_FRAME_QP:
+                p->codec.h264.rc_p_frame_qp = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_H264_B_FRAME_QP:
+                p->codec.h264.rc_b_frame_qp = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_MPEG4_I_FRAME_QP:
+        case V4L2_CID_MPEG_VIDEO_H263_I_FRAME_QP:
+                p->codec.mpeg4.rc_frame_qp = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_MPEG4_MIN_QP:
+        case V4L2_CID_MPEG_VIDEO_H263_MIN_QP:
+                p->codec.mpeg4.rc_min_qp = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_MPEG4_MAX_QP:
+        case V4L2_CID_MPEG_VIDEO_H263_MAX_QP:
+                p->codec.mpeg4.rc_max_qp = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_MPEG4_P_FRAME_QP:
+        case V4L2_CID_MPEG_VIDEO_H263_P_FRAME_QP:
+                p->codec.mpeg4.rc_p_frame_qp = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_MPEG4_B_FRAME_QP:
+        case V4L2_CID_MPEG_VIDEO_H263_B_FRAME_QP:
+                p->codec.mpeg4.rc_b_frame_qp = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_MFC51_VIDEO_H264_ADAPTIVE_RC_DARK:
+                p->codec.h264.rc_mb_dark = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_MFC51_VIDEO_H264_ADAPTIVE_RC_SMOOTH:
+                p->codec.h264.rc_mb_smooth = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_MFC51_VIDEO_H264_ADAPTIVE_RC_STATIC:
+                p->codec.h264.rc_mb_static = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_MFC51_VIDEO_H264_ADAPTIVE_RC_ACTIVITY:
+                p->codec.h264.rc_mb_activity = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_ENABLE:
+                p->codec.h264.vui_sar = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_IDC:
+                p->codec.h264.vui_sar_idc = vui_sar_idc(ctrl->val);
+                break;
+        case V4L2_CID_MPEG_VIDEO_H264_VUI_EXT_SAR_WIDTH:
+                p->codec.h264.vui_ext_sar_width = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_H264_VUI_EXT_SAR_HEIGHT:
+                p->codec.h264.vui_ext_sar_height = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
+                p->codec.h264.open_gop = !ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_H264_I_PERIOD:
+                p->codec.h264.open_gop_size = ctrl->val;
+                break;
+        case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:
+                switch (ctrl->val) {
+                case V4L2_MPEG_VIDEO_MPEG4_PROFILE_SIMPLE:
+                        p->codec.mpeg4.profile =
+                                S5P_FIMV_ENC_PROFILE_MPEG4_SIMPLE;
+                        break;
+                case V4L2_MPEG_VIDEO_MPEG4_PROFILE_ADVANCED_SIMPLE:
+                        p->codec.mpeg4.profile =
+                        S5P_FIMV_ENC_PROFILE_MPEG4_ADVANCED_SIMPLE;
+                        break;
+                default:
+                        ret = -EINVAL;
+                }
+                break;
+        case V4L2_CID_MPEG_VIDEO_MPEG4_QPEL:
+                p->codec.mpeg4.quarter_pixel = ctrl->val;
+                break;
+        default:
+                v4l2_err(&dev->v4l2_dev, "Invalid control, id=%d, val=%d\n",
+                                                        ctrl->id, ctrl->val);
+                ret = -EINVAL;
+        }
+        return ret;
+}
+
+static const struct v4l2_ctrl_ops s5p_mfc_enc_ctrl_ops = {
+        .s_ctrl = s5p_mfc_enc_s_ctrl,
+};
+
+int vidioc_s_parm(struct file *file, void *priv, struct v4l2_streamparm *a)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+        if (a->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                ctx->enc_params.rc_framerate_num =
+                                        a->parm.output.timeperframe.denominator;
+                ctx->enc_params.rc_framerate_denom =
+                                        a->parm.output.timeperframe.numerator;
+        } else {
+                mfc_err("Setting FPS is only possible for the output queue\n");
+                return -EINVAL;
+        }
+        return 0;
+}
+
+int vidioc_g_parm(struct file *file, void *priv, struct v4l2_streamparm *a)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+        if (a->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+                a->parm.output.timeperframe.denominator =
+                                        ctx->enc_params.rc_framerate_num;
+                a->parm.output.timeperframe.numerator =
+                                        ctx->enc_params.rc_framerate_denom;
+        } else {
+                mfc_err("Setting FPS is only possible for the output queue\n");
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static const struct v4l2_ioctl_ops s5p_mfc_enc_ioctl_ops = {
+        .vidioc_querycap = vidioc_querycap,
+        .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
+        .vidioc_enum_fmt_vid_cap_mplane = vidioc_enum_fmt_vid_cap_mplane,
+        .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
+        .vidioc_enum_fmt_vid_out_mplane = vidioc_enum_fmt_vid_out_mplane,
+        .vidioc_g_fmt_vid_cap_mplane = vidioc_g_fmt,
+        .vidioc_g_fmt_vid_out_mplane = vidioc_g_fmt,
+        .vidioc_try_fmt_vid_cap_mplane = vidioc_try_fmt,
+        .vidioc_try_fmt_vid_out_mplane = vidioc_try_fmt,
+        .vidioc_s_fmt_vid_cap_mplane = vidioc_s_fmt,
+        .vidioc_s_fmt_vid_out_mplane = vidioc_s_fmt,
+        .vidioc_reqbufs = vidioc_reqbufs,
+        .vidioc_querybuf = vidioc_querybuf,
+        .vidioc_qbuf = vidioc_qbuf,
+        .vidioc_dqbuf = vidioc_dqbuf,
+        .vidioc_streamon = vidioc_streamon,
+        .vidioc_streamoff = vidioc_streamoff,
+        .vidioc_s_parm = vidioc_s_parm,
+        .vidioc_g_parm = vidioc_g_parm,
+};
+
+static int check_vb_with_fmt(struct s5p_mfc_fmt *fmt, struct vb2_buffer *vb)
+{
+        int i;
+
+        if (!fmt)
+                return -EINVAL;
+        if (fmt->num_planes != vb->num_planes) {
+                mfc_err("invalid plane number for the format\n");
+                return -EINVAL;
+        }
+        for (i = 0; i < fmt->num_planes; i++) {
+                if (!vb2_dma_contig_plane_paddr(vb, i)) {
+                        mfc_err("failed to get plane cookie\n");
+                        return -EINVAL;
+                }
+                mfc_debug(2, "index: %d, plane[%d] cookie: 0x%08zx",
+                                vb->v4l2_buf.index, i,
+                                vb2_dma_contig_plane_paddr(vb, i));
+        }
+        return 0;
+}
+
+static int s5p_mfc_queue_setup(struct vb2_queue *vq,
+                       unsigned int *buf_count, unsigned int *plane_count,
+                       unsigned long psize[], void *allocators[])
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+
+        if (ctx->state != MFCINST_GOT_INST) {
+                mfc_err("inavlid state: %d\n", ctx->state);
+                return -EINVAL;
+        }
+        if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                if (ctx->dst_fmt)
+                        *plane_count = ctx->dst_fmt->num_planes;
+                else
+                        *plane_count = MFC_ENC_CAP_PLANE_COUNT;
+                if (*buf_count < 1)
+                        *buf_count = 1;
+                if (*buf_count > MFC_MAX_BUFFERS)
+                        *buf_count = MFC_MAX_BUFFERS;
+                psize[0] = ctx->enc_dst_buf_size;
+                allocators[0] = ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+        } else if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                if (ctx->src_fmt)
+                        *plane_count = ctx->src_fmt->num_planes;
+                else
+                        *plane_count = MFC_ENC_OUT_PLANE_COUNT;
+
+                if (*buf_count < 1)
+                        *buf_count = 1;
+                if (*buf_count > MFC_MAX_BUFFERS)
+                        *buf_count = MFC_MAX_BUFFERS;
+                psize[0] = ctx->luma_size;
+                psize[1] = ctx->chroma_size;
+                allocators[0] = ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
+                allocators[1] = ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
+        } else {
+                mfc_err("inavlid queue type: %d\n", vq->type);
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static void s5p_mfc_unlock(struct vb2_queue *q)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(q->drv_priv);
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mutex_unlock(&dev->mfc_mutex);
+}
+
+static void s5p_mfc_lock(struct vb2_queue *q)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(q->drv_priv);
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mutex_lock(&dev->mfc_mutex);
+}
+
+static int s5p_mfc_buf_init(struct vb2_buffer *vb)
+{
+        struct vb2_queue *vq = vb->vb2_queue;
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+        unsigned int i;
+        int ret;
+
+        if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                ret = check_vb_with_fmt(ctx->dst_fmt, vb);
+                if (ret < 0)
+                        return ret;
+                i = vb->v4l2_buf.index;
+                ctx->dst_bufs[i].b = vb;
+                ctx->dst_bufs[i].cookie.stream =
+                                        vb2_dma_contig_plane_paddr(vb, 0);
+                ctx->dst_bufs_cnt++;
+        } else if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                ret = check_vb_with_fmt(ctx->src_fmt, vb);
+                if (ret < 0)
+                        return ret;
+                i = vb->v4l2_buf.index;
+                ctx->src_bufs[i].b = vb;
+                ctx->src_bufs[i].cookie.raw.luma =
+                                        vb2_dma_contig_plane_paddr(vb, 0);
+                ctx->src_bufs[i].cookie.raw.chroma =
+                                        vb2_dma_contig_plane_paddr(vb, 1);
+                ctx->src_bufs_cnt++;
+        } else {
+                mfc_err("inavlid queue type: %d\n", vq->type);
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static int s5p_mfc_buf_prepare(struct vb2_buffer *vb)
+{
+        struct vb2_queue *vq = vb->vb2_queue;
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+        int ret;
+
+        if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                ret = check_vb_with_fmt(ctx->dst_fmt, vb);
+                if (ret < 0)
+                        return ret;
+                mfc_debug(2, "plane size: %ld, dst size: %d\n",
+                        vb2_plane_size(vb, 0), ctx->enc_dst_buf_size);
+                if (vb2_plane_size(vb, 0) < ctx->enc_dst_buf_size) {
+                        mfc_err("plane size is too small for capture\n");
+                        return -EINVAL;
+                }
+        } else if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                ret = check_vb_with_fmt(ctx->src_fmt, vb);
+                if (ret < 0)
+                        return ret;
+                mfc_debug(2, "plane size: %ld, luma size: %d\n",
+                        vb2_plane_size(vb, 0), ctx->luma_size);
+                mfc_debug(2, "plane size: %ld, chroma size: %d\n",
+                        vb2_plane_size(vb, 1), ctx->chroma_size);
+                if (vb2_plane_size(vb, 0) < ctx->luma_size ||
+                    vb2_plane_size(vb, 1) < ctx->chroma_size) {
+                        mfc_err("plane size is too small for output\n");
+                        return -EINVAL;
+                }
+        } else {
+                mfc_err("inavlid queue type: %d\n", vq->type);
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static int s5p_mfc_start_streaming(struct vb2_queue *q)
+{
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(q->drv_priv);
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+
+        v4l2_ctrl_handler_setup(&ctx->ctrl_handler);
+        /* If context is ready then dev = work->data;schedule it to run */
+        if (s5p_mfc_ctx_ready(ctx)) {
+                spin_lock_irqsave(&dev->condlock, flags);
+                set_bit(ctx->num, &dev->ctx_work_bits);
+                spin_unlock_irqrestore(&dev->condlock, flags);
+        }
+        s5p_mfc_try_run(dev);
+        return 0;
+}
+
+static int s5p_mfc_stop_streaming(struct vb2_queue *q)
+{
+        unsigned long flags;
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(q->drv_priv);
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        if ((ctx->state == MFCINST_FINISHING ||
+                ctx->state == MFCINST_RUNNING) &&
+                dev->curr_ctx == ctx->num && dev->hw_lock) {
+                ctx->state = MFCINST_ABORT;
+                s5p_mfc_wait_for_done_ctx(ctx, S5P_FIMV_R2H_CMD_FRAME_DONE_RET,
+                                          0);
+        }
+        ctx->state = MFCINST_FINISHED;
+        spin_lock_irqsave(&dev->irqlock, flags);
+        if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
+                INIT_LIST_HEAD(&ctx->dst_queue);
+                ctx->dst_queue_cnt = 0;
+        }
+        if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                cleanup_ref_queue(ctx);
+                s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
+                INIT_LIST_HEAD(&ctx->src_queue);
+                ctx->src_queue_cnt = 0;
+        }
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        return 0;
+}
+
+static void s5p_mfc_buf_queue(struct vb2_buffer *vb)
+{
+        struct vb2_queue *vq = vb->vb2_queue;
+        struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *mfc_buf;
+
+        if (ctx->state == MFCINST_ERROR) {
+                vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+                cleanup_ref_queue(ctx);
+                return;
+        }
+        if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                mfc_buf = &ctx->dst_bufs[vb->v4l2_buf.index];
+                mfc_buf->used = 0;
+                /* Mark destination as available for use by MFC */
+                spin_lock_irqsave(&dev->irqlock, flags);
+                list_add_tail(&mfc_buf->list, &ctx->dst_queue);
+                ctx->dst_queue_cnt++;
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+        } else if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                mfc_buf = &ctx->src_bufs[vb->v4l2_buf.index];
+                mfc_buf->used = 0;
+                spin_lock_irqsave(&dev->irqlock, flags);
+                if (vb->v4l2_planes[0].bytesused == 0) {
+                        mfc_debug(1, "change state to FINISHING\n");
+                        ctx->state = MFCINST_FINISHING;
+                        vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+                        cleanup_ref_queue(ctx);
+                } else {
+                        list_add_tail(&mfc_buf->list, &ctx->src_queue);
+                        ctx->src_queue_cnt++;
+                }
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+        } else {
+                mfc_err("unsupported buffer type (%d)\n", vq->type);
+        }
+        if (s5p_mfc_ctx_ready(ctx)) {
+                spin_lock_irqsave(&dev->condlock, flags);
+                set_bit(ctx->num, &dev->ctx_work_bits);
+                spin_unlock_irqrestore(&dev->condlock, flags);
+        }
+        s5p_mfc_try_run(dev);
+}
+
+static struct vb2_ops s5p_mfc_enc_qops = {
+        .queue_setup            = s5p_mfc_queue_setup,
+        .wait_prepare           = s5p_mfc_unlock,
+        .wait_finish            = s5p_mfc_lock,
+        .buf_init               = s5p_mfc_buf_init,
+        .buf_prepare            = s5p_mfc_buf_prepare,
+        .start_streaming        = s5p_mfc_start_streaming,
+        .stop_streaming         = s5p_mfc_stop_streaming,
+        .buf_queue              = s5p_mfc_buf_queue,
+};
+
+struct s5p_mfc_codec_ops *get_enc_codec_ops(void)
+{
+        return &encoder_codec_ops;
+}
+
+struct vb2_ops *get_enc_queue_ops(void)
+{
+        return &s5p_mfc_enc_qops;
+}
+
+const struct v4l2_ioctl_ops *get_enc_v4l2_ioctl_ops(void)
+{
+        return &s5p_mfc_enc_ioctl_ops;
+}
+
+#define IS_MFC51_PRIV(x) ((V4L2_CTRL_ID2CLASS(x) == V4L2_CTRL_CLASS_MPEG) \
+                                                && V4L2_CTRL_DRIVER_PRIV(x))
+
+int s5p_mfc_enc_ctrls_setup(struct s5p_mfc_ctx *ctx)
+{
+        struct v4l2_ctrl_config cfg;
+        int i;
+
+        v4l2_ctrl_handler_init(&ctx->ctrl_handler, NUM_CTRLS);
+        if (ctx->ctrl_handler.error) {
+                mfc_err("v4l2_ctrl_handler_init failed\n");
+                return ctx->ctrl_handler.error;
+        }
+        for (i = 0; i < NUM_CTRLS; i++) {
+                if (IS_MFC51_PRIV(controls[i].id)) {
+                        cfg.ops = &s5p_mfc_enc_ctrl_ops;
+                        cfg.id = controls[i].id;
+                        cfg.min = controls[i].minimum;
+                        cfg.max = controls[i].maximum;
+                        cfg.def = controls[i].default_value;
+                        cfg.name = controls[i].name;
+                        cfg.type = controls[i].type;
+                        cfg.flags = 0;
+
+                        if (cfg.type == V4L2_CTRL_TYPE_MENU) {
+                                cfg.step = 0;
+                                cfg.menu_skip_mask = cfg.menu_skip_mask;
+                                cfg.qmenu = mfc51_get_menu(cfg.id);
+                        } else {
+                                cfg.step = controls[i].step;
+                                cfg.menu_skip_mask = 0;
+                        }
+                        ctx->ctrls[i] = v4l2_ctrl_new_custom(&ctx->ctrl_handler,
+                                        &cfg, NULL);
+                } else {
+                        if (controls[i].type == V4L2_CTRL_TYPE_MENU) {
+                                ctx->ctrls[i] = v4l2_ctrl_new_std_menu(
+                                        &ctx->ctrl_handler,
+                                        &s5p_mfc_enc_ctrl_ops, controls[i].id,
+                                        controls[i].maximum, 0,
+                                        controls[i].default_value);
+                        } else {
+                                ctx->ctrls[i] = v4l2_ctrl_new_std(
+                                        &ctx->ctrl_handler,
+                                        &s5p_mfc_enc_ctrl_ops, controls[i].id,
+                                        controls[i].minimum,
+                                        controls[i].maximum, controls[i].step,
+                                        controls[i].default_value);
+                        }
+                }
+                if (ctx->ctrl_handler.error) {
+                        mfc_err("Adding control (%d) failed\n", i);
+                        return ctx->ctrl_handler.error;
+                }
+                if (controls[i].is_volatile && ctx->ctrls[i])
+                        ctx->ctrls[i]->is_volatile = 1;
+        }
+        return 0;
+}
+
+void s5p_mfc_enc_ctrls_delete(struct s5p_mfc_ctx *ctx)
+{
+        int i;
+
+        v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+        for (i = 0; i < NUM_CTRLS; i++)
+                ctx->ctrls[i] = NULL;
+}
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_enc.h b/drivers/media/video/s5p-mfc/s5p_mfc_enc.h
new file mode 100644
index 00000000000..405bdd3ee08
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_enc.h
@@ -0,0 +1,23 @@
+/*
+ * linux/drivers/media/video/s5p-mfc/s5p_mfc_enc.h
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.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 S5P_MFC_ENC_H_
+#define S5P_MFC_ENC_H_
+
+struct s5p_mfc_codec_ops *get_enc_codec_ops(void);
+struct vb2_ops *get_enc_queue_ops(void);
+const struct v4l2_ioctl_ops *get_enc_v4l2_ioctl_ops(void);
+struct s5p_mfc_fmt *get_enc_def_fmt(bool src);
+int s5p_mfc_enc_ctrls_setup(struct s5p_mfc_ctx *ctx);
+void s5p_mfc_enc_ctrls_delete(struct s5p_mfc_ctx *ctx);
+
+#endif /* S5P_MFC_ENC_H_  */
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_intr.c b/drivers/media/video/s5p-mfc/s5p_mfc_intr.c
new file mode 100644
index 00000000000..8f2f8bf4da7
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_intr.c
@@ -0,0 +1,92 @@
+/*
+ * drivers/media/video/samsung/mfc5/s5p_mfc_intr.c
+ *
+ * C file for Samsung MFC (Multi Function Codec - FIMV) driver
+ * This file contains functions used to wait for command completion.
+ *
+ * Kamil Debski, Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ * http://www.samsung.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.
+ */
+
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/io.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include "regs-mfc.h"
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_intr.h"
+
+int s5p_mfc_wait_for_done_dev(struct s5p_mfc_dev *dev, int command)
+{
+        int ret;
+
+        ret = wait_event_interruptible_timeout(dev->queue,
+                (dev->int_cond && (dev->int_type == command
+                || dev->int_type == S5P_FIMV_R2H_CMD_ERR_RET)),
+                msecs_to_jiffies(MFC_INT_TIMEOUT));
+        if (ret == 0) {
+                mfc_err("Interrupt (dev->int_type:%d, command:%d) timed out\n",
+                                                        dev->int_type, command);
+                return 1;
+        } else if (ret == -ERESTARTSYS) {
+                mfc_err("Interrupted by a signal\n");
+                return 1;
+        }
+        mfc_debug(1, "Finished waiting (dev->int_type:%d, command: %d)\n",
+                                                        dev->int_type, command);
+        if (dev->int_type == S5P_FIMV_R2H_CMD_ERR_RET)
+                return 1;
+        return 0;
+}
+
+void s5p_mfc_clean_dev_int_flags(struct s5p_mfc_dev *dev)
+{
+        dev->int_cond = 0;
+        dev->int_type = 0;
+        dev->int_err = 0;
+}
+
+int s5p_mfc_wait_for_done_ctx(struct s5p_mfc_ctx *ctx,
+                                    int command, int interrupt)
+{
+        int ret;
+
+        if (interrupt) {
+                ret = wait_event_interruptible_timeout(ctx->queue,
+                                (ctx->int_cond && (ctx->int_type == command
+                        || ctx->int_type == S5P_FIMV_R2H_CMD_ERR_RET)),
+                                        msecs_to_jiffies(MFC_INT_TIMEOUT));
+        } else {
+                ret = wait_event_timeout(ctx->queue,
+                                (ctx->int_cond && (ctx->int_type == command
+                        || ctx->int_type == S5P_FIMV_R2H_CMD_ERR_RET)),
+                                        msecs_to_jiffies(MFC_INT_TIMEOUT));
+        }
+        if (ret == 0) {
+                mfc_err("Interrupt (ctx->int_type:%d, command:%d) timed out\n",
+                                                        ctx->int_type, command);
+                return 1;
+        } else if (ret == -ERESTARTSYS) {
+                mfc_err("Interrupted by a signal\n");
+                return 1;
+        }
+        mfc_debug(1, "Finished waiting (ctx->int_type:%d, command: %d)\n",
+                                                        ctx->int_type, command);
+        if (ctx->int_type == S5P_FIMV_R2H_CMD_ERR_RET)
+                return 1;
+        return 0;
+}
+
+void s5p_mfc_clean_ctx_int_flags(struct s5p_mfc_ctx *ctx)
+{
+        ctx->int_cond = 0;
+        ctx->int_type = 0;
+        ctx->int_err = 0;
+}
+
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_intr.h b/drivers/media/video/s5p-mfc/s5p_mfc_intr.h
new file mode 100644
index 00000000000..122d7732f74
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_intr.h
@@ -0,0 +1,26 @@
+/*
+ * drivers/media/video/samsung/mfc5/s5p_mfc_intr.h
+ *
+ * Header file for Samsung MFC (Multi Function Codec - FIMV) driver
+ * It contains waiting functions declarations.
+ *
+ * Kamil Debski, Copyright (C) 2011 Samsung Electronics
+ * http://www.samsung.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.
+ */
+
+#ifndef S5P_MFC_INTR_H_
+#define S5P_MFC_INTR_H_
+
+#include "s5p_mfc_common.h"
+
+int s5p_mfc_wait_for_done_ctx(struct s5p_mfc_ctx *ctx,
+                              int command, int interrupt);
+int s5p_mfc_wait_for_done_dev(struct s5p_mfc_dev *dev, int command);
+void s5p_mfc_clean_ctx_int_flags(struct s5p_mfc_ctx *ctx);
+void s5p_mfc_clean_dev_int_flags(struct s5p_mfc_dev *dev);
+
+#endif /* S5P_MFC_INTR_H_ */
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_opr.c b/drivers/media/video/s5p-mfc/s5p_mfc_opr.c
new file mode 100644
index 00000000000..7b239168c19
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_opr.c
@@ -0,0 +1,1397 @@
+/*
+ * drivers/media/video/samsung/mfc5/s5p_mfc_opr.c
+ *
+ * Samsung MFC (Multi Function Codec - FIMV) driver
+ * This file contains hw related functions.
+ *
+ * Kamil Debski, Copyright (c) 2011 Samsung Electronics
+ * http://www.samsung.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.
+ */
+
+#include "regs-mfc.h"
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_ctrl.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_opr.h"
+#include "s5p_mfc_pm.h"
+#include "s5p_mfc_shm.h"
+#include <asm/cacheflush.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/firmware.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#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)
+
+/* Allocate temporary buffers for decoding */
+int s5p_mfc_alloc_dec_temp_buffers(struct s5p_mfc_ctx *ctx)
+{
+        void *desc_virt;
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        ctx->desc_buf = vb2_dma_contig_memops.alloc(
+                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], DESC_BUF_SIZE);
+        if (IS_ERR_VALUE((int)ctx->desc_buf)) {
+                ctx->desc_buf = 0;
+                mfc_err("Allocating DESC buffer failed\n");
+                return -ENOMEM;
+        }
+        ctx->desc_phys = s5p_mfc_mem_cookie(
+                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->desc_buf);
+        BUG_ON(ctx->desc_phys & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+        desc_virt = vb2_dma_contig_memops.vaddr(ctx->desc_buf);
+        if (desc_virt == NULL) {
+                vb2_dma_contig_memops.put(ctx->desc_buf);
+                ctx->desc_phys = 0;
+                ctx->desc_buf = 0;
+                mfc_err("Remapping DESC buffer failed\n");
+                return -ENOMEM;
+        }
+        memset(desc_virt, 0, DESC_BUF_SIZE);
+        wmb();
+        return 0;
+}
+
+/* Release temporary buffers for decoding */
+void s5p_mfc_release_dec_desc_buffer(struct s5p_mfc_ctx *ctx)
+{
+        if (ctx->desc_phys) {
+                vb2_dma_contig_memops.put(ctx->desc_buf);
+                ctx->desc_phys = 0;
+                ctx->desc_buf = 0;
+        }
+}
+
+/* Allocate codec buffers */
+int s5p_mfc_alloc_codec_buffers(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned int enc_ref_y_size = 0;
+        unsigned int enc_ref_c_size = 0;
+        unsigned int guard_width, guard_height;
+
+        if (ctx->type == MFCINST_DECODER) {
+                mfc_debug(2, "Luma size:%d Chroma size:%d MV size:%d\n",
+                          ctx->luma_size, ctx->chroma_size, ctx->mv_size);
+                mfc_debug(2, "Totals bufs: %d\n", ctx->total_dpb_count);
+        } else if (ctx->type == MFCINST_ENCODER) {
+                enc_ref_y_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+                        * ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
+                enc_ref_y_size = ALIGN(enc_ref_y_size, S5P_FIMV_NV12MT_SALIGN);
+
+                if (ctx->codec_mode == S5P_FIMV_CODEC_H264_ENC) {
+                        enc_ref_c_size = ALIGN(ctx->img_width,
+                                                S5P_FIMV_NV12MT_HALIGN)
+                                                * ALIGN(ctx->img_height >> 1,
+                                                S5P_FIMV_NV12MT_VALIGN);
+                        enc_ref_c_size = ALIGN(enc_ref_c_size,
+                                                        S5P_FIMV_NV12MT_SALIGN);
+                } else {
+                        guard_width = ALIGN(ctx->img_width + 16,
+                                                        S5P_FIMV_NV12MT_HALIGN);
+                        guard_height = ALIGN((ctx->img_height >> 1) + 4,
+                                                        S5P_FIMV_NV12MT_VALIGN);
+                        enc_ref_c_size = ALIGN(guard_width * guard_height,
+                                               S5P_FIMV_NV12MT_SALIGN);
+                }
+                mfc_debug(2, "recon luma size: %d chroma size: %d\n",
+                          enc_ref_y_size, enc_ref_c_size);
+        } else {
+                return -EINVAL;
+        }
+        /* Codecs have different memory requirements */
+        switch (ctx->codec_mode) {
+        case S5P_FIMV_CODEC_H264_DEC:
+                ctx->bank1_size =
+                    ALIGN(S5P_FIMV_DEC_NB_IP_SIZE +
+                                        S5P_FIMV_DEC_VERT_NB_MV_SIZE,
+                                        S5P_FIMV_DEC_BUF_ALIGN);
+                ctx->bank2_size = ctx->total_dpb_count * ctx->mv_size;
+                break;
+        case S5P_FIMV_CODEC_MPEG4_DEC:
+                ctx->bank1_size =
+                    ALIGN(S5P_FIMV_DEC_NB_DCAC_SIZE +
+                                     S5P_FIMV_DEC_UPNB_MV_SIZE +
+                                     S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
+                                     S5P_FIMV_DEC_STX_PARSER_SIZE +
+                                     S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE,
+                                     S5P_FIMV_DEC_BUF_ALIGN);
+                ctx->bank2_size = 0;
+                break;
+        case S5P_FIMV_CODEC_VC1RCV_DEC:
+        case S5P_FIMV_CODEC_VC1_DEC:
+                ctx->bank1_size =
+                    ALIGN(S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE +
+                             S5P_FIMV_DEC_UPNB_MV_SIZE +
+                             S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
+                             S5P_FIMV_DEC_NB_DCAC_SIZE +
+                             3 * S5P_FIMV_DEC_VC1_BITPLANE_SIZE,
+                             S5P_FIMV_DEC_BUF_ALIGN);
+                ctx->bank2_size = 0;
+                break;
+        case S5P_FIMV_CODEC_MPEG2_DEC:
+                ctx->bank1_size = 0;
+                ctx->bank2_size = 0;
+                break;
+        case S5P_FIMV_CODEC_H263_DEC:
+                ctx->bank1_size =
+                    ALIGN(S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE +
+                             S5P_FIMV_DEC_UPNB_MV_SIZE +
+                             S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
+                             S5P_FIMV_DEC_NB_DCAC_SIZE,
+                             S5P_FIMV_DEC_BUF_ALIGN);
+                ctx->bank2_size = 0;
+                break;
+        case S5P_FIMV_CODEC_H264_ENC:
+                ctx->bank1_size = (enc_ref_y_size * 2) +
+                                   S5P_FIMV_ENC_UPMV_SIZE +
+                                   S5P_FIMV_ENC_COLFLG_SIZE +
+                                   S5P_FIMV_ENC_INTRAMD_SIZE +
+                                   S5P_FIMV_ENC_NBORINFO_SIZE;
+                ctx->bank2_size = (enc_ref_y_size * 2) +
+                                   (enc_ref_c_size * 4) +
+                                   S5P_FIMV_ENC_INTRAPRED_SIZE;
+                break;
+        case S5P_FIMV_CODEC_MPEG4_ENC:
+                ctx->bank1_size = (enc_ref_y_size * 2) +
+                                   S5P_FIMV_ENC_UPMV_SIZE +
+                                   S5P_FIMV_ENC_COLFLG_SIZE +
+                                   S5P_FIMV_ENC_ACDCCOEF_SIZE;
+                ctx->bank2_size = (enc_ref_y_size * 2) +
+                                   (enc_ref_c_size * 4);
+                break;
+        case S5P_FIMV_CODEC_H263_ENC:
+                ctx->bank1_size = (enc_ref_y_size * 2) +
+                                   S5P_FIMV_ENC_UPMV_SIZE +
+                                   S5P_FIMV_ENC_ACDCCOEF_SIZE;
+                ctx->bank2_size = (enc_ref_y_size * 2) +
+                                   (enc_ref_c_size * 4);
+                break;
+        default:
+                break;
+        }
+        /* Allocate only if memory from bank 1 is necessary */
+        if (ctx->bank1_size > 0) {
+                ctx->bank1_buf = vb2_dma_contig_memops.alloc(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_size);
+                if (IS_ERR(ctx->bank1_buf)) {
+                        ctx->bank1_buf = 0;
+                        printk(KERN_ERR
+                               "Buf alloc for decoding failed (port A)\n");
+                        return -ENOMEM;
+                }
+                ctx->bank1_phys = s5p_mfc_mem_cookie(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_buf);
+                BUG_ON(ctx->bank1_phys & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+        }
+        /* Allocate only if memory from bank 2 is necessary */
+        if (ctx->bank2_size > 0) {
+                ctx->bank2_buf = vb2_dma_contig_memops.alloc(
+                dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], ctx->bank2_size);
+                if (IS_ERR(ctx->bank2_buf)) {
+                        ctx->bank2_buf = 0;
+                        mfc_err("Buf alloc for decoding failed (port B)\n");
+                        return -ENOMEM;
+                }
+                ctx->bank2_phys = s5p_mfc_mem_cookie(
+                dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], ctx->bank2_buf);
+                BUG_ON(ctx->bank2_phys & ((1 << MFC_BANK2_ALIGN_ORDER) - 1));
+        }
+        return 0;
+}
+
+/* Release buffers allocated for codec */
+void s5p_mfc_release_codec_buffers(struct s5p_mfc_ctx *ctx)
+{
+        if (ctx->bank1_buf) {
+                vb2_dma_contig_memops.put(ctx->bank1_buf);
+                ctx->bank1_buf = 0;
+                ctx->bank1_phys = 0;
+                ctx->bank1_size = 0;
+        }
+        if (ctx->bank2_buf) {
+                vb2_dma_contig_memops.put(ctx->bank2_buf);
+                ctx->bank2_buf = 0;
+                ctx->bank2_phys = 0;
+                ctx->bank2_size = 0;
+        }
+}
+
+/* Allocate memory for instance data buffer */
+int s5p_mfc_alloc_instance_buffer(struct s5p_mfc_ctx *ctx)
+{
+        void *context_virt;
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC ||
+                ctx->codec_mode == S5P_FIMV_CODEC_H264_ENC)
+                ctx->ctx_size = MFC_H264_CTX_BUF_SIZE;
+        else
+                ctx->ctx_size = MFC_CTX_BUF_SIZE;
+        ctx->ctx_buf = vb2_dma_contig_memops.alloc(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx_size);
+        if (IS_ERR(ctx->ctx_buf)) {
+                mfc_err("Allocating context buffer failed\n");
+                ctx->ctx_phys = 0;
+                ctx->ctx_buf = 0;
+                return -ENOMEM;
+        }
+        ctx->ctx_phys = s5p_mfc_mem_cookie(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx_buf);
+        BUG_ON(ctx->ctx_phys & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+        ctx->ctx_ofs = OFFSETA(ctx->ctx_phys);
+        context_virt = vb2_dma_contig_memops.vaddr(ctx->ctx_buf);
+        if (context_virt == NULL) {
+                mfc_err("Remapping instance buffer failed\n");
+                vb2_dma_contig_memops.put(ctx->ctx_buf);
+                ctx->ctx_phys = 0;
+                ctx->ctx_buf = 0;
+                return -ENOMEM;
+        }
+        /* Zero content of the allocated memory */
+        memset(context_virt, 0, ctx->ctx_size);
+        wmb();
+        if (s5p_mfc_init_shm(ctx) < 0) {
+                vb2_dma_contig_memops.put(ctx->ctx_buf);
+                ctx->ctx_phys = 0;
+                ctx->ctx_buf = 0;
+                return -ENOMEM;
+        }
+        return 0;
+}
+
+/* Release instance buffer */
+void s5p_mfc_release_instance_buffer(struct s5p_mfc_ctx *ctx)
+{
+        if (ctx->ctx_buf) {
+                vb2_dma_contig_memops.put(ctx->ctx_buf);
+                ctx->ctx_phys = 0;
+                ctx->ctx_buf = 0;
+        }
+        if (ctx->shm_alloc) {
+                vb2_dma_contig_memops.put(ctx->shm_alloc);
+                ctx->shm_alloc = 0;
+                ctx->shm = 0;
+        }
+}
+
+/* Set registers for decoding temporary buffers */
+void s5p_mfc_set_dec_desc_buffer(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mfc_write(dev, OFFSETA(ctx->desc_phys), S5P_FIMV_SI_CH0_DESC_ADR);
+        mfc_write(dev, DESC_BUF_SIZE, S5P_FIMV_SI_CH0_DESC_SIZE);
+}
+
+/* Set registers for shared buffer */
+void s5p_mfc_set_shared_buffer(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        mfc_write(dev, ctx->shm_ofs, S5P_FIMV_SI_CH0_HOST_WR_ADR);
+}
+
+/* Set registers for decoding stream buffer */
+int s5p_mfc_set_dec_stream_buffer(struct s5p_mfc_ctx *ctx, int buf_addr,
+                  unsigned int start_num_byte, unsigned int buf_size)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mfc_write(dev, OFFSETA(buf_addr), S5P_FIMV_SI_CH0_SB_ST_ADR);
+        mfc_write(dev, ctx->dec_src_buf_size, S5P_FIMV_SI_CH0_CPB_SIZE);
+        mfc_write(dev, buf_size, S5P_FIMV_SI_CH0_SB_FRM_SIZE);
+        s5p_mfc_write_shm(ctx, start_num_byte, START_BYTE_NUM);
+        return 0;
+}
+
+/* Set decoding frame buffer */
+int s5p_mfc_set_dec_frame_buffer(struct s5p_mfc_ctx *ctx)
+{
+        unsigned int frame_size, i;
+        unsigned int frame_size_ch, frame_size_mv;
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned int dpb;
+        size_t buf_addr1, buf_addr2;
+        int buf_size1, buf_size2;
+
+        buf_addr1 = ctx->bank1_phys;
+        buf_size1 = ctx->bank1_size;
+        buf_addr2 = ctx->bank2_phys;
+        buf_size2 = ctx->bank2_size;
+        dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) &
+                                                ~S5P_FIMV_DPB_COUNT_MASK;
+        mfc_write(dev, ctx->total_dpb_count | dpb,
+                                                S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
+        s5p_mfc_set_shared_buffer(ctx);
+        switch (ctx->codec_mode) {
+        case S5P_FIMV_CODEC_H264_DEC:
+                mfc_write(dev, OFFSETA(buf_addr1),
+                                                S5P_FIMV_H264_VERT_NB_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_VERT_NB_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_VERT_NB_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H264_NB_IP_ADR);
+                buf_addr1 += S5P_FIMV_DEC_NB_IP_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_NB_IP_SIZE;
+                break;
+        case S5P_FIMV_CODEC_MPEG4_DEC:
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_NB_DCAC_ADR);
+                buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_UP_NB_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_SA_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_SP_ADR);
+                buf_addr1 += S5P_FIMV_DEC_STX_PARSER_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_STX_PARSER_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_OT_LINE_ADR);
+                buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+                break;
+        case S5P_FIMV_CODEC_H263_DEC:
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_OT_LINE_ADR);
+                buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_UP_NB_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_SA_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_NB_DCAC_ADR);
+                buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
+                break;
+        case S5P_FIMV_CODEC_VC1_DEC:
+        case S5P_FIMV_CODEC_VC1RCV_DEC:
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_NB_DCAC_ADR);
+                buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_OT_LINE_ADR);
+                buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_UP_NB_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_SA_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE3_ADR);
+                buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE2_ADR);
+                buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE1_ADR);
+                buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+                break;
+        case S5P_FIMV_CODEC_MPEG2_DEC:
+                break;
+        default:
+                mfc_err("Unknown codec for decoding (%x)\n",
+                        ctx->codec_mode);
+                return -EINVAL;
+                break;
+        }
+        frame_size = ctx->luma_size;
+        frame_size_ch = ctx->chroma_size;
+        frame_size_mv = ctx->mv_size;
+        mfc_debug(2, "Frm size: %d ch: %d mv: %d\n", frame_size, frame_size_ch,
+                                                                frame_size_mv);
+        for (i = 0; i < ctx->total_dpb_count; i++) {
+                /* Bank2 */
+                mfc_debug(2, "Luma %d: %x\n", i,
+                                        ctx->dst_bufs[i].cookie.raw.luma);
+                mfc_write(dev, OFFSETB(ctx->dst_bufs[i].cookie.raw.luma),
+                                                S5P_FIMV_DEC_LUMA_ADR + i * 4);
+                mfc_debug(2, "\tChroma %d: %x\n", i,
+                                        ctx->dst_bufs[i].cookie.raw.chroma);
+                mfc_write(dev, OFFSETA(ctx->dst_bufs[i].cookie.raw.chroma),
+                                               S5P_FIMV_DEC_CHROMA_ADR + i * 4);
+                if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC) {
+                        mfc_debug(2, "\tBuf2: %x, size: %d\n",
+                                                        buf_addr2, buf_size2);
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                                S5P_FIMV_H264_MV_ADR + i * 4);
+                        buf_addr2 += frame_size_mv;
+                        buf_size2 -= frame_size_mv;
+                }
+        }
+        mfc_debug(2, "Buf1: %u, buf_size1: %d\n", buf_addr1, buf_size1);
+        mfc_debug(2, "Buf 1/2 size after: %d/%d (frames %d)\n",
+                        buf_size1,  buf_size2, ctx->total_dpb_count);
+        if (buf_size1 < 0 || buf_size2 < 0) {
+                mfc_debug(2, "Not enough memory has been allocated\n");
+                return -ENOMEM;
+        }
+        s5p_mfc_write_shm(ctx, frame_size, ALLOC_LUMA_DPB_SIZE);
+        s5p_mfc_write_shm(ctx, frame_size_ch, ALLOC_CHROMA_DPB_SIZE);
+        if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC)
+                s5p_mfc_write_shm(ctx, frame_size_mv, ALLOC_MV_SIZE);
+        mfc_write(dev, ((S5P_FIMV_CH_INIT_BUFS & S5P_FIMV_CH_MASK)
+                                        << S5P_FIMV_CH_SHIFT) | (ctx->inst_no),
+                                                S5P_FIMV_SI_CH0_INST_ID);
+        return 0;
+}
+
+/* Set registers for encoding stream buffer */
+int s5p_mfc_set_enc_stream_buffer(struct s5p_mfc_ctx *ctx,
+                unsigned long addr, unsigned int size)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mfc_write(dev, OFFSETA(addr), S5P_FIMV_ENC_SI_CH0_SB_ADR);
+        mfc_write(dev, size, S5P_FIMV_ENC_SI_CH0_SB_SIZE);
+        return 0;
+}
+
+void s5p_mfc_set_enc_frame_buffer(struct s5p_mfc_ctx *ctx,
+                unsigned long y_addr, unsigned long c_addr)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mfc_write(dev, OFFSETB(y_addr), S5P_FIMV_ENC_SI_CH0_CUR_Y_ADR);
+        mfc_write(dev, OFFSETB(c_addr), S5P_FIMV_ENC_SI_CH0_CUR_C_ADR);
+}
+
+void s5p_mfc_get_enc_frame_buffer(struct s5p_mfc_ctx *ctx,
+                unsigned long *y_addr, unsigned long *c_addr)
+{
+        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);
+}
+
+/* Set encoding ref & codec buffer */
+int s5p_mfc_set_enc_ref_buffer(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        size_t buf_addr1, buf_addr2;
+        size_t buf_size1, buf_size2;
+        unsigned int enc_ref_y_size, enc_ref_c_size;
+        unsigned int guard_width, guard_height;
+        int i;
+
+        buf_addr1 = ctx->bank1_phys;
+        buf_size1 = ctx->bank1_size;
+        buf_addr2 = ctx->bank2_phys;
+        buf_size2 = ctx->bank2_size;
+        enc_ref_y_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+                * ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
+        enc_ref_y_size = ALIGN(enc_ref_y_size, S5P_FIMV_NV12MT_SALIGN);
+        if (ctx->codec_mode == S5P_FIMV_CODEC_H264_ENC) {
+                enc_ref_c_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+                        * ALIGN((ctx->img_height >> 1), S5P_FIMV_NV12MT_VALIGN);
+                enc_ref_c_size = ALIGN(enc_ref_c_size, S5P_FIMV_NV12MT_SALIGN);
+        } else {
+                guard_width = ALIGN(ctx->img_width + 16,
+                                                S5P_FIMV_NV12MT_HALIGN);
+                guard_height = ALIGN((ctx->img_height >> 1) + 4,
+                                                S5P_FIMV_NV12MT_VALIGN);
+                enc_ref_c_size = ALIGN(guard_width * guard_height,
+                                       S5P_FIMV_NV12MT_SALIGN);
+        }
+        mfc_debug(2, "buf_size1: %d, buf_size2: %d\n", buf_size1, buf_size2);
+        switch (ctx->codec_mode) {
+        case S5P_FIMV_CODEC_H264_ENC:
+                for (i = 0; i < 2; i++) {
+                        mfc_write(dev, OFFSETA(buf_addr1),
+                                S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
+                        buf_addr1 += enc_ref_y_size;
+                        buf_size1 -= enc_ref_y_size;
+
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
+                        buf_addr2 += enc_ref_y_size;
+                        buf_size2 -= enc_ref_y_size;
+                }
+                for (i = 0; i < 4; i++) {
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
+                        buf_addr2 += enc_ref_c_size;
+                        buf_size2 -= enc_ref_c_size;
+                }
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H264_UP_MV_ADR);
+                buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1),
+                                        S5P_FIMV_H264_COZERO_FLAG_ADR);
+                buf_addr1 += S5P_FIMV_ENC_COLFLG_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_COLFLG_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1),
+                                        S5P_FIMV_H264_UP_INTRA_MD_ADR);
+                buf_addr1 += S5P_FIMV_ENC_INTRAMD_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_INTRAMD_SIZE;
+                mfc_write(dev, OFFSETB(buf_addr2),
+                                        S5P_FIMV_H264_UP_INTRA_PRED_ADR);
+                buf_addr2 += S5P_FIMV_ENC_INTRAPRED_SIZE;
+                buf_size2 -= S5P_FIMV_ENC_INTRAPRED_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1),
+                                        S5P_FIMV_H264_NBOR_INFO_ADR);
+                buf_addr1 += S5P_FIMV_ENC_NBORINFO_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_NBORINFO_SIZE;
+                mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
+                        buf_size1, buf_size2);
+                break;
+        case S5P_FIMV_CODEC_MPEG4_ENC:
+                for (i = 0; i < 2; i++) {
+                        mfc_write(dev, OFFSETA(buf_addr1),
+                                S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
+                        buf_addr1 += enc_ref_y_size;
+                        buf_size1 -= enc_ref_y_size;
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
+                        buf_addr2 += enc_ref_y_size;
+                        buf_size2 -= enc_ref_y_size;
+                }
+                for (i = 0; i < 4; i++) {
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
+                        buf_addr2 += enc_ref_c_size;
+                        buf_size2 -= enc_ref_c_size;
+                }
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_UP_MV_ADR);
+                buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1),
+                                                S5P_FIMV_MPEG4_COZERO_FLAG_ADR);
+                buf_addr1 += S5P_FIMV_ENC_COLFLG_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_COLFLG_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1),
+                                                S5P_FIMV_MPEG4_ACDC_COEF_ADR);
+                buf_addr1 += S5P_FIMV_ENC_ACDCCOEF_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_ACDCCOEF_SIZE;
+                mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
+                        buf_size1, buf_size2);
+                break;
+        case S5P_FIMV_CODEC_H263_ENC:
+                for (i = 0; i < 2; i++) {
+                        mfc_write(dev, OFFSETA(buf_addr1),
+                                S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
+                        buf_addr1 += enc_ref_y_size;
+                        buf_size1 -= enc_ref_y_size;
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
+                        buf_addr2 += enc_ref_y_size;
+                        buf_size2 -= enc_ref_y_size;
+                }
+                for (i = 0; i < 4; i++) {
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
+                        buf_addr2 += enc_ref_c_size;
+                        buf_size2 -= enc_ref_c_size;
+                }
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_UP_MV_ADR);
+                buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_ACDC_COEF_ADR);
+                buf_addr1 += S5P_FIMV_ENC_ACDCCOEF_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_ACDCCOEF_SIZE;
+                mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
+                        buf_size1, buf_size2);
+                break;
+        default:
+                mfc_err("Unknown codec set for encoding: %d\n",
+                        ctx->codec_mode);
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        unsigned int reg;
+        unsigned int shm;
+
+        /* width */
+        mfc_write(dev, ctx->img_width, S5P_FIMV_ENC_HSIZE_PX);
+        /* height */
+        mfc_write(dev, ctx->img_height, S5P_FIMV_ENC_VSIZE_PX);
+        /* pictype : enable, IDR period */
+        reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+        reg |= (1 << 18);
+        reg &= ~(0xFFFF);
+        reg |= p->gop_size;
+        mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+        mfc_write(dev, 0, S5P_FIMV_ENC_B_RECON_WRITE_ON);
+        /* multi-slice control */
+        /* multi-slice MB number or bit size */
+        mfc_write(dev, p->slice_mode, S5P_FIMV_ENC_MSLICE_CTRL);
+        if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB) {
+                mfc_write(dev, p->slice_mb, S5P_FIMV_ENC_MSLICE_MB);
+        } else if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES) {
+                mfc_write(dev, p->slice_bit, S5P_FIMV_ENC_MSLICE_BIT);
+        } else {
+                mfc_write(dev, 0, S5P_FIMV_ENC_MSLICE_MB);
+                mfc_write(dev, 0, S5P_FIMV_ENC_MSLICE_BIT);
+        }
+        /* cyclic intra refresh */
+        mfc_write(dev, p->intra_refresh_mb, S5P_FIMV_ENC_CIR_CTRL);
+        /* memory structure cur. frame */
+        if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M)
+                mfc_write(dev, 0, S5P_FIMV_ENC_MAP_FOR_CUR);
+        else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT)
+                mfc_write(dev, 3, S5P_FIMV_ENC_MAP_FOR_CUR);
+        /* padding control & value */
+        reg = mfc_read(dev, S5P_FIMV_ENC_PADDING_CTRL);
+        if (p->pad) {
+                /** enable */
+                reg |= (1 << 31);
+                /** cr value */
+                reg &= ~(0xFF << 16);
+                reg |= (p->pad_cr << 16);
+                /** cb value */
+                reg &= ~(0xFF << 8);
+                reg |= (p->pad_cb << 8);
+                /** y value */
+                reg &= ~(0xFF);
+                reg |= (p->pad_luma);
+        } else {
+                /** disable & all value clear */
+                reg = 0;
+        }
+        mfc_write(dev, reg, S5P_FIMV_ENC_PADDING_CTRL);
+        /* rate control config. */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+        /** frame-level rate control */
+        reg &= ~(0x1 << 9);
+        reg |= (p->rc_frame << 9);
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+        /* bit rate */
+        if (p->rc_frame)
+                mfc_write(dev, p->rc_bitrate,
+                        S5P_FIMV_ENC_RC_BIT_RATE);
+        else
+                mfc_write(dev, 0, S5P_FIMV_ENC_RC_BIT_RATE);
+        /* reaction coefficient */
+        if (p->rc_frame)
+                mfc_write(dev, p->rc_reaction_coeff, S5P_FIMV_ENC_RC_RPARA);
+        shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
+        /* seq header ctrl */
+        shm &= ~(0x1 << 3);
+        shm |= (p->seq_hdr_mode << 3);
+        /* frame skip mode */
+        shm &= ~(0x3 << 1);
+        shm |= (p->frame_skip_mode << 1);
+        s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
+        /* fixed target bit */
+        s5p_mfc_write_shm(ctx, p->fixed_target_bit, RC_CONTROL_CONFIG);
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        struct s5p_mfc_h264_enc_params *p_264 = &p->codec.h264;
+        unsigned int reg;
+        unsigned int shm;
+
+        s5p_mfc_set_enc_params(ctx);
+        /* pictype : number of B */
+        reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+        /* num_b_frame - 0 ~ 2 */
+        reg &= ~(0x3 << 16);
+        reg |= (p->num_b_frame << 16);
+        mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+        /* profile & level */
+        reg = mfc_read(dev, S5P_FIMV_ENC_PROFILE);
+        /* level */
+        reg &= ~(0xFF << 8);
+        reg |= (p_264->level << 8);
+        /* profile - 0 ~ 2 */
+        reg &= ~(0x3F);
+        reg |= p_264->profile;
+        mfc_write(dev, reg, S5P_FIMV_ENC_PROFILE);
+        /* interlace  */
+        mfc_write(dev, p->interlace, S5P_FIMV_ENC_PIC_STRUCT);
+        /* height */
+        if (p->interlace)
+                mfc_write(dev, ctx->img_height >> 1, S5P_FIMV_ENC_VSIZE_PX);
+        /* loopfilter ctrl */
+        mfc_write(dev, p_264->loop_filter_mode, S5P_FIMV_ENC_LF_CTRL);
+        /* loopfilter alpha offset */
+        if (p_264->loop_filter_alpha < 0) {
+                reg = 0x10;
+                reg |= (0xFF - p_264->loop_filter_alpha) + 1;
+        } else {
+                reg = 0x00;
+                reg |= (p_264->loop_filter_alpha & 0xF);
+        }
+        mfc_write(dev, reg, S5P_FIMV_ENC_ALPHA_OFF);
+        /* loopfilter beta offset */
+        if (p_264->loop_filter_beta < 0) {
+                reg = 0x10;
+                reg |= (0xFF - p_264->loop_filter_beta) + 1;
+        } else {
+                reg = 0x00;
+                reg |= (p_264->loop_filter_beta & 0xF);
+        }
+        mfc_write(dev, reg, S5P_FIMV_ENC_BETA_OFF);
+        /* entropy coding mode */
+        if (p_264->entropy_mode == V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC)
+                mfc_write(dev, 1, S5P_FIMV_ENC_H264_ENTROPY_MODE);
+        else
+                mfc_write(dev, 0, S5P_FIMV_ENC_H264_ENTROPY_MODE);
+        /* number of ref. picture */
+        reg = mfc_read(dev, S5P_FIMV_ENC_H264_NUM_OF_REF);
+        /* num of ref. pictures of P */
+        reg &= ~(0x3 << 5);
+        reg |= (p_264->num_ref_pic_4p << 5);
+        /* max number of ref. pictures */
+        reg &= ~(0x1F);
+        reg |= p_264->max_ref_pic;
+        mfc_write(dev, reg, S5P_FIMV_ENC_H264_NUM_OF_REF);
+        /* 8x8 transform enable */
+        mfc_write(dev, p_264->_8x8_transform, S5P_FIMV_ENC_H264_TRANS_FLAG);
+        /* rate control config. */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+        /* macroblock level rate control */
+        reg &= ~(0x1 << 8);
+        reg |= (p_264->rc_mb << 8);
+        /* frame QP */
+        reg &= ~(0x3F);
+        reg |= p_264->rc_frame_qp;
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+        /* frame rate */
+        if (p->rc_frame && p->rc_framerate_denom)
+                mfc_write(dev, p->rc_framerate_num * 1000
+                        / p->rc_framerate_denom, S5P_FIMV_ENC_RC_FRAME_RATE);
+        else
+                mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
+        /* max & min value of QP */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
+        /* max QP */
+        reg &= ~(0x3F << 8);
+        reg |= (p_264->rc_max_qp << 8);
+        /* min QP */
+        reg &= ~(0x3F);
+        reg |= p_264->rc_min_qp;
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
+        /* macroblock adaptive scaling features */
+        if (p_264->rc_mb) {
+                reg = mfc_read(dev, S5P_FIMV_ENC_RC_MB_CTRL);
+                /* dark region */
+                reg &= ~(0x1 << 3);
+                reg |= (p_264->rc_mb_dark << 3);
+                /* smooth region */
+                reg &= ~(0x1 << 2);
+                reg |= (p_264->rc_mb_smooth << 2);
+                /* static region */
+                reg &= ~(0x1 << 1);
+                reg |= (p_264->rc_mb_static << 1);
+                /* high activity region */
+                reg &= ~(0x1);
+                reg |= p_264->rc_mb_activity;
+                mfc_write(dev, reg, S5P_FIMV_ENC_RC_MB_CTRL);
+        }
+        if (!p->rc_frame &&
+            !p_264->rc_mb) {
+                shm = s5p_mfc_read_shm(ctx, P_B_FRAME_QP);
+                shm &= ~(0xFFF);
+                shm |= ((p_264->rc_b_frame_qp & 0x3F) << 6);
+                shm |= (p_264->rc_p_frame_qp & 0x3F);
+                s5p_mfc_write_shm(ctx, shm, P_B_FRAME_QP);
+        }
+        /* extended encoder ctrl */
+        shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
+        /* AR VUI control */
+        shm &= ~(0x1 << 15);
+        shm |= (p_264->vui_sar << 1);
+        s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
+        if (p_264->vui_sar) {
+                /* aspect ration IDC */
+                shm = s5p_mfc_read_shm(ctx, SAMPLE_ASPECT_RATIO_IDC);
+                shm &= ~(0xFF);
+                shm |= p_264->vui_sar_idc;
+                s5p_mfc_write_shm(ctx, shm, SAMPLE_ASPECT_RATIO_IDC);
+                if (p_264->vui_sar_idc == 0xFF) {
+                        /* sample  AR info */
+                        shm = s5p_mfc_read_shm(ctx, EXTENDED_SAR);
+                        shm &= ~(0xFFFFFFFF);
+                        shm |= p_264->vui_ext_sar_width << 16;
+                        shm |= p_264->vui_ext_sar_height;
+                        s5p_mfc_write_shm(ctx, shm, EXTENDED_SAR);
+                }
+        }
+        /* intra picture period for H.264 */
+        shm = s5p_mfc_read_shm(ctx, H264_I_PERIOD);
+        /* control */
+        shm &= ~(0x1 << 16);
+        shm |= (p_264->open_gop << 16);
+        /* value */
+        if (p_264->open_gop) {
+                shm &= ~(0xFFFF);
+                shm |= p_264->open_gop_size;
+        }
+        s5p_mfc_write_shm(ctx, shm, H264_I_PERIOD);
+        /* extended encoder ctrl */
+        shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
+        /* vbv buffer size */
+        if (p->frame_skip_mode ==
+                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+                shm &= ~(0xFFFF << 16);
+                shm |= (p_264->cpb_size << 16);
+        }
+        s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params_mpeg4(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        struct s5p_mfc_mpeg4_enc_params *p_mpeg4 = &p->codec.mpeg4;
+        unsigned int reg;
+        unsigned int shm;
+        unsigned int framerate;
+
+        s5p_mfc_set_enc_params(ctx);
+        /* pictype : number of B */
+        reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+        /* num_b_frame - 0 ~ 2 */
+        reg &= ~(0x3 << 16);
+        reg |= (p->num_b_frame << 16);
+        mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+        /* profile & level */
+        reg = mfc_read(dev, S5P_FIMV_ENC_PROFILE);
+        /* level */
+        reg &= ~(0xFF << 8);
+        reg |= (p_mpeg4->level << 8);
+        /* profile - 0 ~ 2 */
+        reg &= ~(0x3F);
+        reg |= p_mpeg4->profile;
+        mfc_write(dev, reg, S5P_FIMV_ENC_PROFILE);
+        /* quarter_pixel */
+        mfc_write(dev, p_mpeg4->quarter_pixel, S5P_FIMV_ENC_MPEG4_QUART_PXL);
+        /* qp */
+        if (!p->rc_frame) {
+                shm = s5p_mfc_read_shm(ctx, P_B_FRAME_QP);
+                shm &= ~(0xFFF);
+                shm |= ((p_mpeg4->rc_b_frame_qp & 0x3F) << 6);
+                shm |= (p_mpeg4->rc_p_frame_qp & 0x3F);
+                s5p_mfc_write_shm(ctx, shm, P_B_FRAME_QP);
+        }
+        /* frame rate */
+        if (p->rc_frame) {
+                if (p->rc_framerate_denom > 0) {
+                        framerate = p->rc_framerate_num * 1000 /
+                                                p->rc_framerate_denom;
+                        mfc_write(dev, framerate,
+                                S5P_FIMV_ENC_RC_FRAME_RATE);
+                        shm = s5p_mfc_read_shm(ctx, RC_VOP_TIMING);
+                        shm &= ~(0xFFFFFFFF);
+                        shm |= (1 << 31);
+                        shm |= ((p->rc_framerate_num & 0x7FFF) << 16);
+                        shm |= (p->rc_framerate_denom & 0xFFFF);
+                        s5p_mfc_write_shm(ctx, shm, RC_VOP_TIMING);
+                }
+        } else {
+                mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
+        }
+        /* rate control config. */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+        /* frame QP */
+        reg &= ~(0x3F);
+        reg |= p_mpeg4->rc_frame_qp;
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+        /* max & min value of QP */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
+        /* max QP */
+        reg &= ~(0x3F << 8);
+        reg |= (p_mpeg4->rc_max_qp << 8);
+        /* min QP */
+        reg &= ~(0x3F);
+        reg |= p_mpeg4->rc_min_qp;
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
+        /* extended encoder ctrl */
+        shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
+        /* vbv buffer size */
+        if (p->frame_skip_mode ==
+                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+                shm &= ~(0xFFFF << 16);
+                shm |= (p->vbv_size << 16);
+        }
+        s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        struct s5p_mfc_mpeg4_enc_params *p_h263 = &p->codec.mpeg4;
+        unsigned int reg;
+        unsigned int shm;
+
+        s5p_mfc_set_enc_params(ctx);
+        /* qp */
+        if (!p->rc_frame) {
+                shm = s5p_mfc_read_shm(ctx, P_B_FRAME_QP);
+                shm &= ~(0xFFF);
+                shm |= (p_h263->rc_p_frame_qp & 0x3F);
+                s5p_mfc_write_shm(ctx, shm, P_B_FRAME_QP);
+        }
+        /* frame rate */
+        if (p->rc_frame && p->rc_framerate_denom)
+                mfc_write(dev, p->rc_framerate_num * 1000
+                        / p->rc_framerate_denom, S5P_FIMV_ENC_RC_FRAME_RATE);
+        else
+                mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
+        /* rate control config. */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+        /* frame QP */
+        reg &= ~(0x3F);
+        reg |= p_h263->rc_frame_qp;
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+        /* max & min value of QP */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
+        /* max QP */
+        reg &= ~(0x3F << 8);
+        reg |= (p_h263->rc_max_qp << 8);
+        /* min QP */
+        reg &= ~(0x3F);
+        reg |= p_h263->rc_min_qp;
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
+        /* extended encoder ctrl */
+        shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
+        /* vbv buffer size */
+        if (p->frame_skip_mode ==
+                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+                shm &= ~(0xFFFF << 16);
+                shm |= (p->vbv_size << 16);
+        }
+        s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
+        return 0;
+}
+
+/* Initialize decoding */
+int s5p_mfc_init_decode(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        s5p_mfc_set_shared_buffer(ctx);
+        /* Setup loop filter, for decoding this is only valid for MPEG4 */
+        if (ctx->codec_mode == S5P_FIMV_CODEC_MPEG4_DEC)
+                mfc_write(dev, ctx->loop_filter_mpeg4, S5P_FIMV_ENC_LF_CTRL);
+        else
+                mfc_write(dev, 0, S5P_FIMV_ENC_LF_CTRL);
+        mfc_write(dev, ((ctx->slice_interface & S5P_FIMV_SLICE_INT_MASK) <<
+                S5P_FIMV_SLICE_INT_SHIFT) | (ctx->display_delay_enable <<
+                S5P_FIMV_DDELAY_ENA_SHIFT) | ((ctx->display_delay &
+                S5P_FIMV_DDELAY_VAL_MASK) << S5P_FIMV_DDELAY_VAL_SHIFT),
+                S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
+        mfc_write(dev,
+        ((S5P_FIMV_CH_SEQ_HEADER & S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT)
+                                | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+        return 0;
+}
+
+static void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned int dpb;
+
+        if (flush)
+                dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) | (
+                        S5P_FIMV_DPB_FLUSH_MASK << S5P_FIMV_DPB_FLUSH_SHIFT);
+        else
+                dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) &
+                        ~(S5P_FIMV_DPB_FLUSH_MASK << S5P_FIMV_DPB_FLUSH_SHIFT);
+        mfc_write(dev, dpb, S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
+}
+
+/* Decode a single frame */
+int s5p_mfc_decode_one_frame(struct s5p_mfc_ctx *ctx,
+                                        enum s5p_mfc_decode_arg last_frame)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mfc_write(dev, ctx->dec_dst_flag, S5P_FIMV_SI_CH0_RELEASE_BUF);
+        s5p_mfc_set_shared_buffer(ctx);
+        s5p_mfc_set_flush(ctx, ctx->dpb_flush_flag);
+        /* Issue different commands to instance basing on whether it
+         * is the last frame or not. */
+        switch (last_frame) {
+        case MFC_DEC_FRAME:
+                mfc_write(dev, ((S5P_FIMV_CH_FRAME_START & S5P_FIMV_CH_MASK) <<
+                S5P_FIMV_CH_SHIFT) | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+                break;
+        case MFC_DEC_LAST_FRAME:
+                mfc_write(dev, ((S5P_FIMV_CH_LAST_FRAME & S5P_FIMV_CH_MASK) <<
+                S5P_FIMV_CH_SHIFT) | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+                break;
+        case MFC_DEC_RES_CHANGE:
+                mfc_write(dev, ((S5P_FIMV_CH_FRAME_START_REALLOC &
+                S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT) | (ctx->inst_no),
+                S5P_FIMV_SI_CH0_INST_ID);
+                break;
+        }
+        mfc_debug(2, "Decoding a usual frame\n");
+        return 0;
+}
+
+int s5p_mfc_init_encode(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        if (ctx->codec_mode == S5P_FIMV_CODEC_H264_ENC)
+                s5p_mfc_set_enc_params_h264(ctx);
+        else if (ctx->codec_mode == S5P_FIMV_CODEC_MPEG4_ENC)
+                s5p_mfc_set_enc_params_mpeg4(ctx);
+        else if (ctx->codec_mode == S5P_FIMV_CODEC_H263_ENC)
+                s5p_mfc_set_enc_params_h263(ctx);
+        else {
+                mfc_err("Unknown codec for encoding (%x)\n",
+                        ctx->codec_mode);
+                return -EINVAL;
+        }
+        s5p_mfc_set_shared_buffer(ctx);
+        mfc_write(dev, ((S5P_FIMV_CH_SEQ_HEADER << 16) & 0x70000) |
+                (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+        return 0;
+}
+
+/* Encode a single frame */
+int s5p_mfc_encode_one_frame(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        /* memory structure cur. frame */
+        if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M)
+                mfc_write(dev, 0, S5P_FIMV_ENC_MAP_FOR_CUR);
+        else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT)
+                mfc_write(dev, 3, S5P_FIMV_ENC_MAP_FOR_CUR);
+        s5p_mfc_set_shared_buffer(ctx);
+        mfc_write(dev, (S5P_FIMV_CH_FRAME_START << 16 & 0x70000) |
+                (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+        return 0;
+}
+
+static int s5p_mfc_get_new_ctx(struct s5p_mfc_dev *dev)
+{
+        unsigned long flags;
+        int new_ctx;
+        int cnt;
+
+        spin_lock_irqsave(&dev->condlock, flags);
+        new_ctx = (dev->curr_ctx + 1) % MFC_NUM_CONTEXTS;
+        cnt = 0;
+        while (!test_bit(new_ctx, &dev->ctx_work_bits)) {
+                new_ctx = (new_ctx + 1) % MFC_NUM_CONTEXTS;
+                if (++cnt > MFC_NUM_CONTEXTS) {
+                        /* No contexts to run */
+                        spin_unlock_irqrestore(&dev->condlock, flags);
+                        return -EAGAIN;
+                }
+        }
+        spin_unlock_irqrestore(&dev->condlock, flags);
+        return new_ctx;
+}
+
+static void s5p_mfc_run_res_change(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        s5p_mfc_set_dec_stream_buffer(ctx, 0, 0, 0);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        s5p_mfc_decode_one_frame(ctx, MFC_DEC_RES_CHANGE);
+}
+
+static int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx, int last_frame)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf *temp_vb;
+        unsigned long flags;
+        unsigned int index;
+
+        spin_lock_irqsave(&dev->irqlock, flags);
+        /* Frames are being decoded */
+        if (list_empty(&ctx->src_queue)) {
+                mfc_debug(2, "No src buffers\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return -EAGAIN;
+        }
+        /* Get the next source buffer */
+        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        temp_vb->used = 1;
+        s5p_mfc_set_dec_stream_buffer(ctx,
+                vb2_dma_contig_plane_paddr(temp_vb->b, 0), ctx->consumed_stream,
+                                        temp_vb->b->v4l2_planes[0].bytesused);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        index = temp_vb->b->v4l2_buf.index;
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        if (temp_vb->b->v4l2_planes[0].bytesused == 0) {
+                last_frame = MFC_DEC_LAST_FRAME;
+                mfc_debug(2, "Setting ctx->state to FINISHING\n");
+                ctx->state = MFCINST_FINISHING;
+        }
+        s5p_mfc_decode_one_frame(ctx, last_frame);
+        return 0;
+}
+
+static int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *dst_mb;
+        struct s5p_mfc_buf *src_mb;
+        unsigned long src_y_addr, src_c_addr, dst_addr;
+        unsigned int dst_size;
+
+        spin_lock_irqsave(&dev->irqlock, flags);
+        if (list_empty(&ctx->src_queue)) {
+                mfc_debug(2, "no src buffers\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return -EAGAIN;
+        }
+        if (list_empty(&ctx->dst_queue)) {
+                mfc_debug(2, "no dst buffers\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return -EAGAIN;
+        }
+        src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        src_mb->used = 1;
+        src_y_addr = vb2_dma_contig_plane_paddr(src_mb->b, 0);
+        src_c_addr = vb2_dma_contig_plane_paddr(src_mb->b, 1);
+        s5p_mfc_set_enc_frame_buffer(ctx, src_y_addr, src_c_addr);
+        dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+        dst_mb->used = 1;
+        dst_addr = vb2_dma_contig_plane_paddr(dst_mb->b, 0);
+        dst_size = vb2_plane_size(dst_mb->b, 0);
+        s5p_mfc_set_enc_stream_buffer(ctx, dst_addr, dst_size);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        s5p_mfc_encode_one_frame(ctx);
+        return 0;
+}
+
+static void s5p_mfc_run_init_dec(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *temp_vb;
+
+        /* Initializing decoding - parsing header */
+        spin_lock_irqsave(&dev->irqlock, flags);
+        mfc_debug(2, "Preparing to init decoding\n");
+        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        s5p_mfc_set_dec_desc_buffer(ctx);
+        mfc_debug(2, "Header size: %d\n", temp_vb->b->v4l2_planes[0].bytesused);
+        s5p_mfc_set_dec_stream_buffer(ctx,
+                                vb2_dma_contig_plane_paddr(temp_vb->b, 0),
+                                0, temp_vb->b->v4l2_planes[0].bytesused);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        s5p_mfc_init_decode(ctx);
+}
+
+static void s5p_mfc_run_init_enc(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *dst_mb;
+        unsigned long dst_addr;
+        unsigned int dst_size;
+
+        s5p_mfc_set_enc_ref_buffer(ctx);
+        spin_lock_irqsave(&dev->irqlock, flags);
+        dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+        dst_addr = vb2_dma_contig_plane_paddr(dst_mb->b, 0);
+        dst_size = vb2_plane_size(dst_mb->b, 0);
+        s5p_mfc_set_enc_stream_buffer(ctx, dst_addr, dst_size);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        s5p_mfc_init_encode(ctx);
+}
+
+static int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *temp_vb;
+        int ret;
+
+        /*
+         * Header was parsed now starting processing
+         * First set the output frame buffers
+         */
+        if (ctx->capture_state != QUEUE_BUFS_MMAPED) {
+                mfc_err("It seems that not all destionation buffers were "
+                        "mmaped\nMFC requires that all destination are mmaped "
+                        "before starting processing\n");
+                return -EAGAIN;
+        }
+        spin_lock_irqsave(&dev->irqlock, flags);
+        if (list_empty(&ctx->src_queue)) {
+                mfc_err("Header has been deallocated in the middle of"
+                        " initialization\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return -EIO;
+        }
+        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        mfc_debug(2, "Header size: %d\n", temp_vb->b->v4l2_planes[0].bytesused);
+        s5p_mfc_set_dec_stream_buffer(ctx,
+                                vb2_dma_contig_plane_paddr(temp_vb->b, 0),
+                                0, temp_vb->b->v4l2_planes[0].bytesused);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        ret = s5p_mfc_set_dec_frame_buffer(ctx);
+        if (ret) {
+                mfc_err("Failed to alloc frame mem\n");
+                ctx->state = MFCINST_ERROR;
+        }
+        return ret;
+}
+
+/* Try running an operation on hardware */
+void s5p_mfc_try_run(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_ctx *ctx;
+        int new_ctx;
+        unsigned int ret = 0;
+
+        if (test_bit(0, &dev->enter_suspend)) {
+                mfc_debug(1, "Entering suspend so do not schedule any jobs\n");
+                return;
+        }
+        /* Check whether hardware is not running */
+        if (test_and_set_bit(0, &dev->hw_lock) != 0) {
+                /* This is perfectly ok, the scheduled ctx should wait */
+                mfc_debug(1, "Couldn't lock HW\n");
+                return;
+        }
+        /* Choose the context to run */
+        new_ctx = s5p_mfc_get_new_ctx(dev);
+        if (new_ctx < 0) {
+                /* No contexts to run */
+                if (test_and_clear_bit(0, &dev->hw_lock) == 0) {
+                        mfc_err("Failed to unlock hardware\n");
+                        return;
+                }
+                mfc_debug(1, "No ctx is scheduled to be run\n");
+                return;
+        }
+        ctx = dev->ctx[new_ctx];
+        /* Got context to run in ctx */
+        /*
+         * Last frame has already been sent to MFC.
+         * Now obtaining frames from MFC buffer
+         */
+        s5p_mfc_clock_on();
+        if (ctx->type == MFCINST_DECODER) {
+                s5p_mfc_set_dec_desc_buffer(ctx);
+                switch (ctx->state) {
+                case MFCINST_FINISHING:
+                        s5p_mfc_run_dec_frame(ctx, MFC_DEC_LAST_FRAME);
+                        break;
+                case MFCINST_RUNNING:
+                        ret = s5p_mfc_run_dec_frame(ctx, MFC_DEC_FRAME);
+                        break;
+                case MFCINST_INIT:
+                        s5p_mfc_clean_ctx_int_flags(ctx);
+                        ret = s5p_mfc_open_inst_cmd(ctx);
+                        break;
+                case MFCINST_RETURN_INST:
+                        s5p_mfc_clean_ctx_int_flags(ctx);
+                        ret = s5p_mfc_close_inst_cmd(ctx);
+                        break;
+                case MFCINST_GOT_INST:
+                        s5p_mfc_run_init_dec(ctx);
+                        break;
+                case MFCINST_HEAD_PARSED:
+                        ret = s5p_mfc_run_init_dec_buffers(ctx);
+                        mfc_debug(1, "head parsed\n");
+                        break;
+                case MFCINST_RES_CHANGE_INIT:
+                        s5p_mfc_run_res_change(ctx);
+                        break;
+                case MFCINST_RES_CHANGE_FLUSH:
+                        s5p_mfc_run_dec_frame(ctx, MFC_DEC_FRAME);
+                        break;
+                case MFCINST_RES_CHANGE_END:
+                        mfc_debug(2, "Finished remaining frames after resolution change\n");
+                        ctx->capture_state = QUEUE_FREE;
+                        mfc_debug(2, "Will re-init the codec\n");
+                        s5p_mfc_run_init_dec(ctx);
+                        break;
+                default:
+                        ret = -EAGAIN;
+                }
+        } else if (ctx->type == MFCINST_ENCODER) {
+                switch (ctx->state) {
+                case MFCINST_FINISHING:
+                case MFCINST_RUNNING:
+                        ret = s5p_mfc_run_enc_frame(ctx);
+                        break;
+                case MFCINST_INIT:
+                        s5p_mfc_clean_ctx_int_flags(ctx);
+                        ret = s5p_mfc_open_inst_cmd(ctx);
+                        break;
+                case MFCINST_RETURN_INST:
+                        s5p_mfc_clean_ctx_int_flags(ctx);
+                        ret = s5p_mfc_close_inst_cmd(ctx);
+                        break;
+                case MFCINST_GOT_INST:
+                        s5p_mfc_run_init_enc(ctx);
+                        break;
+                default:
+                        ret = -EAGAIN;
+                }
+        } else {
+                mfc_err("Invalid context type: %d\n", ctx->type);
+                ret = -EAGAIN;
+        }
+
+        if (ret) {
+                /* Free hardware lock */
+                if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+                        mfc_err("Failed to unlock hardware\n");
+
+                /* This is in deed imporant, as no operation has been
+                 * scheduled, reduce the clock count as no one will
+                 * ever do this, because no interrupt related to this try_run
+                 * will ever come from hardware. */
+                s5p_mfc_clock_off();
+        }
+}
+
+
+void s5p_mfc_cleanup_queue(struct list_head *lh, struct vb2_queue *vq)
+{
+        struct s5p_mfc_buf *b;
+        int i;
+
+        while (!list_empty(lh)) {
+                b = list_entry(lh->next, struct s5p_mfc_buf, list);
+                for (i = 0; i < b->b->num_planes; i++)
+                        vb2_set_plane_payload(b->b, i, 0);
+                vb2_buffer_done(b->b, VB2_BUF_STATE_ERROR);
+                list_del(&b->list);
+        }
+}
+
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_opr.h b/drivers/media/video/s5p-mfc/s5p_mfc_opr.h
new file mode 100644
index 00000000000..db83836e6a9
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_opr.h
@@ -0,0 +1,91 @@
+/*
+ * drivers/media/video/samsung/mfc5/s5p_mfc_opr.h
+ *
+ * Header file for Samsung MFC (Multi Function Codec - FIMV) driver
+ * Contains declarations of hw related functions.
+ *
+ * Kamil Debski, Copyright (C) 2011 Samsung Electronics
+ * http://www.samsung.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.
+ */
+
+#ifndef S5P_MFC_OPR_H_
+#define S5P_MFC_OPR_H_
+
+#include "s5p_mfc_common.h"
+
+int s5p_mfc_init_decode(struct s5p_mfc_ctx *ctx);
+int s5p_mfc_init_encode(struct s5p_mfc_ctx *mfc_ctx);
+
+/* Decoding functions */
+int s5p_mfc_set_dec_frame_buffer(struct s5p_mfc_ctx *ctx);
+int s5p_mfc_set_dec_stream_buffer(struct s5p_mfc_ctx *ctx, int buf_addr,
+                                                  unsigned int start_num_byte,
+                                                  unsigned int buf_size);
+
+/* Encoding functions */
+void s5p_mfc_set_enc_frame_buffer(struct s5p_mfc_ctx *ctx,
+                unsigned long y_addr, unsigned long c_addr);
+int s5p_mfc_set_enc_stream_buffer(struct s5p_mfc_ctx *ctx,
+                unsigned long addr, unsigned int size);
+void s5p_mfc_get_enc_frame_buffer(struct s5p_mfc_ctx *ctx,
+                unsigned long *y_addr, unsigned long *c_addr);
+int s5p_mfc_set_enc_ref_buffer(struct s5p_mfc_ctx *mfc_ctx);
+
+int s5p_mfc_decode_one_frame(struct s5p_mfc_ctx *ctx,
+                                        enum s5p_mfc_decode_arg last_frame);
+int s5p_mfc_encode_one_frame(struct s5p_mfc_ctx *mfc_ctx);
+
+/* Memory allocation */
+int s5p_mfc_alloc_dec_temp_buffers(struct s5p_mfc_ctx *ctx);
+void s5p_mfc_set_dec_desc_buffer(struct s5p_mfc_ctx *ctx);
+void s5p_mfc_release_dec_desc_buffer(struct s5p_mfc_ctx *ctx);
+
+int s5p_mfc_alloc_codec_buffers(struct s5p_mfc_ctx *ctx);
+void s5p_mfc_release_codec_buffers(struct s5p_mfc_ctx *ctx);
+
+int s5p_mfc_alloc_instance_buffer(struct s5p_mfc_ctx *ctx);
+void s5p_mfc_release_instance_buffer(struct s5p_mfc_ctx *ctx);
+
+void s5p_mfc_try_run(struct s5p_mfc_dev *dev);
+void s5p_mfc_cleanup_queue(struct list_head *lh, struct vb2_queue *vq);
+
+#define s5p_mfc_get_dspl_y_adr()        (readl(dev->regs_base + \
+                                        S5P_FIMV_SI_DISPLAY_Y_ADR) << \
+                                        MFC_OFFSET_SHIFT)
+#define s5p_mfc_get_dec_y_adr()         (readl(dev->regs_base + \
+                                        S5P_FIMV_SI_DISPLAY_Y_ADR) << \
+                                        MFC_OFFSET_SHIFT)
+#define s5p_mfc_get_dspl_status()       readl(dev->regs_base + \
+                                                S5P_FIMV_SI_DISPLAY_STATUS)
+#define s5p_mfc_get_frame_type()        (readl(dev->regs_base + \
+                                                S5P_FIMV_DECODE_FRAME_TYPE) \
+                                        & S5P_FIMV_DECODE_FRAME_MASK)
+#define s5p_mfc_get_consumed_stream()   readl(dev->regs_base + \
+                                                S5P_FIMV_SI_CONSUMED_BYTES)
+#define s5p_mfc_get_int_reason()        (readl(dev->regs_base + \
+                                        S5P_FIMV_RISC2HOST_CMD) & \
+                                        S5P_FIMV_RISC2HOST_CMD_MASK)
+#define s5p_mfc_get_int_err()           readl(dev->regs_base + \
+                                                S5P_FIMV_RISC2HOST_ARG2)
+#define s5p_mfc_err_dec(x)              (((x) & S5P_FIMV_ERR_DEC_MASK) >> \
+                                                        S5P_FIMV_ERR_DEC_SHIFT)
+#define s5p_mfc_err_dspl(x)             (((x) & S5P_FIMV_ERR_DSPL_MASK) >> \
+                                                        S5P_FIMV_ERR_DSPL_SHIFT)
+#define s5p_mfc_get_img_width()         readl(dev->regs_base + \
+                                                S5P_FIMV_SI_HRESOL)
+#define s5p_mfc_get_img_height()        readl(dev->regs_base + \
+                                                S5P_FIMV_SI_VRESOL)
+#define s5p_mfc_get_dpb_count()         readl(dev->regs_base + \
+                                                S5P_FIMV_SI_BUF_NUMBER)
+#define s5p_mfc_get_inst_no()           readl(dev->regs_base + \
+                                                S5P_FIMV_RISC2HOST_ARG1)
+#define s5p_mfc_get_enc_strm_size()     readl(dev->regs_base + \
+                                                S5P_FIMV_ENC_SI_STRM_SIZE)
+#define s5p_mfc_get_enc_slice_type()    readl(dev->regs_base + \
+                                                S5P_FIMV_ENC_SI_SLICE_TYPE)
+
+#endif /* S5P_MFC_OPR_H_ */
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_pm.c b/drivers/media/video/s5p-mfc/s5p_mfc_pm.c
new file mode 100644
index 00000000000..f6a3035c4fb
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_pm.c
@@ -0,0 +1,117 @@
+/*
+ * linux/drivers/media/video/s5p-mfc/s5p_mfc_pm.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.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/clk.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#ifdef CONFIG_PM_RUNTIME
+#include <linux/pm_runtime.h>
+#endif
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_pm.h"
+
+#define MFC_CLKNAME             "sclk_mfc"
+#define MFC_GATE_CLK_NAME       "mfc"
+
+#define CLK_DEBUG
+
+static struct s5p_mfc_pm *pm;
+static struct s5p_mfc_dev *p_dev;
+
+#ifdef CLK_DEBUG
+atomic_t clk_ref;
+#endif
+
+int s5p_mfc_init_pm(struct s5p_mfc_dev *dev)
+{
+        int ret = 0;
+
+        pm = &dev->pm;
+        p_dev = dev;
+        pm->clock_gate = clk_get(&dev->plat_dev->dev, MFC_GATE_CLK_NAME);
+        if (IS_ERR(pm->clock_gate)) {
+                mfc_err("Failed to get clock-gating control\n");
+                ret = -ENOENT;
+                goto err_g_ip_clk;
+        }
+        pm->clock = clk_get(&dev->plat_dev->dev, MFC_CLKNAME);
+        if (IS_ERR(pm->clock)) {
+                mfc_err("Failed to get MFC clock\n");
+                ret = -ENOENT;
+                goto err_g_ip_clk_2;
+        }
+        atomic_set(&pm->power, 0);
+#ifdef CONFIG_PM_RUNTIME
+        pm->device = &dev->plat_dev->dev;
+        pm_runtime_enable(pm->device);
+#endif
+#ifdef CLK_DEBUG
+        atomic_set(&clk_ref, 0);
+#endif
+        return 0;
+err_g_ip_clk_2:
+        clk_put(pm->clock_gate);
+err_g_ip_clk:
+        return ret;
+}
+
+void s5p_mfc_final_pm(struct s5p_mfc_dev *dev)
+{
+        clk_put(pm->clock_gate);
+        clk_put(pm->clock);
+#ifdef CONFIG_PM_RUNTIME
+        pm_runtime_disable(pm->device);
+#endif
+}
+
+int s5p_mfc_clock_on(void)
+{
+        int ret;
+#ifdef CLK_DEBUG
+        atomic_inc(&clk_ref);
+        mfc_debug(3, "+ %d", atomic_read(&clk_ref));
+#endif
+        ret = clk_enable(pm->clock_gate);
+        return ret;
+}
+
+void s5p_mfc_clock_off(void)
+{
+#ifdef CLK_DEBUG
+        atomic_dec(&clk_ref);
+        mfc_debug(3, "- %d", atomic_read(&clk_ref));
+#endif
+        clk_disable(pm->clock_gate);
+}
+
+int s5p_mfc_power_on(void)
+{
+#ifdef CONFIG_PM_RUNTIME
+        return pm_runtime_get_sync(pm->device);
+#else
+        atomic_set(&pm->power, 1);
+        return 0;
+#endif
+}
+
+int s5p_mfc_power_off(void)
+{
+#ifdef CONFIG_PM_RUNTIME
+        return pm_runtime_put_sync(pm->device);
+#else
+        atomic_set(&pm->power, 0);
+        return 0;
+#endif
+}
+
+
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_pm.h b/drivers/media/video/s5p-mfc/s5p_mfc_pm.h
new file mode 100644
index 00000000000..5107914f27e
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_pm.h
@@ -0,0 +1,24 @@
+/*
+ * linux/drivers/media/video/s5p-mfc/s5p_mfc_pm.h
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.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 S5P_MFC_PM_H_
+#define S5P_MFC_PM_H_
+
+int s5p_mfc_init_pm(struct s5p_mfc_dev *dev);
+void s5p_mfc_final_pm(struct s5p_mfc_dev *dev);
+
+int s5p_mfc_clock_on(void);
+void s5p_mfc_clock_off(void);
+int s5p_mfc_power_on(void);
+int s5p_mfc_power_off(void);
+
+#endif /* S5P_MFC_PM_H_ */
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_shm.c b/drivers/media/video/s5p-mfc/s5p_mfc_shm.c
new file mode 100644
index 00000000000..91fdbac8c37
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_shm.c
@@ -0,0 +1,47 @@
+/*
+ * linux/drivers/media/video/s5p-mfc/s5p_mfc_shm.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.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.
+ */
+
+#ifdef CONFIG_ARCH_EXYNOS4
+#include <linux/dma-mapping.h>
+#endif
+#include <linux/io.h>
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_debug.h"
+
+int s5p_mfc_init_shm(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        void *shm_alloc_ctx = dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+
+        ctx->shm_alloc = vb2_dma_contig_memops.alloc(shm_alloc_ctx,
+                                                        SHARED_BUF_SIZE);
+        if (IS_ERR(ctx->shm_alloc)) {
+                mfc_err("failed to allocate shared memory\n");
+                return PTR_ERR(ctx->shm_alloc);
+        }
+        /* shm_ofs only keeps the offset from base (port a) */
+        ctx->shm_ofs = s5p_mfc_mem_cookie(shm_alloc_ctx, ctx->shm_alloc)
+                                                                - dev->bank1;
+        BUG_ON(ctx->shm_ofs & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+        ctx->shm = vb2_dma_contig_memops.vaddr(ctx->shm_alloc);
+        if (!ctx->shm) {
+                vb2_dma_contig_memops.put(ctx->shm_alloc);
+                ctx->shm_ofs = 0;
+                ctx->shm_alloc = NULL;
+                mfc_err("failed to virt addr of shared memory\n");
+                return -ENOMEM;
+        }
+        memset((void *)ctx->shm, 0, SHARED_BUF_SIZE);
+        wmb();
+        return 0;
+}
+
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_shm.h b/drivers/media/video/s5p-mfc/s5p_mfc_shm.h
new file mode 100644
index 00000000000..764eac6bcc4
--- /dev/null
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_shm.h
@@ -0,0 +1,91 @@
+/*
+ * linux/drivers/media/video/s5p-mfc/s5p_mfc_shm.h
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.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 S5P_MFC_SHM_H_
+#define S5P_MFC_SHM_H_
+
+enum MFC_SHM_OFS
+{
+        EXTENEDED_DECODE_STATUS = 0x00, /* D */
+        SET_FRAME_TAG           = 0x04, /* D */
+        GET_FRAME_TAG_TOP       = 0x08, /* D */
+        GET_FRAME_TAG_BOT       = 0x0C, /* D */
+        PIC_TIME_TOP            = 0x10, /* D */
+        PIC_TIME_BOT            = 0x14, /* D */
+        START_BYTE_NUM          = 0x18, /* D */
+
+        CROP_INFO_H             = 0x20, /* D */
+        CROP_INFO_V             = 0x24, /* D */
+        EXT_ENC_CONTROL         = 0x28, /* E */
+        ENC_PARAM_CHANGE        = 0x2C, /* E */
+        RC_VOP_TIMING           = 0x30, /* E, MPEG4 */
+        HEC_PERIOD              = 0x34, /* E, MPEG4 */
+        METADATA_ENABLE         = 0x38, /* C */
+        METADATA_STATUS         = 0x3C, /* C */
+        METADATA_DISPLAY_INDEX  = 0x40, /* C */
+        EXT_METADATA_START_ADDR = 0x44, /* C */
+        PUT_EXTRADATA           = 0x48, /* C */
+        EXTRADATA_ADDR          = 0x4C, /* C */
+
+        ALLOC_LUMA_DPB_SIZE     = 0x64, /* D */
+        ALLOC_CHROMA_DPB_SIZE   = 0x68, /* D */
+        ALLOC_MV_SIZE           = 0x6C, /* D */
+        P_B_FRAME_QP            = 0x70, /* E */
+        SAMPLE_ASPECT_RATIO_IDC = 0x74, /* E, H.264, depend on
+                                ASPECT_RATIO_VUI_ENABLE in EXT_ENC_CONTROL */
+        EXTENDED_SAR            = 0x78, /* E, H.264, depned on
+                                ASPECT_RATIO_VUI_ENABLE in EXT_ENC_CONTROL */
+        DISP_PIC_PROFILE        = 0x7C, /* D */
+        FLUSH_CMD_TYPE          = 0x80, /* C */
+        FLUSH_CMD_INBUF1        = 0x84, /* C */
+        FLUSH_CMD_INBUF2        = 0x88, /* C */
+        FLUSH_CMD_OUTBUF        = 0x8C, /* E */
+        NEW_RC_BIT_RATE         = 0x90, /* E, format as RC_BIT_RATE(0xC5A8)
+                        depend on RC_BIT_RATE_CHANGE in ENC_PARAM_CHANGE */
+        NEW_RC_FRAME_RATE       = 0x94, /* E, format as RC_FRAME_RATE(0xD0D0)
+                        depend on RC_FRAME_RATE_CHANGE in ENC_PARAM_CHANGE */
+        NEW_I_PERIOD            = 0x98, /* E, format as I_FRM_CTRL(0xC504)
+                        depend on I_PERIOD_CHANGE in ENC_PARAM_CHANGE */
+        H264_I_PERIOD           = 0x9C, /* E, H.264, open GOP */
+        RC_CONTROL_CONFIG       = 0xA0, /* E */
+        BATCH_INPUT_ADDR        = 0xA4, /* E */
+        BATCH_OUTPUT_ADDR       = 0xA8, /* E */
+        BATCH_OUTPUT_SIZE       = 0xAC, /* E */
+        MIN_LUMA_DPB_SIZE       = 0xB0, /* D */
+        DEVICE_FORMAT_ID        = 0xB4, /* C */
+        H264_POC_TYPE           = 0xB8, /* D */
+        MIN_CHROMA_DPB_SIZE     = 0xBC, /* D */
+        DISP_PIC_FRAME_TYPE     = 0xC0, /* D */
+        FREE_LUMA_DPB           = 0xC4, /* D, VC1 MPEG4 */
+        ASPECT_RATIO_INFO       = 0xC8, /* D, MPEG4 */
+        EXTENDED_PAR            = 0xCC, /* D, MPEG4 */
+        DBG_HISTORY_INPUT0      = 0xD0, /* C */
+        DBG_HISTORY_INPUT1      = 0xD4, /* C */
+        DBG_HISTORY_OUTPUT      = 0xD8, /* C */
+        HIERARCHICAL_P_QP       = 0xE0, /* E, H.264 */
+};
+
+int s5p_mfc_init_shm(struct s5p_mfc_ctx *ctx);
+
+#define s5p_mfc_write_shm(ctx, x, ofs)          \
+        do {                                    \
+                writel(x, (ctx->shm + ofs));    \
+                wmb();                          \
+        } while (0)
+
+static inline u32 s5p_mfc_read_shm(struct s5p_mfc_ctx *ctx, unsigned int ofs)
+{
+        rmb();
+        return readl(ctx->shm + ofs);
+}
+
+#endif /* S5P_MFC_SHM_H_ */
diff --git a/drivers/media/video/s5p-tv/Kconfig b/drivers/media/video/s5p-tv/Kconfig
new file mode 100644
index 00000000000..9c37dee7bc5
--- /dev/null
+++ b/drivers/media/video/s5p-tv/Kconfig
@@ -0,0 +1,76 @@
+# drivers/media/video/s5p-tv/Kconfig
+#
+# Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+#       http://www.samsung.com/
+# Tomasz Stanislawski <t.stanislaws@samsung.com>
+#
+# Licensed under GPL
+
+config VIDEO_SAMSUNG_S5P_TV
+        bool "Samsung TV driver for S5P platform (experimental)"
+        depends on PLAT_S5P
+        depends on EXPERIMENTAL
+        default n
+        ---help---
+          Say Y here to enable selecting the TV output devices for
+          Samsung S5P platform.
+
+if VIDEO_SAMSUNG_S5P_TV
+
+config VIDEO_SAMSUNG_S5P_HDMI
+        tristate "Samsung HDMI Driver"
+        depends on VIDEO_V4L2
+        depends on VIDEO_SAMSUNG_S5P_TV
+        select VIDEO_SAMSUNG_S5P_HDMIPHY
+        help
+          Say Y here if you want support for the HDMI output
+          interface in S5P Samsung SoC. The driver can be compiled
+          as module. It is an auxiliary driver, that exposes a V4L2
+          subdev for use by other drivers. This driver requires
+          hdmiphy driver to work correctly.
+
+config VIDEO_SAMSUNG_S5P_HDMI_DEBUG
+        bool "Enable debug for HDMI Driver"
+        depends on VIDEO_SAMSUNG_S5P_HDMI
+        default n
+        help
+          Enables debugging for HDMI driver.
+
+config VIDEO_SAMSUNG_S5P_HDMIPHY
+        tristate "Samsung HDMIPHY Driver"
+        depends on VIDEO_DEV && VIDEO_V4L2 && I2C
+        depends on VIDEO_SAMSUNG_S5P_TV
+        help
+          Say Y here if you want support for the physical HDMI
+          interface in S5P Samsung SoC. The driver can be compiled
+          as module. It is an I2C driver, that exposes a V4L2
+          subdev for use by other drivers.
+
+config VIDEO_SAMSUNG_S5P_SDO
+        tristate "Samsung Analog TV Driver"
+        depends on VIDEO_DEV && VIDEO_V4L2
+        depends on VIDEO_SAMSUNG_S5P_TV
+        help
+          Say Y here if you want support for the analog TV output
+          interface in S5P Samsung SoC. The driver can be compiled
+          as module. It is an auxiliary driver, that exposes a V4L2
+          subdev for use by other drivers. This driver requires
+          hdmiphy driver to work correctly.
+
+config VIDEO_SAMSUNG_S5P_MIXER
+        tristate "Samsung Mixer and Video Processor Driver"
+        depends on VIDEO_DEV && VIDEO_V4L2
+        depends on VIDEO_SAMSUNG_S5P_TV
+        select VIDEOBUF2_DMA_CONTIG
+        help
+          Say Y here if you want support for the Mixer in Samsung S5P SoCs.
+          This device produce image data to one of output interfaces.
+
+config VIDEO_SAMSUNG_S5P_MIXER_DEBUG
+        bool "Enable debug for Mixer Driver"
+        depends on VIDEO_SAMSUNG_S5P_MIXER
+        default n
+        help
+          Enables debugging for Mixer driver.
+
+endif # VIDEO_SAMSUNG_S5P_TV
diff --git a/drivers/media/video/s5p-tv/Makefile b/drivers/media/video/s5p-tv/Makefile
new file mode 100644
index 00000000000..37e4c17663b
--- /dev/null
+++ b/drivers/media/video/s5p-tv/Makefile
@@ -0,0 +1,17 @@
+# drivers/media/video/samsung/tvout/Makefile
+#
+# Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+#       http://www.samsung.com/
+# Tomasz Stanislawski <t.stanislaws@samsung.com>
+#
+# Licensed under GPL
+
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_HDMIPHY) += s5p-hdmiphy.o
+s5p-hdmiphy-y += hdmiphy_drv.o
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_HDMI) += s5p-hdmi.o
+s5p-hdmi-y += hdmi_drv.o
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_SDO) += s5p-sdo.o
+s5p-sdo-y += sdo_drv.o
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_MIXER) += s5p-mixer.o
+s5p-mixer-y += mixer_drv.o mixer_video.o mixer_reg.o mixer_grp_layer.o mixer_vp_layer.o
+
diff --git a/drivers/media/video/s5p-tv/hdmi_drv.c b/drivers/media/video/s5p-tv/hdmi_drv.c
new file mode 100644
index 00000000000..06d6663f459
--- /dev/null
+++ b/drivers/media/video/s5p-tv/hdmi_drv.c
@@ -0,0 +1,1042 @@
+/*
+ * Samsung HDMI interface driver
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *
+ * Tomasz Stanislawski, <t.stanislaws@samsung.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 Foundiation. either version 2 of the License,
+ * or (at your option) any later version
+ */
+
+#ifdef CONFIG_VIDEO_SAMSUNG_S5P_HDMI_DEBUG
+#define DEBUG
+#endif
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/i2c.h>
+#include <linux/platform_device.h>
+#include <media/v4l2-subdev.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/delay.h>
+#include <linux/bug.h>
+#include <linux/pm_runtime.h>
+#include <linux/clk.h>
+#include <linux/regulator/consumer.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-device.h>
+
+#include "regs-hdmi.h"
+
+MODULE_AUTHOR("Tomasz Stanislawski, <t.stanislaws@samsung.com>");
+MODULE_DESCRIPTION("Samsung HDMI");
+MODULE_LICENSE("GPL");
+
+/* default preset configured on probe */
+#define HDMI_DEFAULT_PRESET V4L2_DV_1080P60
+
+struct hdmi_resources {
+        struct clk *hdmi;
+        struct clk *sclk_hdmi;
+        struct clk *sclk_pixel;
+        struct clk *sclk_hdmiphy;
+        struct clk *hdmiphy;
+        struct regulator_bulk_data *regul_bulk;
+        int regul_count;
+};
+
+struct hdmi_device {
+        /** base address of HDMI registers */
+        void __iomem *regs;
+        /** HDMI interrupt */
+        unsigned int irq;
+        /** pointer to device parent */
+        struct device *dev;
+        /** subdev generated by HDMI device */
+        struct v4l2_subdev sd;
+        /** V4L2 device structure */
+        struct v4l2_device v4l2_dev;
+        /** subdev of HDMIPHY interface */
+        struct v4l2_subdev *phy_sd;
+        /** configuration of current graphic mode */
+        const struct hdmi_preset_conf *cur_conf;
+        /** current preset */
+        u32 cur_preset;
+        /** other resources */
+        struct hdmi_resources res;
+};
+
+struct hdmi_driver_data {
+        int hdmiphy_bus;
+};
+
+struct hdmi_tg_regs {
+        u8 cmd;
+        u8 h_fsz_l;
+        u8 h_fsz_h;
+        u8 hact_st_l;
+        u8 hact_st_h;
+        u8 hact_sz_l;
+        u8 hact_sz_h;
+        u8 v_fsz_l;
+        u8 v_fsz_h;
+        u8 vsync_l;
+        u8 vsync_h;
+        u8 vsync2_l;
+        u8 vsync2_h;
+        u8 vact_st_l;
+        u8 vact_st_h;
+        u8 vact_sz_l;
+        u8 vact_sz_h;
+        u8 field_chg_l;
+        u8 field_chg_h;
+        u8 vact_st2_l;
+        u8 vact_st2_h;
+        u8 vsync_top_hdmi_l;
+        u8 vsync_top_hdmi_h;
+        u8 vsync_bot_hdmi_l;
+        u8 vsync_bot_hdmi_h;
+        u8 field_top_hdmi_l;
+        u8 field_top_hdmi_h;
+        u8 field_bot_hdmi_l;
+        u8 field_bot_hdmi_h;
+};
+
+struct hdmi_core_regs {
+        u8 h_blank[2];
+        u8 v_blank[3];
+        u8 h_v_line[3];
+        u8 vsync_pol[1];
+        u8 int_pro_mode[1];
+        u8 v_blank_f[3];
+        u8 h_sync_gen[3];
+        u8 v_sync_gen1[3];
+        u8 v_sync_gen2[3];
+        u8 v_sync_gen3[3];
+};
+
+struct hdmi_preset_conf {
+        struct hdmi_core_regs core;
+        struct hdmi_tg_regs tg;
+        struct v4l2_mbus_framefmt mbus_fmt;
+};
+
+/* I2C module and id for HDMIPHY */
+static struct i2c_board_info hdmiphy_info = {
+        I2C_BOARD_INFO("hdmiphy", 0x38),
+};
+
+static struct hdmi_driver_data hdmi_driver_data[] = {
+        { .hdmiphy_bus = 3 },
+        { .hdmiphy_bus = 8 },
+};
+
+static struct platform_device_id hdmi_driver_types[] = {
+        {
+                .name           = "s5pv210-hdmi",
+                .driver_data    = (unsigned long)&hdmi_driver_data[0],
+        }, {
+                .name           = "exynos4-hdmi",
+                .driver_data    = (unsigned long)&hdmi_driver_data[1],
+        }, {
+                /* end node */
+        }
+};
+
+static const struct v4l2_subdev_ops hdmi_sd_ops;
+
+static struct hdmi_device *sd_to_hdmi_dev(struct v4l2_subdev *sd)
+{
+        return container_of(sd, struct hdmi_device, sd);
+}
+
+static inline
+void hdmi_write(struct hdmi_device *hdev, u32 reg_id, u32 value)
+{
+        writel(value, hdev->regs + reg_id);
+}
+
+static inline
+void hdmi_write_mask(struct hdmi_device *hdev, u32 reg_id, u32 value, u32 mask)
+{
+        u32 old = readl(hdev->regs + reg_id);
+        value = (value & mask) | (old & ~mask);
+        writel(value, hdev->regs + reg_id);
+}
+
+static inline
+void hdmi_writeb(struct hdmi_device *hdev, u32 reg_id, u8 value)
+{
+        writeb(value, hdev->regs + reg_id);
+}
+
+static inline u32 hdmi_read(struct hdmi_device *hdev, u32 reg_id)
+{
+        return readl(hdev->regs + reg_id);
+}
+
+static irqreturn_t hdmi_irq_handler(int irq, void *dev_data)
+{
+        struct hdmi_device *hdev = dev_data;
+        u32 intc_flag;
+
+        (void)irq;
+        intc_flag = hdmi_read(hdev, HDMI_INTC_FLAG);
+        /* clearing flags for HPD plug/unplug */
+        if (intc_flag & HDMI_INTC_FLAG_HPD_UNPLUG) {
+                printk(KERN_INFO "unplugged\n");
+                hdmi_write_mask(hdev, HDMI_INTC_FLAG, ~0,
+                        HDMI_INTC_FLAG_HPD_UNPLUG);
+        }
+        if (intc_flag & HDMI_INTC_FLAG_HPD_PLUG) {
+                printk(KERN_INFO "plugged\n");
+                hdmi_write_mask(hdev, HDMI_INTC_FLAG, ~0,
+                        HDMI_INTC_FLAG_HPD_PLUG);
+        }
+
+        return IRQ_HANDLED;
+}
+
+static void hdmi_reg_init(struct hdmi_device *hdev)
+{
+        /* enable HPD interrupts */
+        hdmi_write_mask(hdev, HDMI_INTC_CON, ~0, HDMI_INTC_EN_GLOBAL |
+                HDMI_INTC_EN_HPD_PLUG | HDMI_INTC_EN_HPD_UNPLUG);
+        /* choose HDMI mode */
+        hdmi_write_mask(hdev, HDMI_MODE_SEL,
+                HDMI_MODE_HDMI_EN, HDMI_MODE_MASK);
+        /* disable bluescreen */
+        hdmi_write_mask(hdev, HDMI_CON_0, 0, HDMI_BLUE_SCR_EN);
+        /* choose bluescreen (fecal) color */
+        hdmi_writeb(hdev, HDMI_BLUE_SCREEN_0, 0x12);
+        hdmi_writeb(hdev, HDMI_BLUE_SCREEN_1, 0x34);
+        hdmi_writeb(hdev, HDMI_BLUE_SCREEN_2, 0x56);
+        /* enable AVI packet every vsync, fixes purple line problem */
+        hdmi_writeb(hdev, HDMI_AVI_CON, 0x02);
+        /* force YUV444, look to CEA-861-D, table 7 for more detail */
+        hdmi_writeb(hdev, HDMI_AVI_BYTE(0), 2 << 5);
+        hdmi_write_mask(hdev, HDMI_CON_1, 2, 3 << 5);
+}
+
+static void hdmi_timing_apply(struct hdmi_device *hdev,
+        const struct hdmi_preset_conf *conf)
+{
+        const struct hdmi_core_regs *core = &conf->core;
+        const struct hdmi_tg_regs *tg = &conf->tg;
+
+        /* setting core registers */
+        hdmi_writeb(hdev, HDMI_H_BLANK_0, core->h_blank[0]);
+        hdmi_writeb(hdev, HDMI_H_BLANK_1, core->h_blank[1]);
+        hdmi_writeb(hdev, HDMI_V_BLANK_0, core->v_blank[0]);
+        hdmi_writeb(hdev, HDMI_V_BLANK_1, core->v_blank[1]);
+        hdmi_writeb(hdev, HDMI_V_BLANK_2, core->v_blank[2]);
+        hdmi_writeb(hdev, HDMI_H_V_LINE_0, core->h_v_line[0]);
+        hdmi_writeb(hdev, HDMI_H_V_LINE_1, core->h_v_line[1]);
+        hdmi_writeb(hdev, HDMI_H_V_LINE_2, core->h_v_line[2]);
+        hdmi_writeb(hdev, HDMI_VSYNC_POL, core->vsync_pol[0]);
+        hdmi_writeb(hdev, HDMI_INT_PRO_MODE, core->int_pro_mode[0]);
+        hdmi_writeb(hdev, HDMI_V_BLANK_F_0, core->v_blank_f[0]);
+        hdmi_writeb(hdev, HDMI_V_BLANK_F_1, core->v_blank_f[1]);
+        hdmi_writeb(hdev, HDMI_V_BLANK_F_2, core->v_blank_f[2]);
+        hdmi_writeb(hdev, HDMI_H_SYNC_GEN_0, core->h_sync_gen[0]);
+        hdmi_writeb(hdev, HDMI_H_SYNC_GEN_1, core->h_sync_gen[1]);
+        hdmi_writeb(hdev, HDMI_H_SYNC_GEN_2, core->h_sync_gen[2]);
+        hdmi_writeb(hdev, HDMI_V_SYNC_GEN_1_0, core->v_sync_gen1[0]);
+        hdmi_writeb(hdev, HDMI_V_SYNC_GEN_1_1, core->v_sync_gen1[1]);
+        hdmi_writeb(hdev, HDMI_V_SYNC_GEN_1_2, core->v_sync_gen1[2]);
+        hdmi_writeb(hdev, HDMI_V_SYNC_GEN_2_0, core->v_sync_gen2[0]);
+        hdmi_writeb(hdev, HDMI_V_SYNC_GEN_2_1, core->v_sync_gen2[1]);
+        hdmi_writeb(hdev, HDMI_V_SYNC_GEN_2_2, core->v_sync_gen2[2]);
+        hdmi_writeb(hdev, HDMI_V_SYNC_GEN_3_0, core->v_sync_gen3[0]);
+        hdmi_writeb(hdev, HDMI_V_SYNC_GEN_3_1, core->v_sync_gen3[1]);
+        hdmi_writeb(hdev, HDMI_V_SYNC_GEN_3_2, core->v_sync_gen3[2]);
+        /* Timing generator registers */
+        hdmi_writeb(hdev, HDMI_TG_H_FSZ_L, tg->h_fsz_l);
+        hdmi_writeb(hdev, HDMI_TG_H_FSZ_H, tg->h_fsz_h);
+        hdmi_writeb(hdev, HDMI_TG_HACT_ST_L, tg->hact_st_l);
+        hdmi_writeb(hdev, HDMI_TG_HACT_ST_H, tg->hact_st_h);
+        hdmi_writeb(hdev, HDMI_TG_HACT_SZ_L, tg->hact_sz_l);
+        hdmi_writeb(hdev, HDMI_TG_HACT_SZ_H, tg->hact_sz_h);
+        hdmi_writeb(hdev, HDMI_TG_V_FSZ_L, tg->v_fsz_l);
+        hdmi_writeb(hdev, HDMI_TG_V_FSZ_H, tg->v_fsz_h);
+        hdmi_writeb(hdev, HDMI_TG_VSYNC_L, tg->vsync_l);
+        hdmi_writeb(hdev, HDMI_TG_VSYNC_H, tg->vsync_h);
+        hdmi_writeb(hdev, HDMI_TG_VSYNC2_L, tg->vsync2_l);
+        hdmi_writeb(hdev, HDMI_TG_VSYNC2_H, tg->vsync2_h);
+        hdmi_writeb(hdev, HDMI_TG_VACT_ST_L, tg->vact_st_l);
+        hdmi_writeb(hdev, HDMI_TG_VACT_ST_H, tg->vact_st_h);
+        hdmi_writeb(hdev, HDMI_TG_VACT_SZ_L, tg->vact_sz_l);
+        hdmi_writeb(hdev, HDMI_TG_VACT_SZ_H, tg->vact_sz_h);
+        hdmi_writeb(hdev, HDMI_TG_FIELD_CHG_L, tg->field_chg_l);
+        hdmi_writeb(hdev, HDMI_TG_FIELD_CHG_H, tg->field_chg_h);
+        hdmi_writeb(hdev, HDMI_TG_VACT_ST2_L, tg->vact_st2_l);
+        hdmi_writeb(hdev, HDMI_TG_VACT_ST2_H, tg->vact_st2_h);
+        hdmi_writeb(hdev, HDMI_TG_VSYNC_TOP_HDMI_L, tg->vsync_top_hdmi_l);
+        hdmi_writeb(hdev, HDMI_TG_VSYNC_TOP_HDMI_H, tg->vsync_top_hdmi_h);
+        hdmi_writeb(hdev, HDMI_TG_VSYNC_BOT_HDMI_L, tg->vsync_bot_hdmi_l);
+        hdmi_writeb(hdev, HDMI_TG_VSYNC_BOT_HDMI_H, tg->vsync_bot_hdmi_h);
+        hdmi_writeb(hdev, HDMI_TG_FIELD_TOP_HDMI_L, tg->field_top_hdmi_l);
+        hdmi_writeb(hdev, HDMI_TG_FIELD_TOP_HDMI_H, tg->field_top_hdmi_h);
+        hdmi_writeb(hdev, HDMI_TG_FIELD_BOT_HDMI_L, tg->field_bot_hdmi_l);
+        hdmi_writeb(hdev, HDMI_TG_FIELD_BOT_HDMI_H, tg->field_bot_hdmi_h);
+}
+
+static int hdmi_conf_apply(struct hdmi_device *hdmi_dev)
+{
+        struct device *dev = hdmi_dev->dev;
+        const struct hdmi_preset_conf *conf = hdmi_dev->cur_conf;
+        struct v4l2_dv_preset preset;
+        int ret;
+
+        dev_dbg(dev, "%s\n", __func__);
+
+        /* reset hdmiphy */
+        hdmi_write_mask(hdmi_dev, HDMI_PHY_RSTOUT, ~0, HDMI_PHY_SW_RSTOUT);
+        mdelay(10);
+        hdmi_write_mask(hdmi_dev, HDMI_PHY_RSTOUT,  0, HDMI_PHY_SW_RSTOUT);
+        mdelay(10);
+
+        /* configure presets */
+        preset.preset = hdmi_dev->cur_preset;
+        ret = v4l2_subdev_call(hdmi_dev->phy_sd, video, s_dv_preset, &preset);
+        if (ret) {
+                dev_err(dev, "failed to set preset (%u)\n", preset.preset);
+                return ret;
+        }
+
+        /* resetting HDMI core */
+        hdmi_write_mask(hdmi_dev, HDMI_CORE_RSTOUT,  0, HDMI_CORE_SW_RSTOUT);
+        mdelay(10);
+        hdmi_write_mask(hdmi_dev, HDMI_CORE_RSTOUT, ~0, HDMI_CORE_SW_RSTOUT);
+        mdelay(10);
+
+        hdmi_reg_init(hdmi_dev);
+
+        /* setting core registers */
+        hdmi_timing_apply(hdmi_dev, conf);
+
+        return 0;
+}
+
+static void hdmi_dumpregs(struct hdmi_device *hdev, char *prefix)
+{
+#define DUMPREG(reg_id) \
+        dev_dbg(hdev->dev, "%s:" #reg_id " = %08x\n", prefix, \
+                readl(hdev->regs + reg_id))
+
+        dev_dbg(hdev->dev, "%s: ---- CONTROL REGISTERS ----\n", prefix);
+        DUMPREG(HDMI_INTC_FLAG);
+        DUMPREG(HDMI_INTC_CON);
+        DUMPREG(HDMI_HPD_STATUS);
+        DUMPREG(HDMI_PHY_RSTOUT);
+        DUMPREG(HDMI_PHY_VPLL);
+        DUMPREG(HDMI_PHY_CMU);
+        DUMPREG(HDMI_CORE_RSTOUT);
+
+        dev_dbg(hdev->dev, "%s: ---- CORE REGISTERS ----\n", prefix);
+        DUMPREG(HDMI_CON_0);
+        DUMPREG(HDMI_CON_1);
+        DUMPREG(HDMI_CON_2);
+        DUMPREG(HDMI_SYS_STATUS);
+        DUMPREG(HDMI_PHY_STATUS);
+        DUMPREG(HDMI_STATUS_EN);
+        DUMPREG(HDMI_HPD);
+        DUMPREG(HDMI_MODE_SEL);
+        DUMPREG(HDMI_HPD_GEN);
+        DUMPREG(HDMI_DC_CONTROL);
+        DUMPREG(HDMI_VIDEO_PATTERN_GEN);
+
+        dev_dbg(hdev->dev, "%s: ---- CORE SYNC REGISTERS ----\n", prefix);
+        DUMPREG(HDMI_H_BLANK_0);
+        DUMPREG(HDMI_H_BLANK_1);
+        DUMPREG(HDMI_V_BLANK_0);
+        DUMPREG(HDMI_V_BLANK_1);
+        DUMPREG(HDMI_V_BLANK_2);
+        DUMPREG(HDMI_H_V_LINE_0);
+        DUMPREG(HDMI_H_V_LINE_1);
+        DUMPREG(HDMI_H_V_LINE_2);
+        DUMPREG(HDMI_VSYNC_POL);
+        DUMPREG(HDMI_INT_PRO_MODE);
+        DUMPREG(HDMI_V_BLANK_F_0);
+        DUMPREG(HDMI_V_BLANK_F_1);
+        DUMPREG(HDMI_V_BLANK_F_2);
+        DUMPREG(HDMI_H_SYNC_GEN_0);
+        DUMPREG(HDMI_H_SYNC_GEN_1);
+        DUMPREG(HDMI_H_SYNC_GEN_2);
+        DUMPREG(HDMI_V_SYNC_GEN_1_0);
+        DUMPREG(HDMI_V_SYNC_GEN_1_1);
+        DUMPREG(HDMI_V_SYNC_GEN_1_2);
+        DUMPREG(HDMI_V_SYNC_GEN_2_0);
+        DUMPREG(HDMI_V_SYNC_GEN_2_1);
+        DUMPREG(HDMI_V_SYNC_GEN_2_2);
+        DUMPREG(HDMI_V_SYNC_GEN_3_0);
+        DUMPREG(HDMI_V_SYNC_GEN_3_1);
+        DUMPREG(HDMI_V_SYNC_GEN_3_2);
+
+        dev_dbg(hdev->dev, "%s: ---- TG REGISTERS ----\n", prefix);
+        DUMPREG(HDMI_TG_CMD);
+        DUMPREG(HDMI_TG_H_FSZ_L);
+        DUMPREG(HDMI_TG_H_FSZ_H);
+        DUMPREG(HDMI_TG_HACT_ST_L);
+        DUMPREG(HDMI_TG_HACT_ST_H);
+        DUMPREG(HDMI_TG_HACT_SZ_L);
+        DUMPREG(HDMI_TG_HACT_SZ_H);
+        DUMPREG(HDMI_TG_V_FSZ_L);
+        DUMPREG(HDMI_TG_V_FSZ_H);
+        DUMPREG(HDMI_TG_VSYNC_L);
+        DUMPREG(HDMI_TG_VSYNC_H);
+        DUMPREG(HDMI_TG_VSYNC2_L);
+        DUMPREG(HDMI_TG_VSYNC2_H);
+        DUMPREG(HDMI_TG_VACT_ST_L);
+        DUMPREG(HDMI_TG_VACT_ST_H);
+        DUMPREG(HDMI_TG_VACT_SZ_L);
+        DUMPREG(HDMI_TG_VACT_SZ_H);
+        DUMPREG(HDMI_TG_FIELD_CHG_L);
+        DUMPREG(HDMI_TG_FIELD_CHG_H);
+        DUMPREG(HDMI_TG_VACT_ST2_L);
+        DUMPREG(HDMI_TG_VACT_ST2_H);
+        DUMPREG(HDMI_TG_VSYNC_TOP_HDMI_L);
+        DUMPREG(HDMI_TG_VSYNC_TOP_HDMI_H);
+        DUMPREG(HDMI_TG_VSYNC_BOT_HDMI_L);
+        DUMPREG(HDMI_TG_VSYNC_BOT_HDMI_H);
+        DUMPREG(HDMI_TG_FIELD_TOP_HDMI_L);
+        DUMPREG(HDMI_TG_FIELD_TOP_HDMI_H);
+        DUMPREG(HDMI_TG_FIELD_BOT_HDMI_L);
+        DUMPREG(HDMI_TG_FIELD_BOT_HDMI_H);
+#undef DUMPREG
+}
+
+static const struct hdmi_preset_conf hdmi_conf_480p = {
+        .core = {
+                .h_blank = {0x8a, 0x00},
+                .v_blank = {0x0d, 0x6a, 0x01},
+                .h_v_line = {0x0d, 0xa2, 0x35},
+                .vsync_pol = {0x01},
+                .int_pro_mode = {0x00},
+                .v_blank_f = {0x00, 0x00, 0x00},
+                .h_sync_gen = {0x0e, 0x30, 0x11},
+                .v_sync_gen1 = {0x0f, 0x90, 0x00},
+                /* other don't care */
+        },
+        .tg = {
+                0x00, /* cmd */
+                0x5a, 0x03, /* h_fsz */
+                0x8a, 0x00, 0xd0, 0x02, /* hact */
+                0x0d, 0x02, /* v_fsz */
+                0x01, 0x00, 0x33, 0x02, /* vsync */
+                0x2d, 0x00, 0xe0, 0x01, /* vact */
+                0x33, 0x02, /* field_chg */
+                0x49, 0x02, /* vact_st2 */
+                0x01, 0x00, 0x33, 0x02, /* vsync top/bot */
+                0x01, 0x00, 0x33, 0x02, /* field top/bot */
+        },
+        .mbus_fmt = {
+                .width = 720,
+                .height = 480,
+                .code = V4L2_MBUS_FMT_FIXED, /* means RGB888 */
+                .field = V4L2_FIELD_NONE,
+        },
+};
+
+static const struct hdmi_preset_conf hdmi_conf_720p60 = {
+        .core = {
+                .h_blank = {0x72, 0x01},
+                .v_blank = {0xee, 0xf2, 0x00},
+                .h_v_line = {0xee, 0x22, 0x67},
+                .vsync_pol = {0x00},
+                .int_pro_mode = {0x00},
+                .v_blank_f = {0x00, 0x00, 0x00}, /* don't care */
+                .h_sync_gen = {0x6c, 0x50, 0x02},
+                .v_sync_gen1 = {0x0a, 0x50, 0x00},
+                /* other don't care */
+        },
+        .tg = {
+                0x00, /* cmd */
+                0x72, 0x06, /* h_fsz */
+                0x72, 0x01, 0x00, 0x05, /* hact */
+                0xee, 0x02, /* v_fsz */
+                0x01, 0x00, 0x33, 0x02, /* vsync */
+                0x1e, 0x00, 0xd0, 0x02, /* vact */
+                0x33, 0x02, /* field_chg */
+                0x49, 0x02, /* vact_st2 */
+                0x01, 0x00, 0x33, 0x02, /* vsync top/bot */
+                0x01, 0x00, 0x33, 0x02, /* field top/bot */
+        },
+        .mbus_fmt = {
+                .width = 1280,
+                .height = 720,
+                .code = V4L2_MBUS_FMT_FIXED, /* means RGB888 */
+                .field = V4L2_FIELD_NONE,
+        },
+};
+
+static const struct hdmi_preset_conf hdmi_conf_1080p50 = {
+        .core = {
+                .h_blank = {0xd0, 0x02},
+                .v_blank = {0x65, 0x6c, 0x01},
+                .h_v_line = {0x65, 0x04, 0xa5},
+                .vsync_pol = {0x00},
+                .int_pro_mode = {0x00},
+                .v_blank_f = {0x00, 0x00, 0x00}, /* don't care */
+                .h_sync_gen = {0x0e, 0xea, 0x08},
+                .v_sync_gen1 = {0x09, 0x40, 0x00},
+                /* other don't care */
+        },
+        .tg = {
+                0x00, /* cmd */
+                0x98, 0x08, /* h_fsz */
+                0x18, 0x01, 0x80, 0x07, /* hact */
+                0x65, 0x04, /* v_fsz */
+                0x01, 0x00, 0x33, 0x02, /* vsync */
+                0x2d, 0x00, 0x38, 0x04, /* vact */
+                0x33, 0x02, /* field_chg */
+                0x49, 0x02, /* vact_st2 */
+                0x01, 0x00, 0x33, 0x02, /* vsync top/bot */
+                0x01, 0x00, 0x33, 0x02, /* field top/bot */
+        },
+        .mbus_fmt = {
+                .width = 1920,
+                .height = 1080,
+                .code = V4L2_MBUS_FMT_FIXED, /* means RGB888 */
+                .field = V4L2_FIELD_NONE,
+        },
+};
+
+static const struct hdmi_preset_conf hdmi_conf_1080p60 = {
+        .core = {
+                .h_blank = {0x18, 0x01},
+                .v_blank = {0x65, 0x6c, 0x01},
+                .h_v_line = {0x65, 0x84, 0x89},
+                .vsync_pol = {0x00},
+                .int_pro_mode = {0x00},
+                .v_blank_f = {0x00, 0x00, 0x00}, /* don't care */
+                .h_sync_gen = {0x56, 0x08, 0x02},
+                .v_sync_gen1 = {0x09, 0x40, 0x00},
+                /* other don't care */
+        },
+        .tg = {
+                0x00, /* cmd */
+                0x98, 0x08, /* h_fsz */
+                0x18, 0x01, 0x80, 0x07, /* hact */
+                0x65, 0x04, /* v_fsz */
+                0x01, 0x00, 0x33, 0x02, /* vsync */
+                0x2d, 0x00, 0x38, 0x04, /* vact */
+                0x33, 0x02, /* field_chg */
+                0x48, 0x02, /* vact_st2 */
+                0x01, 0x00, 0x01, 0x00, /* vsync top/bot */
+                0x01, 0x00, 0x33, 0x02, /* field top/bot */
+        },
+        .mbus_fmt = {
+                .width = 1920,
+                .height = 1080,
+                .code = V4L2_MBUS_FMT_FIXED, /* means RGB888 */
+                .field = V4L2_FIELD_NONE,
+        },
+};
+
+static const struct {
+        u32 preset;
+        const struct hdmi_preset_conf *conf;
+} hdmi_conf[] = {
+        { V4L2_DV_480P59_94, &hdmi_conf_480p },
+        { V4L2_DV_720P59_94, &hdmi_conf_720p60 },
+        { V4L2_DV_1080P50, &hdmi_conf_1080p50 },
+        { V4L2_DV_1080P30, &hdmi_conf_1080p60 },
+        { V4L2_DV_1080P60, &hdmi_conf_1080p60 },
+};
+
+static const struct hdmi_preset_conf *hdmi_preset2conf(u32 preset)
+{
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(hdmi_conf); ++i)
+                if (hdmi_conf[i].preset == preset)
+                        return  hdmi_conf[i].conf;
+        return NULL;
+}
+
+static int hdmi_streamon(struct hdmi_device *hdev)
+{
+        struct device *dev = hdev->dev;
+        struct hdmi_resources *res = &hdev->res;
+        int ret, tries;
+
+        dev_dbg(dev, "%s\n", __func__);
+
+        ret = v4l2_subdev_call(hdev->phy_sd, video, s_stream, 1);
+        if (ret)
+                return ret;
+
+        /* waiting for HDMIPHY's PLL to get to steady state */
+        for (tries = 100; tries; --tries) {
+                u32 val = hdmi_read(hdev, HDMI_PHY_STATUS);
+                if (val & HDMI_PHY_STATUS_READY)
+                        break;
+                mdelay(1);
+        }
+        /* steady state not achieved */
+        if (tries == 0) {
+                dev_err(dev, "hdmiphy's pll could not reach steady state.\n");
+                v4l2_subdev_call(hdev->phy_sd, video, s_stream, 0);
+                hdmi_dumpregs(hdev, "s_stream");
+                return -EIO;
+        }
+
+        /* hdmiphy clock is used for HDMI in streaming mode */
+        clk_disable(res->sclk_hdmi);
+        clk_set_parent(res->sclk_hdmi, res->sclk_hdmiphy);
+        clk_enable(res->sclk_hdmi);
+
+        /* enable HDMI and timing generator */
+        hdmi_write_mask(hdev, HDMI_CON_0, ~0, HDMI_EN);
+        hdmi_write_mask(hdev, HDMI_TG_CMD, ~0, HDMI_TG_EN);
+        hdmi_dumpregs(hdev, "streamon");
+        return 0;
+}
+
+static int hdmi_streamoff(struct hdmi_device *hdev)
+{
+        struct device *dev = hdev->dev;
+        struct hdmi_resources *res = &hdev->res;
+
+        dev_dbg(dev, "%s\n", __func__);
+
+        hdmi_write_mask(hdev, HDMI_CON_0, 0, HDMI_EN);
+        hdmi_write_mask(hdev, HDMI_TG_CMD, 0, HDMI_TG_EN);
+
+        /* pixel(vpll) clock is used for HDMI in config mode */
+        clk_disable(res->sclk_hdmi);
+        clk_set_parent(res->sclk_hdmi, res->sclk_pixel);
+        clk_enable(res->sclk_hdmi);
+
+        v4l2_subdev_call(hdev->phy_sd, video, s_stream, 0);
+
+        hdmi_dumpregs(hdev, "streamoff");
+        return 0;
+}
+
+static int hdmi_s_stream(struct v4l2_subdev *sd, int enable)
+{
+        struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
+        struct device *dev = hdev->dev;
+
+        dev_dbg(dev, "%s(%d)\n", __func__, enable);
+        if (enable)
+                return hdmi_streamon(hdev);
+        return hdmi_streamoff(hdev);
+}
+
+static void hdmi_resource_poweron(struct hdmi_resources *res)
+{
+        /* turn HDMI power on */
+        regulator_bulk_enable(res->regul_count, res->regul_bulk);
+        /* power-on hdmi physical interface */
+        clk_enable(res->hdmiphy);
+        /* use VPP as parent clock; HDMIPHY is not working yet */
+        clk_set_parent(res->sclk_hdmi, res->sclk_pixel);
+        /* turn clocks on */
+        clk_enable(res->sclk_hdmi);
+}
+
+static void hdmi_resource_poweroff(struct hdmi_resources *res)
+{
+        /* turn clocks off */
+        clk_disable(res->sclk_hdmi);
+        /* power-off hdmiphy */
+        clk_disable(res->hdmiphy);
+        /* turn HDMI power off */
+        regulator_bulk_disable(res->regul_count, res->regul_bulk);
+}
+
+static int hdmi_s_power(struct v4l2_subdev *sd, int on)
+{
+        struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
+        int ret;
+
+        if (on)
+                ret = pm_runtime_get_sync(hdev->dev);
+        else
+                ret = pm_runtime_put_sync(hdev->dev);
+        /* only values < 0 indicate errors */
+        return IS_ERR_VALUE(ret) ? ret : 0;
+}
+
+static int hdmi_s_dv_preset(struct v4l2_subdev *sd,
+        struct v4l2_dv_preset *preset)
+{
+        struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
+        struct device *dev = hdev->dev;
+        const struct hdmi_preset_conf *conf;
+
+        conf = hdmi_preset2conf(preset->preset);
+        if (conf == NULL) {
+                dev_err(dev, "preset (%u) not supported\n", preset->preset);
+                return -EINVAL;
+        }
+        hdev->cur_conf = conf;
+        hdev->cur_preset = preset->preset;
+        return 0;
+}
+
+static int hdmi_g_dv_preset(struct v4l2_subdev *sd,
+        struct v4l2_dv_preset *preset)
+{
+        memset(preset, 0, sizeof(*preset));
+        preset->preset = sd_to_hdmi_dev(sd)->cur_preset;
+        return 0;
+}
+
+static int hdmi_g_mbus_fmt(struct v4l2_subdev *sd,
+          struct v4l2_mbus_framefmt *fmt)
+{
+        struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
+        struct device *dev = hdev->dev;
+
+        dev_dbg(dev, "%s\n", __func__);
+        if (!hdev->cur_conf)
+                return -EINVAL;
+        *fmt = hdev->cur_conf->mbus_fmt;
+        return 0;
+}
+
+static int hdmi_enum_dv_presets(struct v4l2_subdev *sd,
+        struct v4l2_dv_enum_preset *preset)
+{
+        if (preset->index >= ARRAY_SIZE(hdmi_conf))
+                return -EINVAL;
+        return v4l_fill_dv_preset_info(hdmi_conf[preset->index].preset, preset);
+}
+
+static const struct v4l2_subdev_core_ops hdmi_sd_core_ops = {
+        .s_power = hdmi_s_power,
+};
+
+static const struct v4l2_subdev_video_ops hdmi_sd_video_ops = {
+        .s_dv_preset = hdmi_s_dv_preset,
+        .g_dv_preset = hdmi_g_dv_preset,
+        .enum_dv_presets = hdmi_enum_dv_presets,
+        .g_mbus_fmt = hdmi_g_mbus_fmt,
+        .s_stream = hdmi_s_stream,
+};
+
+static const struct v4l2_subdev_ops hdmi_sd_ops = {
+        .core = &hdmi_sd_core_ops,
+        .video = &hdmi_sd_video_ops,
+};
+
+static int hdmi_runtime_suspend(struct device *dev)
+{
+        struct v4l2_subdev *sd = dev_get_drvdata(dev);
+        struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
+
+        dev_dbg(dev, "%s\n", __func__);
+        hdmi_resource_poweroff(&hdev->res);
+        return 0;
+}
+
+static int hdmi_runtime_resume(struct device *dev)
+{
+        struct v4l2_subdev *sd = dev_get_drvdata(dev);
+        struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
+        int ret = 0;
+
+        dev_dbg(dev, "%s\n", __func__);
+
+        hdmi_resource_poweron(&hdev->res);
+
+        ret = hdmi_conf_apply(hdev);
+        if (ret)
+                goto fail;
+
+        dev_dbg(dev, "poweron succeed\n");
+
+        return 0;
+
+fail:
+        hdmi_resource_poweroff(&hdev->res);
+        dev_err(dev, "poweron failed\n");
+
+        return ret;
+}
+
+static const struct dev_pm_ops hdmi_pm_ops = {
+        .runtime_suspend = hdmi_runtime_suspend,
+        .runtime_resume  = hdmi_runtime_resume,
+};
+
+static void hdmi_resources_cleanup(struct hdmi_device *hdev)
+{
+        struct hdmi_resources *res = &hdev->res;
+
+        dev_dbg(hdev->dev, "HDMI resource cleanup\n");
+        /* put clocks, power */
+        if (res->regul_count)
+                regulator_bulk_free(res->regul_count, res->regul_bulk);
+        /* kfree is NULL-safe */
+        kfree(res->regul_bulk);
+        if (!IS_ERR_OR_NULL(res->hdmiphy))
+                clk_put(res->hdmiphy);
+        if (!IS_ERR_OR_NULL(res->sclk_hdmiphy))
+                clk_put(res->sclk_hdmiphy);
+        if (!IS_ERR_OR_NULL(res->sclk_pixel))
+                clk_put(res->sclk_pixel);
+        if (!IS_ERR_OR_NULL(res->sclk_hdmi))
+                clk_put(res->sclk_hdmi);
+        if (!IS_ERR_OR_NULL(res->hdmi))
+                clk_put(res->hdmi);
+        memset(res, 0, sizeof *res);
+}
+
+static int hdmi_resources_init(struct hdmi_device *hdev)
+{
+        struct device *dev = hdev->dev;
+        struct hdmi_resources *res = &hdev->res;
+        static char *supply[] = {
+                "hdmi-en",
+                "vdd",
+                "vdd_osc",
+                "vdd_pll",
+        };
+        int i, ret;
+
+        dev_dbg(dev, "HDMI resource init\n");
+
+        memset(res, 0, sizeof *res);
+        /* get clocks, power */
+
+        res->hdmi = clk_get(dev, "hdmi");
+        if (IS_ERR_OR_NULL(res->hdmi)) {
+                dev_err(dev, "failed to get clock 'hdmi'\n");
+                goto fail;
+        }
+        res->sclk_hdmi = clk_get(dev, "sclk_hdmi");
+        if (IS_ERR_OR_NULL(res->sclk_hdmi)) {
+                dev_err(dev, "failed to get clock 'sclk_hdmi'\n");
+                goto fail;
+        }
+        res->sclk_pixel = clk_get(dev, "sclk_pixel");
+        if (IS_ERR_OR_NULL(res->sclk_pixel)) {
+                dev_err(dev, "failed to get clock 'sclk_pixel'\n");
+                goto fail;
+        }
+        res->sclk_hdmiphy = clk_get(dev, "sclk_hdmiphy");
+        if (IS_ERR_OR_NULL(res->sclk_hdmiphy)) {
+                dev_err(dev, "failed to get clock 'sclk_hdmiphy'\n");
+                goto fail;
+        }
+        res->hdmiphy = clk_get(dev, "hdmiphy");
+        if (IS_ERR_OR_NULL(res->hdmiphy)) {
+                dev_err(dev, "failed to get clock 'hdmiphy'\n");
+                goto fail;
+        }
+        res->regul_bulk = kzalloc(ARRAY_SIZE(supply) *
+                sizeof res->regul_bulk[0], GFP_KERNEL);
+        if (!res->regul_bulk) {
+                dev_err(dev, "failed to get memory for regulators\n");
+                goto fail;
+        }
+        for (i = 0; i < ARRAY_SIZE(supply); ++i) {
+                res->regul_bulk[i].supply = supply[i];
+                res->regul_bulk[i].consumer = NULL;
+        }
+
+        ret = regulator_bulk_get(dev, ARRAY_SIZE(supply), res->regul_bulk);
+        if (ret) {
+                dev_err(dev, "failed to get regulators\n");
+                goto fail;
+        }
+        res->regul_count = ARRAY_SIZE(supply);
+
+        return 0;
+fail:
+        dev_err(dev, "HDMI resource init - failed\n");
+        hdmi_resources_cleanup(hdev);
+        return -ENODEV;
+}
+
+static int __devinit hdmi_probe(struct platform_device *pdev)
+{
+        struct device *dev = &pdev->dev;
+        struct resource *res;
+        struct i2c_adapter *phy_adapter;
+        struct v4l2_subdev *sd;
+        struct hdmi_device *hdmi_dev = NULL;
+        struct hdmi_driver_data *drv_data;
+        int ret;
+
+        dev_dbg(dev, "probe start\n");
+
+        hdmi_dev = kzalloc(sizeof(*hdmi_dev), GFP_KERNEL);
+        if (!hdmi_dev) {
+                dev_err(dev, "out of memory\n");
+                ret = -ENOMEM;
+                goto fail;
+        }
+
+        hdmi_dev->dev = dev;
+
+        ret = hdmi_resources_init(hdmi_dev);
+        if (ret)
+                goto fail_hdev;
+
+        /* mapping HDMI registers */
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (res == NULL) {
+                dev_err(dev, "get memory resource failed.\n");
+                ret = -ENXIO;
+                goto fail_init;
+        }
+
+        hdmi_dev->regs = ioremap(res->start, resource_size(res));
+        if (hdmi_dev->regs == NULL) {
+                dev_err(dev, "register mapping failed.\n");
+                ret = -ENXIO;
+                goto fail_hdev;
+        }
+
+        res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+        if (res == NULL) {
+                dev_err(dev, "get interrupt resource failed.\n");
+                ret = -ENXIO;
+                goto fail_regs;
+        }
+
+        ret = request_irq(res->start, hdmi_irq_handler, 0, "hdmi", hdmi_dev);
+        if (ret) {
+                dev_err(dev, "request interrupt failed.\n");
+                goto fail_regs;
+        }
+        hdmi_dev->irq = res->start;
+
+        /* setting v4l2 name to prevent WARN_ON in v4l2_device_register */
+        strlcpy(hdmi_dev->v4l2_dev.name, dev_name(dev),
+                sizeof(hdmi_dev->v4l2_dev.name));
+        /* passing NULL owner prevents driver from erasing drvdata */
+        ret = v4l2_device_register(NULL, &hdmi_dev->v4l2_dev);
+        if (ret) {
+                dev_err(dev, "could not register v4l2 device.\n");
+                goto fail_irq;
+        }
+
+        drv_data = (struct hdmi_driver_data *)
+                platform_get_device_id(pdev)->driver_data;
+        phy_adapter = i2c_get_adapter(drv_data->hdmiphy_bus);
+        if (phy_adapter == NULL) {
+                dev_err(dev, "adapter request failed\n");
+                ret = -ENXIO;
+                goto fail_vdev;
+        }
+
+        hdmi_dev->phy_sd = v4l2_i2c_new_subdev_board(&hdmi_dev->v4l2_dev,
+                phy_adapter, &hdmiphy_info, NULL);
+        /* on failure or not adapter is no longer useful */
+        i2c_put_adapter(phy_adapter);
+        if (hdmi_dev->phy_sd == NULL) {
+                dev_err(dev, "missing subdev for hdmiphy\n");
+                ret = -ENODEV;
+                goto fail_vdev;
+        }
+
+        clk_enable(hdmi_dev->res.hdmi);
+
+        pm_runtime_enable(dev);
+
+        sd = &hdmi_dev->sd;
+        v4l2_subdev_init(sd, &hdmi_sd_ops);
+        sd->owner = THIS_MODULE;
+
+        strlcpy(sd->name, "s5p-hdmi", sizeof sd->name);
+        hdmi_dev->cur_preset = HDMI_DEFAULT_PRESET;
+        /* FIXME: missing fail preset is not supported */
+        hdmi_dev->cur_conf = hdmi_preset2conf(hdmi_dev->cur_preset);
+
+        /* storing subdev for call that have only access to struct device */
+        dev_set_drvdata(dev, sd);
+
+        dev_info(dev, "probe sucessful\n");
+
+        return 0;
+
+fail_vdev:
+        v4l2_device_unregister(&hdmi_dev->v4l2_dev);
+
+fail_irq:
+        free_irq(hdmi_dev->irq, hdmi_dev);
+
+fail_regs:
+        iounmap(hdmi_dev->regs);
+
+fail_init:
+        hdmi_resources_cleanup(hdmi_dev);
+
+fail_hdev:
+        kfree(hdmi_dev);
+
+fail:
+        dev_err(dev, "probe failed\n");
+        return ret;
+}
+
+static int __devexit hdmi_remove(struct platform_device *pdev)
+{
+        struct device *dev = &pdev->dev;
+        struct v4l2_subdev *sd = dev_get_drvdata(dev);
+        struct hdmi_device *hdmi_dev = sd_to_hdmi_dev(sd);
+
+        pm_runtime_disable(dev);
+        clk_disable(hdmi_dev->res.hdmi);
+        v4l2_device_unregister(&hdmi_dev->v4l2_dev);
+        disable_irq(hdmi_dev->irq);
+        free_irq(hdmi_dev->irq, hdmi_dev);
+        iounmap(hdmi_dev->regs);
+        hdmi_resources_cleanup(hdmi_dev);
+        kfree(hdmi_dev);
+        dev_info(dev, "remove sucessful\n");
+
+        return 0;
+}
+
+static struct platform_driver hdmi_driver __refdata = {
+        .probe = hdmi_probe,
+        .remove = __devexit_p(hdmi_remove),
+        .id_table = hdmi_driver_types,
+        .driver = {
+                .name = "s5p-hdmi",
+                .owner = THIS_MODULE,
+                .pm = &hdmi_pm_ops,
+        }
+};
+
+/* D R I V E R   I N I T I A L I Z A T I O N */
+
+static int __init hdmi_init(void)
+{
+        int ret;
+        static const char banner[] __initdata = KERN_INFO \
+                "Samsung HDMI output driver, "
+                "(c) 2010-2011 Samsung Electronics Co., Ltd.\n";
+        printk(banner);
+
+        ret = platform_driver_register(&hdmi_driver);
+        if (ret)
+                printk(KERN_ERR "HDMI platform driver register failed\n");
+
+        return ret;
+}
+module_init(hdmi_init);
+
+static void __exit hdmi_exit(void)
+{
+        platform_driver_unregister(&hdmi_driver);
+}
+module_exit(hdmi_exit);
+
+
diff --git a/drivers/media/video/s5p-tv/hdmiphy_drv.c b/drivers/media/video/s5p-tv/hdmiphy_drv.c
new file mode 100644
index 00000000000..6693f4aff10
--- /dev/null
+++ b/drivers/media/video/s5p-tv/hdmiphy_drv.c
@@ -0,0 +1,188 @@
+/*
+ * Samsung HDMI Physical interface driver
+ *
+ * Copyright (C) 2010-2011 Samsung Electronics Co.Ltd
+ * Author: Tomasz Stanislawski <t.stanislaws@samsung.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/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/err.h>
+
+#include <media/v4l2-subdev.h>
+
+MODULE_AUTHOR("Tomasz Stanislawski <t.stanislaws@samsung.com>");
+MODULE_DESCRIPTION("Samsung HDMI Physical interface driver");
+MODULE_LICENSE("GPL");
+
+struct hdmiphy_conf {
+        u32 preset;
+        const u8 *data;
+};
+
+static const u8 hdmiphy_conf27[32] = {
+        0x01, 0x05, 0x00, 0xD8, 0x10, 0x1C, 0x30, 0x40,
+        0x6B, 0x10, 0x02, 0x51, 0xDf, 0xF2, 0x54, 0x87,
+        0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+        0x22, 0x40, 0xe3, 0x26, 0x00, 0x00, 0x00, 0x00,
+};
+
+static const u8 hdmiphy_conf74_175[32] = {
+        0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xef, 0x5B,
+        0x6D, 0x10, 0x01, 0x51, 0xef, 0xF3, 0x54, 0xb9,
+        0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+        0x22, 0x40, 0xa5, 0x26, 0x01, 0x00, 0x00, 0x00,
+};
+
+static const u8 hdmiphy_conf74_25[32] = {
+        0x01, 0x05, 0x00, 0xd8, 0x10, 0x9c, 0xf8, 0x40,
+        0x6a, 0x10, 0x01, 0x51, 0xff, 0xf1, 0x54, 0xba,
+        0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xe0,
+        0x22, 0x40, 0xa4, 0x26, 0x01, 0x00, 0x00, 0x00,
+};
+
+static const u8 hdmiphy_conf148_5[32] = {
+        0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xf8, 0x40,
+        0x6A, 0x18, 0x00, 0x51, 0xff, 0xF1, 0x54, 0xba,
+        0x84, 0x00, 0x10, 0x38, 0x00, 0x08, 0x10, 0xE0,
+        0x22, 0x40, 0xa4, 0x26, 0x02, 0x00, 0x00, 0x00,
+};
+
+static const struct hdmiphy_conf hdmiphy_conf[] = {
+        { V4L2_DV_480P59_94, hdmiphy_conf27 },
+        { V4L2_DV_1080P30, hdmiphy_conf74_175 },
+        { V4L2_DV_720P59_94, hdmiphy_conf74_175 },
+        { V4L2_DV_720P60, hdmiphy_conf74_25 },
+        { V4L2_DV_1080P50, hdmiphy_conf148_5 },
+        { V4L2_DV_1080P60, hdmiphy_conf148_5 },
+};
+
+const u8 *hdmiphy_preset2conf(u32 preset)
+{
+        int i;
+        for (i = 0; i < ARRAY_SIZE(hdmiphy_conf); ++i)
+                if (hdmiphy_conf[i].preset == preset)
+                        return hdmiphy_conf[i].data;
+        return NULL;
+}
+
+static int hdmiphy_s_power(struct v4l2_subdev *sd, int on)
+{
+        /* to be implemented */
+        return 0;
+}
+
+static int hdmiphy_s_dv_preset(struct v4l2_subdev *sd,
+        struct v4l2_dv_preset *preset)
+{
+        const u8 *data;
+        u8 buffer[32];
+        int ret;
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct device *dev = &client->dev;
+
+        dev_info(dev, "s_dv_preset(preset = %d)\n", preset->preset);
+        data = hdmiphy_preset2conf(preset->preset);
+        if (!data) {
+                dev_err(dev, "format not supported\n");
+                return -EINVAL;
+        }
+
+        /* storing configuration to the device */
+        memcpy(buffer, data, 32);
+        ret = i2c_master_send(client, buffer, 32);
+        if (ret != 32) {
+                dev_err(dev, "failed to configure HDMIPHY via I2C\n");
+                return -EIO;
+        }
+
+        return 0;
+}
+
+static int hdmiphy_s_stream(struct v4l2_subdev *sd, int enable)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct device *dev = &client->dev;
+        u8 buffer[2];
+        int ret;
+
+        dev_info(dev, "s_stream(%d)\n", enable);
+        /* going to/from configuration from/to operation mode */
+        buffer[0] = 0x1f;
+        buffer[1] = enable ? 0x80 : 0x00;
+
+        ret = i2c_master_send(client, buffer, 2);
+        if (ret != 2) {
+                dev_err(dev, "stream (%d) failed\n", enable);
+                return -EIO;
+        }
+        return 0;
+}
+
+static const struct v4l2_subdev_core_ops hdmiphy_core_ops = {
+        .s_power =  hdmiphy_s_power,
+};
+
+static const struct v4l2_subdev_video_ops hdmiphy_video_ops = {
+        .s_dv_preset = hdmiphy_s_dv_preset,
+        .s_stream =  hdmiphy_s_stream,
+};
+
+static const struct v4l2_subdev_ops hdmiphy_ops = {
+        .core = &hdmiphy_core_ops,
+        .video = &hdmiphy_video_ops,
+};
+
+static int __devinit hdmiphy_probe(struct i2c_client *client,
+        const struct i2c_device_id *id)
+{
+        static struct v4l2_subdev sd;
+
+        v4l2_i2c_subdev_init(&sd, client, &hdmiphy_ops);
+        dev_info(&client->dev, "probe successful\n");
+        return 0;
+}
+
+static int __devexit hdmiphy_remove(struct i2c_client *client)
+{
+        dev_info(&client->dev, "remove successful\n");
+        return 0;
+}
+
+static const struct i2c_device_id hdmiphy_id[] = {
+        { "hdmiphy", 0 },
+        { },
+};
+MODULE_DEVICE_TABLE(i2c, hdmiphy_id);
+
+static struct i2c_driver hdmiphy_driver = {
+        .driver = {
+                .name   = "s5p-hdmiphy",
+                .owner  = THIS_MODULE,
+        },
+        .probe          = hdmiphy_probe,
+        .remove         = __devexit_p(hdmiphy_remove),
+        .id_table = hdmiphy_id,
+};
+
+static int __init hdmiphy_init(void)
+{
+        return i2c_add_driver(&hdmiphy_driver);
+}
+module_init(hdmiphy_init);
+
+static void __exit hdmiphy_exit(void)
+{
+        i2c_del_driver(&hdmiphy_driver);
+}
+module_exit(hdmiphy_exit);
diff --git a/drivers/media/video/s5p-tv/mixer.h b/drivers/media/video/s5p-tv/mixer.h
new file mode 100644
index 00000000000..e2242243f63
--- /dev/null
+++ b/drivers/media/video/s5p-tv/mixer.h
@@ -0,0 +1,354 @@
+/*
+ * Samsung TV Mixer driver
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *
+ * Tomasz Stanislawski, <t.stanislaws@samsung.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 Foundiation. either version 2 of the License,
+ * or (at your option) any later version
+ */
+
+#ifndef SAMSUNG_MIXER_H
+#define SAMSUNG_MIXER_H
+
+#ifdef CONFIG_VIDEO_SAMSUNG_S5P_MIXER_DEBUG
+        #define DEBUG
+#endif
+
+#include <linux/fb.h>
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/wait.h>
+#include <media/v4l2-device.h>
+#include <media/videobuf2-core.h>
+
+#include "regs-mixer.h"
+
+/** maximum number of output interfaces */
+#define MXR_MAX_OUTPUTS 2
+/** maximum number of input interfaces (layers) */
+#define MXR_MAX_LAYERS 3
+#define MXR_DRIVER_NAME "s5p-mixer"
+/** maximal number of planes for every layer */
+#define MXR_MAX_PLANES  2
+
+#define MXR_ENABLE 1
+#define MXR_DISABLE 0
+
+/** description of a macroblock for packed formats */
+struct mxr_block {
+        /** vertical number of pixels in macroblock */
+        unsigned int width;
+        /** horizontal number of pixels in macroblock */
+        unsigned int height;
+        /** size of block in bytes */
+        unsigned int size;
+};
+
+/** description of supported format */
+struct mxr_format {
+        /** format name/mnemonic */
+        const char *name;
+        /** fourcc identifier */
+        u32 fourcc;
+        /** colorspace identifier */
+        enum v4l2_colorspace colorspace;
+        /** number of planes in image data */
+        int num_planes;
+        /** description of block for each plane */
+        struct mxr_block plane[MXR_MAX_PLANES];
+        /** number of subframes in image data */
+        int num_subframes;
+        /** specifies to which subframe belong given plane */
+        int plane2subframe[MXR_MAX_PLANES];
+        /** internal code, driver dependant */
+        unsigned long cookie;
+};
+
+/** description of crop configuration for image */
+struct mxr_crop {
+        /** width of layer in pixels */
+        unsigned int full_width;
+        /** height of layer in pixels */
+        unsigned int full_height;
+        /** horizontal offset of first pixel to be displayed */
+        unsigned int x_offset;
+        /** vertical offset of first pixel to be displayed */
+        unsigned int y_offset;
+        /** width of displayed data in pixels */
+        unsigned int width;
+        /** height of displayed data in pixels */
+        unsigned int height;
+        /** indicate which fields are present in buffer */
+        unsigned int field;
+};
+
+/** description of transformation from source to destination image */
+struct mxr_geometry {
+        /** cropping for source image */
+        struct mxr_crop src;
+        /** cropping for destination image */
+        struct mxr_crop dst;
+        /** layer-dependant description of horizontal scaling */
+        unsigned int x_ratio;
+        /** layer-dependant description of vertical scaling */
+        unsigned int y_ratio;
+};
+
+/** instance of a buffer */
+struct mxr_buffer {
+        /** common v4l buffer stuff -- must be first */
+        struct vb2_buffer       vb;
+        /** node for layer's lists */
+        struct list_head        list;
+};
+
+
+/** internal states of layer */
+enum mxr_layer_state {
+        /** layers is not shown */
+        MXR_LAYER_IDLE = 0,
+        /** state between STREAMON and hardware start */
+        MXR_LAYER_STREAMING_START,
+        /** layer is shown */
+        MXR_LAYER_STREAMING,
+        /** state before STREAMOFF is finished */
+        MXR_LAYER_STREAMING_FINISH,
+};
+
+/** forward declarations */
+struct mxr_device;
+struct mxr_layer;
+
+/** callback for layers operation */
+struct mxr_layer_ops {
+        /* TODO: try to port it to subdev API */
+        /** handler for resource release function */
+        void (*release)(struct mxr_layer *);
+        /** setting buffer to HW */
+        void (*buffer_set)(struct mxr_layer *, struct mxr_buffer *);
+        /** setting format and geometry in HW */
+        void (*format_set)(struct mxr_layer *);
+        /** streaming stop/start */
+        void (*stream_set)(struct mxr_layer *, int);
+        /** adjusting geometry */
+        void (*fix_geometry)(struct mxr_layer *);
+};
+
+/** layer instance, a single window and content displayed on output */
+struct mxr_layer {
+        /** parent mixer device */
+        struct mxr_device *mdev;
+        /** layer index (unique identifier) */
+        int idx;
+        /** callbacks for layer methods */
+        struct mxr_layer_ops ops;
+        /** format array */
+        const struct mxr_format **fmt_array;
+        /** size of format array */
+        unsigned long fmt_array_size;
+
+        /** lock for protection of list and state fields */
+        spinlock_t enq_slock;
+        /** list for enqueued buffers */
+        struct list_head enq_list;
+        /** buffer currently owned by hardware in temporary registers */
+        struct mxr_buffer *update_buf;
+        /** buffer currently owned by hardware in shadow registers */
+        struct mxr_buffer *shadow_buf;
+        /** state of layer IDLE/STREAMING */
+        enum mxr_layer_state state;
+
+        /** mutex for protection of fields below */
+        struct mutex mutex;
+        /** handler for video node */
+        struct video_device vfd;
+        /** queue for output buffers */
+        struct vb2_queue vb_queue;
+        /** current image format */
+        const struct mxr_format *fmt;
+        /** current geometry of image */
+        struct mxr_geometry geo;
+};
+
+/** description of mixers output interface */
+struct mxr_output {
+        /** name of output */
+        char name[32];
+        /** output subdev */
+        struct v4l2_subdev *sd;
+        /** cookie used for configuration of registers */
+        int cookie;
+};
+
+/** specify source of output subdevs */
+struct mxr_output_conf {
+        /** name of output (connector) */
+        char *output_name;
+        /** name of module that generates output subdev */
+        char *module_name;
+        /** cookie need for mixer HW */
+        int cookie;
+};
+
+struct clk;
+struct regulator;
+
+/** auxiliary resources used my mixer */
+struct mxr_resources {
+        /** interrupt index */
+        int irq;
+        /** pointer to Mixer registers */
+        void __iomem *mxr_regs;
+        /** pointer to Video Processor registers */
+        void __iomem *vp_regs;
+        /** other resources, should used under mxr_device.mutex */
+        struct clk *mixer;
+        struct clk *vp;
+        struct clk *sclk_mixer;
+        struct clk *sclk_hdmi;
+        struct clk *sclk_dac;
+};
+
+/* event flags used  */
+enum mxr_devide_flags {
+        MXR_EVENT_VSYNC = 0,
+};
+
+/** drivers instance */
+struct mxr_device {
+        /** master device */
+        struct device *dev;
+        /** state of each layer */
+        struct mxr_layer *layer[MXR_MAX_LAYERS];
+        /** state of each output */
+        struct mxr_output *output[MXR_MAX_OUTPUTS];
+        /** number of registered outputs */
+        int output_cnt;
+
+        /* video resources */
+
+        /** V4L2 device */
+        struct v4l2_device v4l2_dev;
+        /** context of allocator */
+        void *alloc_ctx;
+        /** event wait queue */
+        wait_queue_head_t event_queue;
+        /** state flags */
+        unsigned long event_flags;
+
+        /** spinlock for protection of registers */
+        spinlock_t reg_slock;
+
+        /** mutex for protection of fields below */
+        struct mutex mutex;
+        /** number of entities depndant on output configuration */
+        int n_output;
+        /** number of users that do streaming */
+        int n_streamer;
+        /** index of current output */
+        int current_output;
+        /** auxiliary resources used my mixer */
+        struct mxr_resources res;
+};
+
+/** transform device structure into mixer device */
+static inline struct mxr_device *to_mdev(struct device *dev)
+{
+        struct v4l2_device *vdev = dev_get_drvdata(dev);
+        return container_of(vdev, struct mxr_device, v4l2_dev);
+}
+
+/** get current output data, should be called under mdev's mutex */
+static inline struct mxr_output *to_output(struct mxr_device *mdev)
+{
+        return mdev->output[mdev->current_output];
+}
+
+/** get current output subdev, should be called under mdev's mutex */
+static inline struct v4l2_subdev *to_outsd(struct mxr_device *mdev)
+{
+        struct mxr_output *out = to_output(mdev);
+        return out ? out->sd : NULL;
+}
+
+/** forward declaration for mixer platform data */
+struct mxr_platform_data;
+
+/** acquiring common video resources */
+int __devinit mxr_acquire_video(struct mxr_device *mdev,
+        struct mxr_output_conf *output_cont, int output_count);
+
+/** releasing common video resources */
+void __devexit mxr_release_video(struct mxr_device *mdev);
+
+struct mxr_layer *mxr_graph_layer_create(struct mxr_device *mdev, int idx);
+struct mxr_layer *mxr_vp_layer_create(struct mxr_device *mdev, int idx);
+struct mxr_layer *mxr_base_layer_create(struct mxr_device *mdev,
+        int idx, char *name, struct mxr_layer_ops *ops);
+
+void mxr_base_layer_release(struct mxr_layer *layer);
+void mxr_layer_release(struct mxr_layer *layer);
+
+int mxr_base_layer_register(struct mxr_layer *layer);
+void mxr_base_layer_unregister(struct mxr_layer *layer);
+
+unsigned long mxr_get_plane_size(const struct mxr_block *blk,
+        unsigned int width, unsigned int height);
+
+/** adds new consumer for mixer's power */
+int __must_check mxr_power_get(struct mxr_device *mdev);
+/** removes consumer for mixer's power */
+void mxr_power_put(struct mxr_device *mdev);
+/** add new client for output configuration */
+void mxr_output_get(struct mxr_device *mdev);
+/** removes new client for output configuration */
+void mxr_output_put(struct mxr_device *mdev);
+/** add new client for streaming */
+void mxr_streamer_get(struct mxr_device *mdev);
+/** removes new client for streaming */
+void mxr_streamer_put(struct mxr_device *mdev);
+/** returns format of data delivared to current output */
+void mxr_get_mbus_fmt(struct mxr_device *mdev,
+        struct v4l2_mbus_framefmt *mbus_fmt);
+
+/* Debug */
+
+#define mxr_err(mdev, fmt, ...)  dev_err(mdev->dev, fmt, ##__VA_ARGS__)
+#define mxr_warn(mdev, fmt, ...) dev_warn(mdev->dev, fmt, ##__VA_ARGS__)
+#define mxr_info(mdev, fmt, ...) dev_info(mdev->dev, fmt, ##__VA_ARGS__)
+
+#ifdef CONFIG_VIDEO_SAMSUNG_S5P_MIXER_DEBUG
+        #define mxr_dbg(mdev, fmt, ...)  dev_dbg(mdev->dev, fmt, ##__VA_ARGS__)
+#else
+        #define mxr_dbg(mdev, fmt, ...)  do { (void) mdev; } while (0)
+#endif
+
+/* accessing Mixer's and Video Processor's registers */
+
+void mxr_vsync_set_update(struct mxr_device *mdev, int en);
+void mxr_reg_reset(struct mxr_device *mdev);
+irqreturn_t mxr_irq_handler(int irq, void *dev_data);
+void mxr_reg_s_output(struct mxr_device *mdev, int cookie);
+void mxr_reg_streamon(struct mxr_device *mdev);
+void mxr_reg_streamoff(struct mxr_device *mdev);
+int mxr_reg_wait4vsync(struct mxr_device *mdev);
+void mxr_reg_set_mbus_fmt(struct mxr_device *mdev,
+        struct v4l2_mbus_framefmt *fmt);
+void mxr_reg_graph_layer_stream(struct mxr_device *mdev, int idx, int en);
+void mxr_reg_graph_buffer(struct mxr_device *mdev, int idx, dma_addr_t addr);
+void mxr_reg_graph_format(struct mxr_device *mdev, int idx,
+        const struct mxr_format *fmt, const struct mxr_geometry *geo);
+
+void mxr_reg_vp_layer_stream(struct mxr_device *mdev, int en);
+void mxr_reg_vp_buffer(struct mxr_device *mdev,
+        dma_addr_t luma_addr[2], dma_addr_t chroma_addr[2]);
+void mxr_reg_vp_format(struct mxr_device *mdev,
+        const struct mxr_format *fmt, const struct mxr_geometry *geo);
+void mxr_reg_dump(struct mxr_device *mdev);
+
+#endif /* SAMSUNG_MIXER_H */
+
diff --git a/drivers/media/video/s5p-tv/mixer_drv.c b/drivers/media/video/s5p-tv/mixer_drv.c
new file mode 100644
index 00000000000..00643094b22
--- /dev/null
+++ b/drivers/media/video/s5p-tv/mixer_drv.c
@@ -0,0 +1,487 @@
+/*
+ * Samsung TV Mixer driver
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *
+ * Tomasz Stanislawski, <t.stanislaws@samsung.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 Foundiation. either version 2 of the License,
+ * or (at your option) any later version
+ */
+
+#include "mixer.h"
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/fb.h>
+#include <linux/delay.h>
+#include <linux/pm_runtime.h>
+#include <linux/clk.h>
+
+MODULE_AUTHOR("Tomasz Stanislawski, <t.stanislaws@samsung.com>");
+MODULE_DESCRIPTION("Samsung MIXER");
+MODULE_LICENSE("GPL");
+
+/* --------- DRIVER PARAMETERS ---------- */
+
+static struct mxr_output_conf mxr_output_conf[] = {
+        {
+                .output_name = "S5P HDMI connector",
+                .module_name = "s5p-hdmi",
+                .cookie = 1,
+        },
+        {
+                .output_name = "S5P SDO connector",
+                .module_name = "s5p-sdo",
+                .cookie = 0,
+        },
+};
+
+void mxr_get_mbus_fmt(struct mxr_device *mdev,
+        struct v4l2_mbus_framefmt *mbus_fmt)
+{
+        struct v4l2_subdev *sd;
+        int ret;
+
+        mutex_lock(&mdev->mutex);
+        sd = to_outsd(mdev);
+        ret = v4l2_subdev_call(sd, video, g_mbus_fmt, mbus_fmt);
+        WARN(ret, "failed to get mbus_fmt for output %s\n", sd->name);
+        mutex_unlock(&mdev->mutex);
+}
+
+void mxr_streamer_get(struct mxr_device *mdev)
+{
+        mutex_lock(&mdev->mutex);
+        ++mdev->n_streamer;
+        mxr_dbg(mdev, "%s(%d)\n", __func__, mdev->n_streamer);
+        if (mdev->n_streamer == 1) {
+                struct v4l2_subdev *sd = to_outsd(mdev);
+                struct v4l2_mbus_framefmt mbus_fmt;
+                struct mxr_resources *res = &mdev->res;
+                int ret;
+
+                if (to_output(mdev)->cookie == 0)
+                        clk_set_parent(res->sclk_mixer, res->sclk_dac);
+                else
+                        clk_set_parent(res->sclk_mixer, res->sclk_hdmi);
+                mxr_reg_s_output(mdev, to_output(mdev)->cookie);
+
+                ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mbus_fmt);
+                WARN(ret, "failed to get mbus_fmt for output %s\n", sd->name);
+                ret = v4l2_subdev_call(sd, video, s_stream, 1);
+                WARN(ret, "starting stream failed for output %s\n", sd->name);
+
+                mxr_reg_set_mbus_fmt(mdev, &mbus_fmt);
+                mxr_reg_streamon(mdev);
+                ret = mxr_reg_wait4vsync(mdev);
+                WARN(ret, "failed to get vsync (%d) from output\n", ret);
+        }
+        mutex_unlock(&mdev->mutex);
+        mxr_reg_dump(mdev);
+        /* FIXME: what to do when streaming fails? */
+}
+
+void mxr_streamer_put(struct mxr_device *mdev)
+{
+        mutex_lock(&mdev->mutex);
+        --mdev->n_streamer;
+        mxr_dbg(mdev, "%s(%d)\n", __func__, mdev->n_streamer);
+        if (mdev->n_streamer == 0) {
+                int ret;
+                struct v4l2_subdev *sd = to_outsd(mdev);
+
+                mxr_reg_streamoff(mdev);
+                /* vsync applies Mixer setup */
+                ret = mxr_reg_wait4vsync(mdev);
+                WARN(ret, "failed to get vsync (%d) from output\n", ret);
+                ret = v4l2_subdev_call(sd, video, s_stream, 0);
+                WARN(ret, "stopping stream failed for output %s\n", sd->name);
+        }
+        WARN(mdev->n_streamer < 0, "negative number of streamers (%d)\n",
+                mdev->n_streamer);
+        mutex_unlock(&mdev->mutex);
+        mxr_reg_dump(mdev);
+}
+
+void mxr_output_get(struct mxr_device *mdev)
+{
+        mutex_lock(&mdev->mutex);
+        ++mdev->n_output;
+        mxr_dbg(mdev, "%s(%d)\n", __func__, mdev->n_output);
+        /* turn on auxiliary driver */
+        if (mdev->n_output == 1)
+                v4l2_subdev_call(to_outsd(mdev), core, s_power, 1);
+        mutex_unlock(&mdev->mutex);
+}
+
+void mxr_output_put(struct mxr_device *mdev)
+{
+        mutex_lock(&mdev->mutex);
+        --mdev->n_output;
+        mxr_dbg(mdev, "%s(%d)\n", __func__, mdev->n_output);
+        /* turn on auxiliary driver */
+        if (mdev->n_output == 0)
+                v4l2_subdev_call(to_outsd(mdev), core, s_power, 0);
+        WARN(mdev->n_output < 0, "negative number of output users (%d)\n",
+                mdev->n_output);
+        mutex_unlock(&mdev->mutex);
+}
+
+int mxr_power_get(struct mxr_device *mdev)
+{
+        int ret = pm_runtime_get_sync(mdev->dev);
+
+        /* returning 1 means that power is already enabled,
+         * so zero success be returned */
+        if (IS_ERR_VALUE(ret))
+                return ret;
+        return 0;
+}
+
+void mxr_power_put(struct mxr_device *mdev)
+{
+        pm_runtime_put_sync(mdev->dev);
+}
+
+/* --------- RESOURCE MANAGEMENT -------------*/
+
+static int __devinit mxr_acquire_plat_resources(struct mxr_device *mdev,
+        struct platform_device *pdev)
+{
+        struct resource *res;
+        int ret;
+
+        res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "mxr");
+        if (res == NULL) {
+                mxr_err(mdev, "get memory resource failed.\n");
+                ret = -ENXIO;
+                goto fail;
+        }
+
+        mdev->res.mxr_regs = ioremap(res->start, resource_size(res));
+        if (mdev->res.mxr_regs == NULL) {
+                mxr_err(mdev, "register mapping failed.\n");
+                ret = -ENXIO;
+                goto fail;
+        }
+
+        res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "vp");
+        if (res == NULL) {
+                mxr_err(mdev, "get memory resource failed.\n");
+                ret = -ENXIO;
+                goto fail_mxr_regs;
+        }
+
+        mdev->res.vp_regs = ioremap(res->start, resource_size(res));
+        if (mdev->res.vp_regs == NULL) {
+                mxr_err(mdev, "register mapping failed.\n");
+                ret = -ENXIO;
+                goto fail_mxr_regs;
+        }
+
+        res = platform_get_resource_byname(pdev, IORESOURCE_IRQ, "irq");
+        if (res == NULL) {
+                mxr_err(mdev, "get interrupt resource failed.\n");
+                ret = -ENXIO;
+                goto fail_vp_regs;
+        }
+
+        ret = request_irq(res->start, mxr_irq_handler, 0, "s5p-mixer", mdev);
+        if (ret) {
+                mxr_err(mdev, "request interrupt failed.\n");
+                goto fail_vp_regs;
+        }
+        mdev->res.irq = res->start;
+
+        return 0;
+
+fail_vp_regs:
+        iounmap(mdev->res.vp_regs);
+
+fail_mxr_regs:
+        iounmap(mdev->res.mxr_regs);
+
+fail:
+        return ret;
+}
+
+static void mxr_release_plat_resources(struct mxr_device *mdev)
+{
+        free_irq(mdev->res.irq, mdev);
+        iounmap(mdev->res.vp_regs);
+        iounmap(mdev->res.mxr_regs);
+}
+
+static void mxr_release_clocks(struct mxr_device *mdev)
+{
+        struct mxr_resources *res = &mdev->res;
+
+        if (!IS_ERR_OR_NULL(res->sclk_dac))
+                clk_put(res->sclk_dac);
+        if (!IS_ERR_OR_NULL(res->sclk_hdmi))
+                clk_put(res->sclk_hdmi);
+        if (!IS_ERR_OR_NULL(res->sclk_mixer))
+                clk_put(res->sclk_mixer);
+        if (!IS_ERR_OR_NULL(res->vp))
+                clk_put(res->vp);
+        if (!IS_ERR_OR_NULL(res->mixer))
+                clk_put(res->mixer);
+}
+
+static int mxr_acquire_clocks(struct mxr_device *mdev)
+{
+        struct mxr_resources *res = &mdev->res;
+        struct device *dev = mdev->dev;
+
+        res->mixer = clk_get(dev, "mixer");
+        if (IS_ERR_OR_NULL(res->mixer)) {
+                mxr_err(mdev, "failed to get clock 'mixer'\n");
+                goto fail;
+        }
+        res->vp = clk_get(dev, "vp");
+        if (IS_ERR_OR_NULL(res->vp)) {
+                mxr_err(mdev, "failed to get clock 'vp'\n");
+                goto fail;
+        }
+        res->sclk_mixer = clk_get(dev, "sclk_mixer");
+        if (IS_ERR_OR_NULL(res->sclk_mixer)) {
+                mxr_err(mdev, "failed to get clock 'sclk_mixer'\n");
+                goto fail;
+        }
+        res->sclk_hdmi = clk_get(dev, "sclk_hdmi");
+        if (IS_ERR_OR_NULL(res->sclk_hdmi)) {
+                mxr_err(mdev, "failed to get clock 'sclk_hdmi'\n");
+                goto fail;
+        }
+        res->sclk_dac = clk_get(dev, "sclk_dac");
+        if (IS_ERR_OR_NULL(res->sclk_dac)) {
+                mxr_err(mdev, "failed to get clock 'sclk_dac'\n");
+                goto fail;
+        }
+
+        return 0;
+fail:
+        mxr_release_clocks(mdev);
+        return -ENODEV;
+}
+
+static int __devinit mxr_acquire_resources(struct mxr_device *mdev,
+        struct platform_device *pdev)
+{
+        int ret;
+        ret = mxr_acquire_plat_resources(mdev, pdev);
+
+        if (ret)
+                goto fail;
+
+        ret = mxr_acquire_clocks(mdev);
+        if (ret)
+                goto fail_plat;
+
+        mxr_info(mdev, "resources acquired\n");
+        return 0;
+
+fail_plat:
+        mxr_release_plat_resources(mdev);
+fail:
+        mxr_err(mdev, "resources acquire failed\n");
+        return ret;
+}
+
+static void mxr_release_resources(struct mxr_device *mdev)
+{
+        mxr_release_clocks(mdev);
+        mxr_release_plat_resources(mdev);
+        memset(&mdev->res, 0, sizeof mdev->res);
+}
+
+static void mxr_release_layers(struct mxr_device *mdev)
+{
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(mdev->layer); ++i)
+                if (mdev->layer[i])
+                        mxr_layer_release(mdev->layer[i]);
+}
+
+static int __devinit mxr_acquire_layers(struct mxr_device *mdev,
+        struct mxr_platform_data *pdata)
+{
+        mdev->layer[0] = mxr_graph_layer_create(mdev, 0);
+        mdev->layer[1] = mxr_graph_layer_create(mdev, 1);
+        mdev->layer[2] = mxr_vp_layer_create(mdev, 0);
+
+        if (!mdev->layer[0] || !mdev->layer[1] || !mdev->layer[2]) {
+                mxr_err(mdev, "failed to acquire layers\n");
+                goto fail;
+        }
+
+        return 0;
+
+fail:
+        mxr_release_layers(mdev);
+        return -ENODEV;
+}
+
+/* ---------- POWER MANAGEMENT ----------- */
+
+static int mxr_runtime_resume(struct device *dev)
+{
+        struct mxr_device *mdev = to_mdev(dev);
+        struct mxr_resources *res = &mdev->res;
+
+        mxr_dbg(mdev, "resume - start\n");
+        mutex_lock(&mdev->mutex);
+        /* turn clocks on */
+        clk_enable(res->mixer);
+        clk_enable(res->vp);
+        clk_enable(res->sclk_mixer);
+        /* apply default configuration */
+        mxr_reg_reset(mdev);
+        mxr_dbg(mdev, "resume - finished\n");
+
+        mutex_unlock(&mdev->mutex);
+        return 0;
+}
+
+static int mxr_runtime_suspend(struct device *dev)
+{
+        struct mxr_device *mdev = to_mdev(dev);
+        struct mxr_resources *res = &mdev->res;
+        mxr_dbg(mdev, "suspend - start\n");
+        mutex_lock(&mdev->mutex);
+        /* turn clocks off */
+        clk_disable(res->sclk_mixer);
+        clk_disable(res->vp);
+        clk_disable(res->mixer);
+        mutex_unlock(&mdev->mutex);
+        mxr_dbg(mdev, "suspend - finished\n");
+        return 0;
+}
+
+static const struct dev_pm_ops mxr_pm_ops = {
+        .runtime_suspend = mxr_runtime_suspend,
+        .runtime_resume  = mxr_runtime_resume,
+};
+
+/* --------- DRIVER INITIALIZATION ---------- */
+
+static int __devinit mxr_probe(struct platform_device *pdev)
+{
+        struct device *dev = &pdev->dev;
+        struct mxr_platform_data *pdata = dev->platform_data;
+        struct mxr_device *mdev;
+        int ret;
+
+        /* mdev does not exist yet so no mxr_dbg is used */
+        dev_info(dev, "probe start\n");
+
+        mdev = kzalloc(sizeof *mdev, GFP_KERNEL);
+        if (!mdev) {
+                mxr_err(mdev, "not enough memory.\n");
+                ret = -ENOMEM;
+                goto fail;
+        }
+
+        /* setup pointer to master device */
+        mdev->dev = dev;
+
+        mutex_init(&mdev->mutex);
+        spin_lock_init(&mdev->reg_slock);
+        init_waitqueue_head(&mdev->event_queue);
+
+        /* acquire resources: regs, irqs, clocks, regulators */
+        ret = mxr_acquire_resources(mdev, pdev);
+        if (ret)
+                goto fail_mem;
+
+        /* configure resources for video output */
+        ret = mxr_acquire_video(mdev, mxr_output_conf,
+                ARRAY_SIZE(mxr_output_conf));
+        if (ret)
+                goto fail_resources;
+
+        /* configure layers */
+        ret = mxr_acquire_layers(mdev, pdata);
+        if (ret)
+                goto fail_video;
+
+        pm_runtime_enable(dev);
+
+        mxr_info(mdev, "probe successful\n");
+        return 0;
+
+fail_video:
+        mxr_release_video(mdev);
+
+fail_resources:
+        mxr_release_resources(mdev);
+
+fail_mem:
+        kfree(mdev);
+
+fail:
+        dev_info(dev, "probe failed\n");
+        return ret;
+}
+
+static int __devexit mxr_remove(struct platform_device *pdev)
+{
+        struct device *dev = &pdev->dev;
+        struct mxr_device *mdev = to_mdev(dev);
+
+        pm_runtime_disable(dev);
+
+        mxr_release_layers(mdev);
+        mxr_release_video(mdev);
+        mxr_release_resources(mdev);
+
+        kfree(mdev);
+
+        dev_info(dev, "remove sucessful\n");
+        return 0;
+}
+
+static struct platform_driver mxr_driver __refdata = {
+        .probe = mxr_probe,
+        .remove = __devexit_p(mxr_remove),
+        .driver = {
+                .name = MXR_DRIVER_NAME,
+                .owner = THIS_MODULE,
+                .pm = &mxr_pm_ops,
+        }
+};
+
+static int __init mxr_init(void)
+{
+        int i, ret;
+        static const char banner[] __initdata = KERN_INFO
+                "Samsung TV Mixer driver, "
+                "(c) 2010-2011 Samsung Electronics Co., Ltd.\n";
+        printk(banner);
+
+        /* Loading auxiliary modules */
+        for (i = 0; i < ARRAY_SIZE(mxr_output_conf); ++i)
+                request_module(mxr_output_conf[i].module_name);
+
+        ret = platform_driver_register(&mxr_driver);
+        if (ret != 0) {
+                printk(KERN_ERR "registration of MIXER driver failed\n");
+                return -ENXIO;
+        }
+
+        return 0;
+}
+module_init(mxr_init);
+
+static void __exit mxr_exit(void)
+{
+        platform_driver_unregister(&mxr_driver);
+}
+module_exit(mxr_exit);
diff --git a/drivers/media/video/s5p-tv/mixer_grp_layer.c b/drivers/media/video/s5p-tv/mixer_grp_layer.c
new file mode 100644
index 00000000000..58f0ba49580
--- /dev/null
+++ b/drivers/media/video/s5p-tv/mixer_grp_layer.c
@@ -0,0 +1,185 @@
+/*
+ * Samsung TV Mixer driver
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *
+ * Tomasz Stanislawski, <t.stanislaws@samsung.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 Foundiation. either version 2 of the License,
+ * or (at your option) any later version
+ */
+
+#include "mixer.h"
+
+#include <media/videobuf2-dma-contig.h>
+
+/* FORMAT DEFINITIONS */
+
+static const struct mxr_format mxr_fb_fmt_rgb565 = {
+        .name = "RGB565",
+        .fourcc = V4L2_PIX_FMT_RGB565,
+        .colorspace = V4L2_COLORSPACE_SRGB,
+        .num_planes = 1,
+        .plane = {
+                { .width = 1, .height = 1, .size = 2 },
+        },
+        .num_subframes = 1,
+        .cookie = 4,
+};
+
+static const struct mxr_format mxr_fb_fmt_argb1555 = {
+        .name = "ARGB1555",
+        .num_planes = 1,
+        .fourcc = V4L2_PIX_FMT_RGB555,
+        .colorspace = V4L2_COLORSPACE_SRGB,
+        .plane = {
+                { .width = 1, .height = 1, .size = 2 },
+        },
+        .num_subframes = 1,
+        .cookie = 5,
+};
+
+static const struct mxr_format mxr_fb_fmt_argb4444 = {
+        .name = "ARGB4444",
+        .num_planes = 1,
+        .fourcc = V4L2_PIX_FMT_RGB444,
+        .colorspace = V4L2_COLORSPACE_SRGB,
+        .plane = {
+                { .width = 1, .height = 1, .size = 2 },
+        },
+        .num_subframes = 1,
+        .cookie = 6,
+};
+
+static const struct mxr_format mxr_fb_fmt_argb8888 = {
+        .name = "ARGB8888",
+        .fourcc = V4L2_PIX_FMT_BGR32,
+        .colorspace = V4L2_COLORSPACE_SRGB,
+        .num_planes = 1,
+        .plane = {
+                { .width = 1, .height = 1, .size = 4 },
+        },
+        .num_subframes = 1,
+        .cookie = 7,
+};
+
+static const struct mxr_format *mxr_graph_format[] = {
+        &mxr_fb_fmt_rgb565,
+        &mxr_fb_fmt_argb1555,
+        &mxr_fb_fmt_argb4444,
+        &mxr_fb_fmt_argb8888,
+};
+
+/* AUXILIARY CALLBACKS */
+
+static void mxr_graph_layer_release(struct mxr_layer *layer)
+{
+        mxr_base_layer_unregister(layer);
+        mxr_base_layer_release(layer);
+}
+
+static void mxr_graph_buffer_set(struct mxr_layer *layer,
+        struct mxr_buffer *buf)
+{
+        dma_addr_t addr = 0;
+
+        if (buf)
+                addr = vb2_dma_contig_plane_paddr(&buf->vb, 0);
+        mxr_reg_graph_buffer(layer->mdev, layer->idx, addr);
+}
+
+static void mxr_graph_stream_set(struct mxr_layer *layer, int en)
+{
+        mxr_reg_graph_layer_stream(layer->mdev, layer->idx, en);
+}
+
+static void mxr_graph_format_set(struct mxr_layer *layer)
+{
+        mxr_reg_graph_format(layer->mdev, layer->idx,
+                layer->fmt, &layer->geo);
+}
+
+static void mxr_graph_fix_geometry(struct mxr_layer *layer)
+{
+        struct mxr_geometry *geo = &layer->geo;
+
+        /* limit to boundary size */
+        geo->src.full_width = clamp_val(geo->src.full_width, 1, 32767);
+        geo->src.full_height = clamp_val(geo->src.full_height, 1, 2047);
+        geo->src.width = clamp_val(geo->src.width, 1, geo->src.full_width);
+        geo->src.width = min(geo->src.width, 2047U);
+        /* not possible to crop of Y axis */
+        geo->src.y_offset = min(geo->src.y_offset, geo->src.full_height - 1);
+        geo->src.height = geo->src.full_height - geo->src.y_offset;
+        /* limitting offset */
+        geo->src.x_offset = min(geo->src.x_offset,
+                geo->src.full_width - geo->src.width);
+
+        /* setting position in output */
+        geo->dst.width = min(geo->dst.width, geo->dst.full_width);
+        geo->dst.height = min(geo->dst.height, geo->dst.full_height);
+
+        /* Mixer supports only 1x and 2x scaling */
+        if (geo->dst.width >= 2 * geo->src.width) {
+                geo->x_ratio = 1;
+                geo->dst.width = 2 * geo->src.width;
+        } else {
+                geo->x_ratio = 0;
+                geo->dst.width = geo->src.width;
+        }
+
+        if (geo->dst.height >= 2 * geo->src.height) {
+                geo->y_ratio = 1;
+                geo->dst.height = 2 * geo->src.height;
+        } else {
+                geo->y_ratio = 0;
+                geo->dst.height = geo->src.height;
+        }
+
+        geo->dst.x_offset = min(geo->dst.x_offset,
+                geo->dst.full_width - geo->dst.width);
+        geo->dst.y_offset = min(geo->dst.y_offset,
+                geo->dst.full_height - geo->dst.height);
+}
+
+/* PUBLIC API */
+
+struct mxr_layer *mxr_graph_layer_create(struct mxr_device *mdev, int idx)
+{
+        struct mxr_layer *layer;
+        int ret;
+        struct mxr_layer_ops ops = {
+                .release = mxr_graph_layer_release,
+                .buffer_set = mxr_graph_buffer_set,
+                .stream_set = mxr_graph_stream_set,
+                .format_set = mxr_graph_format_set,
+                .fix_geometry = mxr_graph_fix_geometry,
+        };
+        char name[32];
+
+        sprintf(name, "graph%d", idx);
+
+        layer = mxr_base_layer_create(mdev, idx, name, &ops);
+        if (layer == NULL) {
+                mxr_err(mdev, "failed to initialize layer(%d) base\n", idx);
+                goto fail;
+        }
+
+        layer->fmt_array = mxr_graph_format;
+        layer->fmt_array_size = ARRAY_SIZE(mxr_graph_format);
+
+        ret = mxr_base_layer_register(layer);
+        if (ret)
+                goto fail_layer;
+
+        return layer;
+
+fail_layer:
+        mxr_base_layer_release(layer);
+
+fail:
+        return NULL;
+}
+
diff --git a/drivers/media/video/s5p-tv/mixer_reg.c b/drivers/media/video/s5p-tv/mixer_reg.c
new file mode 100644
index 00000000000..38dac672aa1
--- /dev/null
+++ b/drivers/media/video/s5p-tv/mixer_reg.c
@@ -0,0 +1,541 @@
+/*
+ * Samsung TV Mixer driver
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *
+ * Tomasz Stanislawski, <t.stanislaws@samsung.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 Foundiation. either version 2 of the License,
+ * or (at your option) any later version
+ */
+
+#include "mixer.h"
+#include "regs-mixer.h"
+#include "regs-vp.h"
+
+#include <linux/delay.h>
+
+/* Register access subroutines */
+
+static inline u32 vp_read(struct mxr_device *mdev, u32 reg_id)
+{
+        return readl(mdev->res.vp_regs + reg_id);
+}
+
+static inline void vp_write(struct mxr_device *mdev, u32 reg_id, u32 val)
+{
+        writel(val, mdev->res.vp_regs + reg_id);
+}
+
+static inline void vp_write_mask(struct mxr_device *mdev, u32 reg_id,
+        u32 val, u32 mask)
+{
+        u32 old = vp_read(mdev, reg_id);
+
+        val = (val & mask) | (old & ~mask);
+        writel(val, mdev->res.vp_regs + reg_id);
+}
+
+static inline u32 mxr_read(struct mxr_device *mdev, u32 reg_id)
+{
+        return readl(mdev->res.mxr_regs + reg_id);
+}
+
+static inline void mxr_write(struct mxr_device *mdev, u32 reg_id, u32 val)
+{
+        writel(val, mdev->res.mxr_regs + reg_id);
+}
+
+static inline void mxr_write_mask(struct mxr_device *mdev, u32 reg_id,
+        u32 val, u32 mask)
+{
+        u32 old = mxr_read(mdev, reg_id);
+
+        val = (val & mask) | (old & ~mask);
+        writel(val, mdev->res.mxr_regs + reg_id);
+}
+
+void mxr_vsync_set_update(struct mxr_device *mdev, int en)
+{
+        /* block update on vsync */
+        mxr_write_mask(mdev, MXR_STATUS, en ? MXR_STATUS_SYNC_ENABLE : 0,
+                MXR_STATUS_SYNC_ENABLE);
+        vp_write(mdev, VP_SHADOW_UPDATE, en ? VP_SHADOW_UPDATE_ENABLE : 0);
+}
+
+static void __mxr_reg_vp_reset(struct mxr_device *mdev)
+{
+        int tries = 100;
+
+        vp_write(mdev, VP_SRESET, VP_SRESET_PROCESSING);
+        for (tries = 100; tries; --tries) {
+                /* waiting until VP_SRESET_PROCESSING is 0 */
+                if (~vp_read(mdev, VP_SRESET) & VP_SRESET_PROCESSING)
+                        break;
+                mdelay(10);
+        }
+        WARN(tries == 0, "failed to reset Video Processor\n");
+}
+
+static void mxr_reg_vp_default_filter(struct mxr_device *mdev);
+
+void mxr_reg_reset(struct mxr_device *mdev)
+{
+        unsigned long flags;
+        u32 val; /* value stored to register */
+
+        spin_lock_irqsave(&mdev->reg_slock, flags);
+        mxr_vsync_set_update(mdev, MXR_DISABLE);
+
+        /* set output in RGB888 mode */
+        mxr_write(mdev, MXR_CFG, MXR_CFG_OUT_YUV444);
+
+        /* 16 beat burst in DMA */
+        mxr_write_mask(mdev, MXR_STATUS, MXR_STATUS_16_BURST,
+                MXR_STATUS_BURST_MASK);
+
+        /* setting default layer priority: layer1 > video > layer0
+         * because typical usage scenario would be
+         * layer0 - framebuffer
+         * video - video overlay
+         * layer1 - OSD
+         */
+        val  = MXR_LAYER_CFG_GRP0_VAL(1);
+        val |= MXR_LAYER_CFG_VP_VAL(2);
+        val |= MXR_LAYER_CFG_GRP1_VAL(3);
+        mxr_write(mdev, MXR_LAYER_CFG, val);
+
+        /* use dark gray background color */
+        mxr_write(mdev, MXR_BG_COLOR0, 0x808080);
+        mxr_write(mdev, MXR_BG_COLOR1, 0x808080);
+        mxr_write(mdev, MXR_BG_COLOR2, 0x808080);
+
+        /* setting graphical layers */
+
+        val  = MXR_GRP_CFG_COLOR_KEY_DISABLE; /* no blank key */
+        val |= MXR_GRP_CFG_BLEND_PRE_MUL; /* premul mode */
+        val |= MXR_GRP_CFG_ALPHA_VAL(0xff); /* non-transparent alpha */
+
+        /* the same configuration for both layers */
+        mxr_write(mdev, MXR_GRAPHIC_CFG(0), val);
+        mxr_write(mdev, MXR_GRAPHIC_CFG(1), val);
+
+        /* configuration of Video Processor Registers */
+        __mxr_reg_vp_reset(mdev);
+        mxr_reg_vp_default_filter(mdev);
+
+        /* enable all interrupts */
+        mxr_write_mask(mdev, MXR_INT_EN, ~0, MXR_INT_EN_ALL);
+
+        mxr_vsync_set_update(mdev, MXR_ENABLE);
+        spin_unlock_irqrestore(&mdev->reg_slock, flags);
+}
+
+void mxr_reg_graph_format(struct mxr_device *mdev, int idx,
+        const struct mxr_format *fmt, const struct mxr_geometry *geo)
+{
+        u32 val;
+        unsigned long flags;
+
+        spin_lock_irqsave(&mdev->reg_slock, flags);
+        mxr_vsync_set_update(mdev, MXR_DISABLE);
+
+        /* setup format */
+        mxr_write_mask(mdev, MXR_GRAPHIC_CFG(idx),
+                MXR_GRP_CFG_FORMAT_VAL(fmt->cookie), MXR_GRP_CFG_FORMAT_MASK);
+
+        /* setup geometry */
+        mxr_write(mdev, MXR_GRAPHIC_SPAN(idx), geo->src.full_width);
+        val  = MXR_GRP_WH_WIDTH(geo->src.width);
+        val |= MXR_GRP_WH_HEIGHT(geo->src.height);
+        val |= MXR_GRP_WH_H_SCALE(geo->x_ratio);
+        val |= MXR_GRP_WH_V_SCALE(geo->y_ratio);
+        mxr_write(mdev, MXR_GRAPHIC_WH(idx), val);
+
+        /* setup offsets in source image */
+        val  = MXR_GRP_SXY_SX(geo->src.x_offset);
+        val |= MXR_GRP_SXY_SY(geo->src.y_offset);
+        mxr_write(mdev, MXR_GRAPHIC_SXY(idx), val);
+
+        /* setup offsets in display image */
+        val  = MXR_GRP_DXY_DX(geo->dst.x_offset);
+        val |= MXR_GRP_DXY_DY(geo->dst.y_offset);
+        mxr_write(mdev, MXR_GRAPHIC_DXY(idx), val);
+
+        mxr_vsync_set_update(mdev, MXR_ENABLE);
+        spin_unlock_irqrestore(&mdev->reg_slock, flags);
+}
+
+void mxr_reg_vp_format(struct mxr_device *mdev,
+        const struct mxr_format *fmt, const struct mxr_geometry *geo)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&mdev->reg_slock, flags);
+        mxr_vsync_set_update(mdev, MXR_DISABLE);
+
+        vp_write_mask(mdev, VP_MODE, fmt->cookie, VP_MODE_FMT_MASK);
+
+        /* setting size of input image */
+        vp_write(mdev, VP_IMG_SIZE_Y, VP_IMG_HSIZE(geo->src.full_width) |
+                VP_IMG_VSIZE(geo->src.full_height));
+        /* chroma height has to reduced by 2 to avoid chroma distorions */
+        vp_write(mdev, VP_IMG_SIZE_C, VP_IMG_HSIZE(geo->src.full_width) |
+                VP_IMG_VSIZE(geo->src.full_height / 2));
+
+        vp_write(mdev, VP_SRC_WIDTH, geo->src.width);
+        vp_write(mdev, VP_SRC_HEIGHT, geo->src.height);
+        vp_write(mdev, VP_SRC_H_POSITION,
+                VP_SRC_H_POSITION_VAL(geo->src.x_offset));
+        vp_write(mdev, VP_SRC_V_POSITION, geo->src.y_offset);
+
+        vp_write(mdev, VP_DST_WIDTH, geo->dst.width);
+        vp_write(mdev, VP_DST_H_POSITION, geo->dst.x_offset);
+        if (geo->dst.field == V4L2_FIELD_INTERLACED) {
+                vp_write(mdev, VP_DST_HEIGHT, geo->dst.height / 2);
+                vp_write(mdev, VP_DST_V_POSITION, geo->dst.y_offset / 2);
+        } else {
+                vp_write(mdev, VP_DST_HEIGHT, geo->dst.height);
+                vp_write(mdev, VP_DST_V_POSITION, geo->dst.y_offset);
+        }
+
+        vp_write(mdev, VP_H_RATIO, geo->x_ratio);
+        vp_write(mdev, VP_V_RATIO, geo->y_ratio);
+
+        vp_write(mdev, VP_ENDIAN_MODE, VP_ENDIAN_MODE_LITTLE);
+
+        mxr_vsync_set_update(mdev, MXR_ENABLE);
+        spin_unlock_irqrestore(&mdev->reg_slock, flags);
+
+}
+
+void mxr_reg_graph_buffer(struct mxr_device *mdev, int idx, dma_addr_t addr)
+{
+        u32 val = addr ? ~0 : 0;
+        unsigned long flags;
+
+        spin_lock_irqsave(&mdev->reg_slock, flags);
+        mxr_vsync_set_update(mdev, MXR_DISABLE);
+
+        if (idx == 0)
+                mxr_write_mask(mdev, MXR_CFG, val, MXR_CFG_GRP0_ENABLE);
+        else
+                mxr_write_mask(mdev, MXR_CFG, val, MXR_CFG_GRP1_ENABLE);
+        mxr_write(mdev, MXR_GRAPHIC_BASE(idx), addr);
+
+        mxr_vsync_set_update(mdev, MXR_ENABLE);
+        spin_unlock_irqrestore(&mdev->reg_slock, flags);
+}
+
+void mxr_reg_vp_buffer(struct mxr_device *mdev,
+        dma_addr_t luma_addr[2], dma_addr_t chroma_addr[2])
+{
+        u32 val = luma_addr[0] ? ~0 : 0;
+        unsigned long flags;
+
+        spin_lock_irqsave(&mdev->reg_slock, flags);
+        mxr_vsync_set_update(mdev, MXR_DISABLE);
+
+        mxr_write_mask(mdev, MXR_CFG, val, MXR_CFG_VP_ENABLE);
+        vp_write_mask(mdev, VP_ENABLE, val, VP_ENABLE_ON);
+        /* TODO: fix tiled mode */
+        vp_write(mdev, VP_TOP_Y_PTR, luma_addr[0]);
+        vp_write(mdev, VP_TOP_C_PTR, chroma_addr[0]);
+        vp_write(mdev, VP_BOT_Y_PTR, luma_addr[1]);
+        vp_write(mdev, VP_BOT_C_PTR, chroma_addr[1]);
+
+        mxr_vsync_set_update(mdev, MXR_ENABLE);
+        spin_unlock_irqrestore(&mdev->reg_slock, flags);
+}
+
+static void mxr_irq_layer_handle(struct mxr_layer *layer)
+{
+        struct list_head *head = &layer->enq_list;
+        struct mxr_buffer *done;
+
+        /* skip non-existing layer */
+        if (layer == NULL)
+                return;
+
+        spin_lock(&layer->enq_slock);
+        if (layer->state == MXR_LAYER_IDLE)
+                goto done;
+
+        done = layer->shadow_buf;
+        layer->shadow_buf = layer->update_buf;
+
+        if (list_empty(head)) {
+                if (layer->state != MXR_LAYER_STREAMING)
+                        layer->update_buf = NULL;
+        } else {
+                struct mxr_buffer *next;
+                next = list_first_entry(head, struct mxr_buffer, list);
+                list_del(&next->list);
+                layer->update_buf = next;
+        }
+
+        layer->ops.buffer_set(layer, layer->update_buf);
+
+        if (done && done != layer->shadow_buf)
+                vb2_buffer_done(&done->vb, VB2_BUF_STATE_DONE);
+
+done:
+        spin_unlock(&layer->enq_slock);
+}
+
+irqreturn_t mxr_irq_handler(int irq, void *dev_data)
+{
+        struct mxr_device *mdev = dev_data;
+        u32 i, val;
+
+        spin_lock(&mdev->reg_slock);
+        val = mxr_read(mdev, MXR_INT_STATUS);
+
+        /* wake up process waiting for VSYNC */
+        if (val & MXR_INT_STATUS_VSYNC) {
+                set_bit(MXR_EVENT_VSYNC, &mdev->event_flags);
+                wake_up(&mdev->event_queue);
+        }
+
+        /* clear interrupts */
+        if (~val & MXR_INT_EN_VSYNC) {
+                /* vsync interrupt use different bit for read and clear */
+                val &= ~MXR_INT_EN_VSYNC;
+                val |= MXR_INT_CLEAR_VSYNC;
+        }
+        mxr_write(mdev, MXR_INT_STATUS, val);
+
+        spin_unlock(&mdev->reg_slock);
+        /* leave on non-vsync event */
+        if (~val & MXR_INT_CLEAR_VSYNC)
+                return IRQ_HANDLED;
+        for (i = 0; i < MXR_MAX_LAYERS; ++i)
+                mxr_irq_layer_handle(mdev->layer[i]);
+        return IRQ_HANDLED;
+}
+
+void mxr_reg_s_output(struct mxr_device *mdev, int cookie)
+{
+        u32 val;
+
+        val = cookie == 0 ? MXR_CFG_DST_SDO : MXR_CFG_DST_HDMI;
+        mxr_write_mask(mdev, MXR_CFG, val, MXR_CFG_DST_MASK);
+}
+
+void mxr_reg_streamon(struct mxr_device *mdev)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&mdev->reg_slock, flags);
+        /* single write -> no need to block vsync update */
+
+        /* start MIXER */
+        mxr_write_mask(mdev, MXR_STATUS, ~0, MXR_STATUS_REG_RUN);
+
+        spin_unlock_irqrestore(&mdev->reg_slock, flags);
+}
+
+void mxr_reg_streamoff(struct mxr_device *mdev)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&mdev->reg_slock, flags);
+        /* single write -> no need to block vsync update */
+
+        /* stop MIXER */
+        mxr_write_mask(mdev, MXR_STATUS, 0, MXR_STATUS_REG_RUN);
+
+        spin_unlock_irqrestore(&mdev->reg_slock, flags);
+}
+
+int mxr_reg_wait4vsync(struct mxr_device *mdev)
+{
+        int ret;
+
+        clear_bit(MXR_EVENT_VSYNC, &mdev->event_flags);
+        /* TODO: consider adding interruptible */
+        ret = wait_event_timeout(mdev->event_queue,
+                test_bit(MXR_EVENT_VSYNC, &mdev->event_flags),
+                msecs_to_jiffies(1000));
+        if (ret > 0)
+                return 0;
+        if (ret < 0)
+                return ret;
+        mxr_warn(mdev, "no vsync detected - timeout\n");
+        return -ETIME;
+}
+
+void mxr_reg_set_mbus_fmt(struct mxr_device *mdev,
+        struct v4l2_mbus_framefmt *fmt)
+{
+        u32 val = 0;
+        unsigned long flags;
+
+        spin_lock_irqsave(&mdev->reg_slock, flags);
+        mxr_vsync_set_update(mdev, MXR_DISABLE);
+
+        /* choosing between interlace and progressive mode */
+        if (fmt->field == V4L2_FIELD_INTERLACED)
+                val |= MXR_CFG_SCAN_INTERLACE;
+        else
+                val |= MXR_CFG_SCAN_PROGRASSIVE;
+
+        /* choosing between porper HD and SD mode */
+        if (fmt->height == 480)
+                val |= MXR_CFG_SCAN_NTSC | MXR_CFG_SCAN_SD;
+        else if (fmt->height == 576)
+                val |= MXR_CFG_SCAN_PAL | MXR_CFG_SCAN_SD;
+        else if (fmt->height == 720)
+                val |= MXR_CFG_SCAN_HD_720 | MXR_CFG_SCAN_HD;
+        else if (fmt->height == 1080)
+                val |= MXR_CFG_SCAN_HD_1080 | MXR_CFG_SCAN_HD;
+        else
+                WARN(1, "unrecognized mbus height %u!\n", fmt->height);
+
+        mxr_write_mask(mdev, MXR_CFG, val, MXR_CFG_SCAN_MASK);
+
+        val = (fmt->field == V4L2_FIELD_INTERLACED) ? ~0 : 0;
+        vp_write_mask(mdev, VP_MODE, val,
+                VP_MODE_LINE_SKIP | VP_MODE_FIELD_ID_AUTO_TOGGLING);
+
+        mxr_vsync_set_update(mdev, MXR_ENABLE);
+        spin_unlock_irqrestore(&mdev->reg_slock, flags);
+}
+
+void mxr_reg_graph_layer_stream(struct mxr_device *mdev, int idx, int en)
+{
+        /* no extra actions need to be done */
+}
+
+void mxr_reg_vp_layer_stream(struct mxr_device *mdev, int en)
+{
+        /* no extra actions need to be done */
+}
+
+static const u8 filter_y_horiz_tap8[] = {
+        0,      -1,     -1,     -1,     -1,     -1,     -1,     -1,
+        -1,     -1,     -1,     -1,     -1,     0,      0,      0,
+        0,      2,      4,      5,      6,      6,      6,      6,
+        6,      5,      5,      4,      3,      2,      1,      1,
+        0,      -6,     -12,    -16,    -18,    -20,    -21,    -20,
+        -20,    -18,    -16,    -13,    -10,    -8,     -5,     -2,
+        127,    126,    125,    121,    114,    107,    99,     89,
+        79,     68,     57,     46,     35,     25,     16,     8,
+};
+
+static const u8 filter_y_vert_tap4[] = {
+        0,      -3,     -6,     -8,     -8,     -8,     -8,     -7,
+        -6,     -5,     -4,     -3,     -2,     -1,     -1,     0,
+        127,    126,    124,    118,    111,    102,    92,     81,
+        70,     59,     48,     37,     27,     19,     11,     5,
+        0,      5,      11,     19,     27,     37,     48,     59,
+        70,     81,     92,     102,    111,    118,    124,    126,
+        0,      0,      -1,     -1,     -2,     -3,     -4,     -5,
+        -6,     -7,     -8,     -8,     -8,     -8,     -6,     -3,
+};
+
+static const u8 filter_cr_horiz_tap4[] = {
+        0,      -3,     -6,     -8,     -8,     -8,     -8,     -7,
+        -6,     -5,     -4,     -3,     -2,     -1,     -1,     0,
+        127,    126,    124,    118,    111,    102,    92,     81,
+        70,     59,     48,     37,     27,     19,     11,     5,
+};
+
+static inline void mxr_reg_vp_filter_set(struct mxr_device *mdev,
+        int reg_id, const u8 *data, unsigned int size)
+{
+        /* assure 4-byte align */
+        BUG_ON(size & 3);
+        for (; size; size -= 4, reg_id += 4, data += 4) {
+                u32 val = (data[0] << 24) |  (data[1] << 16) |
+                        (data[2] << 8) | data[3];
+                vp_write(mdev, reg_id, val);
+        }
+}
+
+static void mxr_reg_vp_default_filter(struct mxr_device *mdev)
+{
+        mxr_reg_vp_filter_set(mdev, VP_POLY8_Y0_LL,
+                filter_y_horiz_tap8, sizeof filter_y_horiz_tap8);
+        mxr_reg_vp_filter_set(mdev, VP_POLY4_Y0_LL,
+                filter_y_vert_tap4, sizeof filter_y_vert_tap4);
+        mxr_reg_vp_filter_set(mdev, VP_POLY4_C0_LL,
+                filter_cr_horiz_tap4, sizeof filter_cr_horiz_tap4);
+}
+
+static void mxr_reg_mxr_dump(struct mxr_device *mdev)
+{
+#define DUMPREG(reg_id) \
+do { \
+        mxr_dbg(mdev, #reg_id " = %08x\n", \
+                (u32)readl(mdev->res.mxr_regs + reg_id)); \
+} while (0)
+
+        DUMPREG(MXR_STATUS);
+        DUMPREG(MXR_CFG);
+        DUMPREG(MXR_INT_EN);
+        DUMPREG(MXR_INT_STATUS);
+
+        DUMPREG(MXR_LAYER_CFG);
+        DUMPREG(MXR_VIDEO_CFG);
+
+        DUMPREG(MXR_GRAPHIC0_CFG);
+        DUMPREG(MXR_GRAPHIC0_BASE);
+        DUMPREG(MXR_GRAPHIC0_SPAN);
+        DUMPREG(MXR_GRAPHIC0_WH);
+        DUMPREG(MXR_GRAPHIC0_SXY);
+        DUMPREG(MXR_GRAPHIC0_DXY);
+
+        DUMPREG(MXR_GRAPHIC1_CFG);
+        DUMPREG(MXR_GRAPHIC1_BASE);
+        DUMPREG(MXR_GRAPHIC1_SPAN);
+        DUMPREG(MXR_GRAPHIC1_WH);
+        DUMPREG(MXR_GRAPHIC1_SXY);
+        DUMPREG(MXR_GRAPHIC1_DXY);
+#undef DUMPREG
+}
+
+static void mxr_reg_vp_dump(struct mxr_device *mdev)
+{
+#define DUMPREG(reg_id) \
+do { \
+        mxr_dbg(mdev, #reg_id " = %08x\n", \
+                (u32) readl(mdev->res.vp_regs + reg_id)); \
+} while (0)
+
+
+        DUMPREG(VP_ENABLE);
+        DUMPREG(VP_SRESET);
+        DUMPREG(VP_SHADOW_UPDATE);
+        DUMPREG(VP_FIELD_ID);
+        DUMPREG(VP_MODE);
+        DUMPREG(VP_IMG_SIZE_Y);
+        DUMPREG(VP_IMG_SIZE_C);
+        DUMPREG(VP_PER_RATE_CTRL);
+        DUMPREG(VP_TOP_Y_PTR);
+        DUMPREG(VP_BOT_Y_PTR);
+        DUMPREG(VP_TOP_C_PTR);
+        DUMPREG(VP_BOT_C_PTR);
+        DUMPREG(VP_ENDIAN_MODE);
+        DUMPREG(VP_SRC_H_POSITION);
+        DUMPREG(VP_SRC_V_POSITION);
+        DUMPREG(VP_SRC_WIDTH);
+        DUMPREG(VP_SRC_HEIGHT);
+        DUMPREG(VP_DST_H_POSITION);
+        DUMPREG(VP_DST_V_POSITION);
+        DUMPREG(VP_DST_WIDTH);
+        DUMPREG(VP_DST_HEIGHT);
+        DUMPREG(VP_H_RATIO);
+        DUMPREG(VP_V_RATIO);
+
+#undef DUMPREG
+}
+
+void mxr_reg_dump(struct mxr_device *mdev)
+{
+        mxr_reg_mxr_dump(mdev);
+        mxr_reg_vp_dump(mdev);
+}
+
diff --git a/drivers/media/video/s5p-tv/mixer_video.c b/drivers/media/video/s5p-tv/mixer_video.c
new file mode 100644
index 00000000000..43ac22f35bc
--- /dev/null
+++ b/drivers/media/video/s5p-tv/mixer_video.c
@@ -0,0 +1,1006 @@
+/*
+ * Samsung TV Mixer driver
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *
+ * Tomasz Stanislawski, <t.stanislaws@samsung.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 "mixer.h"
+
+#include <media/v4l2-ioctl.h>
+#include <linux/videodev2.h>
+#include <linux/mm.h>
+#include <linux/version.h>
+#include <linux/timer.h>
+#include <media/videobuf2-dma-contig.h>
+
+static int find_reg_callback(struct device *dev, void *p)
+{
+        struct v4l2_subdev **sd = p;
+
+        *sd = dev_get_drvdata(dev);
+        /* non-zero value stops iteration */
+        return 1;
+}
+
+static struct v4l2_subdev *find_and_register_subdev(
+        struct mxr_device *mdev, char *module_name)
+{
+        struct device_driver *drv;
+        struct v4l2_subdev *sd = NULL;
+        int ret;
+
+        /* TODO: add waiting until probe is finished */
+        drv = driver_find(module_name, &platform_bus_type);
+        if (!drv) {
+                mxr_warn(mdev, "module %s is missing\n", module_name);
+                return NULL;
+        }
+        /* driver refcnt is increased, it is safe to iterate over devices */
+        ret = driver_for_each_device(drv, NULL, &sd, find_reg_callback);
+        /* ret == 0 means that find_reg_callback was never executed */
+        if (sd == NULL) {
+                mxr_warn(mdev, "module %s provides no subdev!\n", module_name);
+                goto done;
+        }
+        /* v4l2_device_register_subdev detects if sd is NULL */
+        ret = v4l2_device_register_subdev(&mdev->v4l2_dev, sd);
+        if (ret) {
+                mxr_warn(mdev, "failed to register subdev %s\n", sd->name);
+                sd = NULL;
+        }
+
+done:
+        put_driver(drv);
+        return sd;
+}
+
+int __devinit mxr_acquire_video(struct mxr_device *mdev,
+        struct mxr_output_conf *output_conf, int output_count)
+{
+        struct device *dev = mdev->dev;
+        struct v4l2_device *v4l2_dev = &mdev->v4l2_dev;
+        int i;
+        int ret = 0;
+        struct v4l2_subdev *sd;
+
+        strlcpy(v4l2_dev->name, dev_name(mdev->dev), sizeof(v4l2_dev->name));
+        /* prepare context for V4L2 device */
+        ret = v4l2_device_register(dev, v4l2_dev);
+        if (ret) {
+                mxr_err(mdev, "could not register v4l2 device.\n");
+                goto fail;
+        }
+
+        mdev->alloc_ctx = vb2_dma_contig_init_ctx(mdev->dev);
+        if (IS_ERR_OR_NULL(mdev->alloc_ctx)) {
+                mxr_err(mdev, "could not acquire vb2 allocator\n");
+                goto fail_v4l2_dev;
+        }
+
+        /* registering outputs */
+        mdev->output_cnt = 0;
+        for (i = 0; i < output_count; ++i) {
+                struct mxr_output_conf *conf = &output_conf[i];
+                struct mxr_output *out;
+
+                sd = find_and_register_subdev(mdev, conf->module_name);
+                /* trying to register next output */
+                if (sd == NULL)
+                        continue;
+                out = kzalloc(sizeof *out, GFP_KERNEL);
+                if (out == NULL) {
+                        mxr_err(mdev, "no memory for '%s'\n",
+                                conf->output_name);
+                        ret = -ENOMEM;
+                        /* registered subdevs are removed in fail_v4l2_dev */
+                        goto fail_output;
+                }
+                strlcpy(out->name, conf->output_name, sizeof(out->name));
+                out->sd = sd;
+                out->cookie = conf->cookie;
+                mdev->output[mdev->output_cnt++] = out;
+                mxr_info(mdev, "added output '%s' from module '%s'\n",
+                        conf->output_name, conf->module_name);
+                /* checking if maximal number of outputs is reached */
+                if (mdev->output_cnt >= MXR_MAX_OUTPUTS)
+                        break;
+        }
+
+        if (mdev->output_cnt == 0) {
+                mxr_err(mdev, "failed to register any output\n");
+                ret = -ENODEV;
+                /* skipping fail_output because there is nothing to free */
+                goto fail_vb2_allocator;
+        }
+
+        return 0;
+
+fail_output:
+        /* kfree is NULL-safe */
+        for (i = 0; i < mdev->output_cnt; ++i)
+                kfree(mdev->output[i]);
+        memset(mdev->output, 0, sizeof mdev->output);
+
+fail_vb2_allocator:
+        /* freeing allocator context */
+        vb2_dma_contig_cleanup_ctx(mdev->alloc_ctx);
+
+fail_v4l2_dev:
+        /* NOTE: automatically unregister all subdevs */
+        v4l2_device_unregister(v4l2_dev);
+
+fail:
+        return ret;
+}
+
+void __devexit mxr_release_video(struct mxr_device *mdev)
+{
+        int i;
+
+        /* kfree is NULL-safe */
+        for (i = 0; i < mdev->output_cnt; ++i)
+                kfree(mdev->output[i]);
+
+        vb2_dma_contig_cleanup_ctx(mdev->alloc_ctx);
+        v4l2_device_unregister(&mdev->v4l2_dev);
+}
+
+static int mxr_querycap(struct file *file, void *priv,
+        struct v4l2_capability *cap)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+
+        mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+
+        strlcpy(cap->driver, MXR_DRIVER_NAME, sizeof cap->driver);
+        strlcpy(cap->card, layer->vfd.name, sizeof cap->card);
+        sprintf(cap->bus_info, "%d", layer->idx);
+        cap->version = KERNEL_VERSION(0, 1, 0);
+        cap->capabilities = V4L2_CAP_STREAMING |
+                V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
+
+        return 0;
+}
+
+/* Geometry handling */
+static void mxr_layer_geo_fix(struct mxr_layer *layer)
+{
+        struct mxr_device *mdev = layer->mdev;
+        struct v4l2_mbus_framefmt mbus_fmt;
+
+        /* TODO: add some dirty flag to avoid unnecessary adjustments */
+        mxr_get_mbus_fmt(mdev, &mbus_fmt);
+        layer->geo.dst.full_width = mbus_fmt.width;
+        layer->geo.dst.full_height = mbus_fmt.height;
+        layer->geo.dst.field = mbus_fmt.field;
+        layer->ops.fix_geometry(layer);
+}
+
+static void mxr_layer_default_geo(struct mxr_layer *layer)
+{
+        struct mxr_device *mdev = layer->mdev;
+        struct v4l2_mbus_framefmt mbus_fmt;
+
+        memset(&layer->geo, 0, sizeof layer->geo);
+
+        mxr_get_mbus_fmt(mdev, &mbus_fmt);
+
+        layer->geo.dst.full_width = mbus_fmt.width;
+        layer->geo.dst.full_height = mbus_fmt.height;
+        layer->geo.dst.width = layer->geo.dst.full_width;
+        layer->geo.dst.height = layer->geo.dst.full_height;
+        layer->geo.dst.field = mbus_fmt.field;
+
+        layer->geo.src.full_width = mbus_fmt.width;
+        layer->geo.src.full_height = mbus_fmt.height;
+        layer->geo.src.width = layer->geo.src.full_width;
+        layer->geo.src.height = layer->geo.src.full_height;
+
+        layer->ops.fix_geometry(layer);
+}
+
+static void mxr_geometry_dump(struct mxr_device *mdev, struct mxr_geometry *geo)
+{
+        mxr_dbg(mdev, "src.full_size = (%u, %u)\n",
+                geo->src.full_width, geo->src.full_height);
+        mxr_dbg(mdev, "src.size = (%u, %u)\n",
+                geo->src.width, geo->src.height);
+        mxr_dbg(mdev, "src.offset = (%u, %u)\n",
+                geo->src.x_offset, geo->src.y_offset);
+        mxr_dbg(mdev, "dst.full_size = (%u, %u)\n",
+                geo->dst.full_width, geo->dst.full_height);
+        mxr_dbg(mdev, "dst.size = (%u, %u)\n",
+                geo->dst.width, geo->dst.height);
+        mxr_dbg(mdev, "dst.offset = (%u, %u)\n",
+                geo->dst.x_offset, geo->dst.y_offset);
+        mxr_dbg(mdev, "ratio = (%u, %u)\n",
+                geo->x_ratio, geo->y_ratio);
+}
+
+
+static const struct mxr_format *find_format_by_fourcc(
+        struct mxr_layer *layer, unsigned long fourcc);
+static const struct mxr_format *find_format_by_index(
+        struct mxr_layer *layer, unsigned long index);
+
+static int mxr_enum_fmt(struct file *file, void  *priv,
+        struct v4l2_fmtdesc *f)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+        struct mxr_device *mdev = layer->mdev;
+        const struct mxr_format *fmt;
+
+        mxr_dbg(mdev, "%s\n", __func__);
+        fmt = find_format_by_index(layer, f->index);
+        if (fmt == NULL)
+                return -EINVAL;
+
+        strlcpy(f->description, fmt->name, sizeof(f->description));
+        f->pixelformat = fmt->fourcc;
+
+        return 0;
+}
+
+static int mxr_s_fmt(struct file *file, void *priv,
+        struct v4l2_format *f)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+        const struct mxr_format *fmt;
+        struct v4l2_pix_format_mplane *pix;
+        struct mxr_device *mdev = layer->mdev;
+        struct mxr_geometry *geo = &layer->geo;
+
+        mxr_dbg(mdev, "%s:%d\n", __func__, __LINE__);
+
+        pix = &f->fmt.pix_mp;
+        fmt = find_format_by_fourcc(layer, pix->pixelformat);
+        if (fmt == NULL) {
+                mxr_warn(mdev, "not recognized fourcc: %08x\n",
+                        pix->pixelformat);
+                return -EINVAL;
+        }
+        layer->fmt = fmt;
+        geo->src.full_width = pix->width;
+        geo->src.width = pix->width;
+        geo->src.full_height = pix->height;
+        geo->src.height = pix->height;
+        /* assure consistency of geometry */
+        mxr_layer_geo_fix(layer);
+        mxr_dbg(mdev, "width=%u height=%u span=%u\n",
+                geo->src.width, geo->src.height, geo->src.full_width);
+
+        return 0;
+}
+
+static unsigned int divup(unsigned int divident, unsigned int divisor)
+{
+        return (divident + divisor - 1) / divisor;
+}
+
+unsigned long mxr_get_plane_size(const struct mxr_block *blk,
+        unsigned int width, unsigned int height)
+{
+        unsigned int bl_width = divup(width, blk->width);
+        unsigned int bl_height = divup(height, blk->height);
+
+        return bl_width * bl_height * blk->size;
+}
+
+static void mxr_mplane_fill(struct v4l2_plane_pix_format *planes,
+        const struct mxr_format *fmt, u32 width, u32 height)
+{
+        int i;
+
+        memset(planes, 0, sizeof(*planes) * fmt->num_subframes);
+        for (i = 0; i < fmt->num_planes; ++i) {
+                struct v4l2_plane_pix_format *plane = planes
+                        + fmt->plane2subframe[i];
+                const struct mxr_block *blk = &fmt->plane[i];
+                u32 bl_width = divup(width, blk->width);
+                u32 bl_height = divup(height, blk->height);
+                u32 sizeimage = bl_width * bl_height * blk->size;
+                u16 bytesperline = bl_width * blk->size / blk->height;
+
+                plane->sizeimage += sizeimage;
+                plane->bytesperline = max(plane->bytesperline, bytesperline);
+        }
+}
+
+static int mxr_g_fmt(struct file *file, void *priv,
+                             struct v4l2_format *f)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+        struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
+
+        mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+
+        pix->width = layer->geo.src.full_width;
+        pix->height = layer->geo.src.full_height;
+        pix->field = V4L2_FIELD_NONE;
+        pix->pixelformat = layer->fmt->fourcc;
+        pix->colorspace = layer->fmt->colorspace;
+        mxr_mplane_fill(pix->plane_fmt, layer->fmt, pix->width, pix->height);
+
+        return 0;
+}
+
+static inline struct mxr_crop *choose_crop_by_type(struct mxr_geometry *geo,
+        enum v4l2_buf_type type)
+{
+        switch (type) {
+        case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+        case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+                return &geo->dst;
+        case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+                return &geo->src;
+        default:
+                return NULL;
+        }
+}
+
+static int mxr_g_crop(struct file *file, void *fh, struct v4l2_crop *a)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+        struct mxr_crop *crop;
+
+        mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+        crop = choose_crop_by_type(&layer->geo, a->type);
+        if (crop == NULL)
+                return -EINVAL;
+        mxr_layer_geo_fix(layer);
+        a->c.left = crop->x_offset;
+        a->c.top = crop->y_offset;
+        a->c.width = crop->width;
+        a->c.height = crop->height;
+        return 0;
+}
+
+static int mxr_s_crop(struct file *file, void *fh, struct v4l2_crop *a)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+        struct mxr_crop *crop;
+
+        mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+        crop = choose_crop_by_type(&layer->geo, a->type);
+        if (crop == NULL)
+                return -EINVAL;
+        crop->x_offset = a->c.left;
+        crop->y_offset = a->c.top;
+        crop->width = a->c.width;
+        crop->height = a->c.height;
+        mxr_layer_geo_fix(layer);
+        return 0;
+}
+
+static int mxr_cropcap(struct file *file, void *fh, struct v4l2_cropcap *a)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+        struct mxr_crop *crop;
+
+        mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+        crop = choose_crop_by_type(&layer->geo, a->type);
+        if (crop == NULL)
+                return -EINVAL;
+        mxr_layer_geo_fix(layer);
+        a->bounds.left = 0;
+        a->bounds.top = 0;
+        a->bounds.width = crop->full_width;
+        a->bounds.top = crop->full_height;
+        a->defrect = a->bounds;
+        /* setting pixel aspect to 1/1 */
+        a->pixelaspect.numerator = 1;
+        a->pixelaspect.denominator = 1;
+        return 0;
+}
+
+static int mxr_enum_dv_presets(struct file *file, void *fh,
+        struct v4l2_dv_enum_preset *preset)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+        struct mxr_device *mdev = layer->mdev;
+        int ret;
+
+        /* lock protects from changing sd_out */
+        mutex_lock(&mdev->mutex);
+        ret = v4l2_subdev_call(to_outsd(mdev), video, enum_dv_presets, preset);
+        mutex_unlock(&mdev->mutex);
+
+        return ret ? -EINVAL : 0;
+}
+
+static int mxr_s_dv_preset(struct file *file, void *fh,
+        struct v4l2_dv_preset *preset)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+        struct mxr_device *mdev = layer->mdev;
+        int ret;
+
+        /* lock protects from changing sd_out */
+        mutex_lock(&mdev->mutex);
+
+        /* preset change cannot be done while there is an entity
+         * dependant on output configuration
+         */
+        if (mdev->n_output > 0) {
+                mutex_unlock(&mdev->mutex);
+                return -EBUSY;
+        }
+
+        ret = v4l2_subdev_call(to_outsd(mdev), video, s_dv_preset, preset);
+
+        mutex_unlock(&mdev->mutex);
+
+        /* any failure should return EINVAL according to V4L2 doc */
+        return ret ? -EINVAL : 0;
+}
+
+static int mxr_g_dv_preset(struct file *file, void *fh,
+        struct v4l2_dv_preset *preset)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+        struct mxr_device *mdev = layer->mdev;
+        int ret;
+
+        /* lock protects from changing sd_out */
+        mutex_lock(&mdev->mutex);
+        ret = v4l2_subdev_call(to_outsd(mdev), video, g_dv_preset, preset);
+        mutex_unlock(&mdev->mutex);
+
+        return ret ? -EINVAL : 0;
+}
+
+static int mxr_s_std(struct file *file, void *fh, v4l2_std_id *norm)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+        struct mxr_device *mdev = layer->mdev;
+        int ret;
+
+        /* lock protects from changing sd_out */
+        mutex_lock(&mdev->mutex);
+
+        /* standard change cannot be done while there is an entity
+         * dependant on output configuration
+         */
+        if (mdev->n_output > 0) {
+                mutex_unlock(&mdev->mutex);
+                return -EBUSY;
+        }
+
+        ret = v4l2_subdev_call(to_outsd(mdev), video, s_std_output, *norm);
+
+        mutex_unlock(&mdev->mutex);
+
+        return ret ? -EINVAL : 0;
+}
+
+static int mxr_g_std(struct file *file, void *fh, v4l2_std_id *norm)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+        struct mxr_device *mdev = layer->mdev;
+        int ret;
+
+        /* lock protects from changing sd_out */
+        mutex_lock(&mdev->mutex);
+        ret = v4l2_subdev_call(to_outsd(mdev), video, g_std_output, norm);
+        mutex_unlock(&mdev->mutex);
+
+        return ret ? -EINVAL : 0;
+}
+
+static int mxr_enum_output(struct file *file, void *fh, struct v4l2_output *a)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+        struct mxr_device *mdev = layer->mdev;
+        struct mxr_output *out;
+        struct v4l2_subdev *sd;
+
+        if (a->index >= mdev->output_cnt)
+                return -EINVAL;
+        out = mdev->output[a->index];
+        BUG_ON(out == NULL);
+        sd = out->sd;
+        strlcpy(a->name, out->name, sizeof(a->name));
+
+        /* try to obtain supported tv norms */
+        v4l2_subdev_call(sd, video, g_tvnorms_output, &a->std);
+        a->capabilities = 0;
+        if (sd->ops->video && sd->ops->video->s_dv_preset)
+                a->capabilities |= V4L2_OUT_CAP_PRESETS;
+        if (sd->ops->video && sd->ops->video->s_std_output)
+                a->capabilities |= V4L2_OUT_CAP_STD;
+        a->type = V4L2_OUTPUT_TYPE_ANALOG;
+
+        return 0;
+}
+
+static int mxr_s_output(struct file *file, void *fh, unsigned int i)
+{
+        struct video_device *vfd = video_devdata(file);
+        struct mxr_layer *layer = video_drvdata(file);
+        struct mxr_device *mdev = layer->mdev;
+        int ret = 0;
+
+        if (i >= mdev->output_cnt || mdev->output[i] == NULL)
+                return -EINVAL;
+
+        mutex_lock(&mdev->mutex);
+        if (mdev->n_output > 0) {
+                ret = -EBUSY;
+                goto done;
+        }
+        mdev->current_output = i;
+        vfd->tvnorms = 0;
+        v4l2_subdev_call(to_outsd(mdev), video, g_tvnorms_output,
+                &vfd->tvnorms);
+        mxr_dbg(mdev, "tvnorms = %08llx\n", vfd->tvnorms);
+
+done:
+        mutex_unlock(&mdev->mutex);
+        return ret;
+}
+
+static int mxr_g_output(struct file *file, void *fh, unsigned int *p)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+        struct mxr_device *mdev = layer->mdev;
+
+        mutex_lock(&mdev->mutex);
+        *p = mdev->current_output;
+        mutex_unlock(&mdev->mutex);
+
+        return 0;
+}
+
+static int mxr_reqbufs(struct file *file, void *priv,
+                          struct v4l2_requestbuffers *p)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+
+        mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+        return vb2_reqbufs(&layer->vb_queue, p);
+}
+
+static int mxr_querybuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+
+        mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+        return vb2_querybuf(&layer->vb_queue, p);
+}
+
+static int mxr_qbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+
+        mxr_dbg(layer->mdev, "%s:%d(%d)\n", __func__, __LINE__, p->index);
+        return vb2_qbuf(&layer->vb_queue, p);
+}
+
+static int mxr_dqbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+
+        mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+        return vb2_dqbuf(&layer->vb_queue, p, file->f_flags & O_NONBLOCK);
+}
+
+static int mxr_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+
+        mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+        return vb2_streamon(&layer->vb_queue, i);
+}
+
+static int mxr_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+
+        mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+        return vb2_streamoff(&layer->vb_queue, i);
+}
+
+static const struct v4l2_ioctl_ops mxr_ioctl_ops = {
+        .vidioc_querycap = mxr_querycap,
+        /* format handling */
+        .vidioc_enum_fmt_vid_out = mxr_enum_fmt,
+        .vidioc_s_fmt_vid_out_mplane = mxr_s_fmt,
+        .vidioc_g_fmt_vid_out_mplane = mxr_g_fmt,
+        /* buffer control */
+        .vidioc_reqbufs = mxr_reqbufs,
+        .vidioc_querybuf = mxr_querybuf,
+        .vidioc_qbuf = mxr_qbuf,
+        .vidioc_dqbuf = mxr_dqbuf,
+        /* Streaming control */
+        .vidioc_streamon = mxr_streamon,
+        .vidioc_streamoff = mxr_streamoff,
+        /* Preset functions */
+        .vidioc_enum_dv_presets = mxr_enum_dv_presets,
+        .vidioc_s_dv_preset = mxr_s_dv_preset,
+        .vidioc_g_dv_preset = mxr_g_dv_preset,
+        /* analog TV standard functions */
+        .vidioc_s_std = mxr_s_std,
+        .vidioc_g_std = mxr_g_std,
+        /* Output handling */
+        .vidioc_enum_output = mxr_enum_output,
+        .vidioc_s_output = mxr_s_output,
+        .vidioc_g_output = mxr_g_output,
+        /* Crop ioctls */
+        .vidioc_g_crop = mxr_g_crop,
+        .vidioc_s_crop = mxr_s_crop,
+        .vidioc_cropcap = mxr_cropcap,
+};
+
+static int mxr_video_open(struct file *file)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+        struct mxr_device *mdev = layer->mdev;
+        int ret = 0;
+
+        mxr_dbg(mdev, "%s:%d\n", __func__, __LINE__);
+        /* assure device probe is finished */
+        wait_for_device_probe();
+        /* creating context for file descriptor */
+        ret = v4l2_fh_open(file);
+        if (ret) {
+                mxr_err(mdev, "v4l2_fh_open failed\n");
+                return ret;
+        }
+
+        /* leaving if layer is already initialized */
+        if (!v4l2_fh_is_singular_file(file))
+                return 0;
+
+        /* FIXME: should power be enabled on open? */
+        ret = mxr_power_get(mdev);
+        if (ret) {
+                mxr_err(mdev, "power on failed\n");
+                goto fail_fh_open;
+        }
+
+        ret = vb2_queue_init(&layer->vb_queue);
+        if (ret != 0) {
+                mxr_err(mdev, "failed to initialize vb2 queue\n");
+                goto fail_power;
+        }
+        /* set default format, first on the list */
+        layer->fmt = layer->fmt_array[0];
+        /* setup default geometry */
+        mxr_layer_default_geo(layer);
+
+        return 0;
+
+fail_power:
+        mxr_power_put(mdev);
+
+fail_fh_open:
+        v4l2_fh_release(file);
+
+        return ret;
+}
+
+static unsigned int
+mxr_video_poll(struct file *file, struct poll_table_struct *wait)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+
+        mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+
+        return vb2_poll(&layer->vb_queue, file, wait);
+}
+
+static int mxr_video_mmap(struct file *file, struct vm_area_struct *vma)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+
+        mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+
+        return vb2_mmap(&layer->vb_queue, vma);
+}
+
+static int mxr_video_release(struct file *file)
+{
+        struct mxr_layer *layer = video_drvdata(file);
+
+        mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+        if (v4l2_fh_is_singular_file(file)) {
+                vb2_queue_release(&layer->vb_queue);
+                mxr_power_put(layer->mdev);
+        }
+        v4l2_fh_release(file);
+        return 0;
+}
+
+static const struct v4l2_file_operations mxr_fops = {
+        .owner = THIS_MODULE,
+        .open = mxr_video_open,
+        .poll = mxr_video_poll,
+        .mmap = mxr_video_mmap,
+        .release = mxr_video_release,
+        .unlocked_ioctl = video_ioctl2,
+};
+
+static int queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+        unsigned int *nplanes, unsigned long sizes[],
+        void *alloc_ctxs[])
+{
+        struct mxr_layer *layer = vb2_get_drv_priv(vq);
+        const struct mxr_format *fmt = layer->fmt;
+        int i;
+        struct mxr_device *mdev = layer->mdev;
+        struct v4l2_plane_pix_format planes[3];
+
+        mxr_dbg(mdev, "%s\n", __func__);
+        /* checking if format was configured */
+        if (fmt == NULL)
+                return -EINVAL;
+        mxr_dbg(mdev, "fmt = %s\n", fmt->name);
+        mxr_mplane_fill(planes, fmt, layer->geo.src.full_width,
+                layer->geo.src.full_height);
+
+        *nplanes = fmt->num_subframes;
+        for (i = 0; i < fmt->num_subframes; ++i) {
+                alloc_ctxs[i] = layer->mdev->alloc_ctx;
+                sizes[i] = PAGE_ALIGN(planes[i].sizeimage);
+                mxr_dbg(mdev, "size[%d] = %08lx\n", i, sizes[i]);
+        }
+
+        if (*nbuffers == 0)
+                *nbuffers = 1;
+
+        return 0;
+}
+
+static void buf_queue(struct vb2_buffer *vb)
+{
+        struct mxr_buffer *buffer = container_of(vb, struct mxr_buffer, vb);
+        struct mxr_layer *layer = vb2_get_drv_priv(vb->vb2_queue);
+        struct mxr_device *mdev = layer->mdev;
+        unsigned long flags;
+        int must_start = 0;
+
+        spin_lock_irqsave(&layer->enq_slock, flags);
+        if (layer->state == MXR_LAYER_STREAMING_START) {
+                layer->state = MXR_LAYER_STREAMING;
+                must_start = 1;
+        }
+        list_add_tail(&buffer->list, &layer->enq_list);
+        spin_unlock_irqrestore(&layer->enq_slock, flags);
+        if (must_start) {
+                layer->ops.stream_set(layer, MXR_ENABLE);
+                mxr_streamer_get(mdev);
+        }
+
+        mxr_dbg(mdev, "queuing buffer\n");
+}
+
+static void wait_lock(struct vb2_queue *vq)
+{
+        struct mxr_layer *layer = vb2_get_drv_priv(vq);
+
+        mxr_dbg(layer->mdev, "%s\n", __func__);
+        mutex_lock(&layer->mutex);
+}
+
+static void wait_unlock(struct vb2_queue *vq)
+{
+        struct mxr_layer *layer = vb2_get_drv_priv(vq);
+
+        mxr_dbg(layer->mdev, "%s\n", __func__);
+        mutex_unlock(&layer->mutex);
+}
+
+static int start_streaming(struct vb2_queue *vq)
+{
+        struct mxr_layer *layer = vb2_get_drv_priv(vq);
+        struct mxr_device *mdev = layer->mdev;
+        unsigned long flags;
+
+        mxr_dbg(mdev, "%s\n", __func__);
+        /* block any changes in output configuration */
+        mxr_output_get(mdev);
+
+        /* update layers geometry */
+        mxr_layer_geo_fix(layer);
+        mxr_geometry_dump(mdev, &layer->geo);
+
+        layer->ops.format_set(layer);
+        /* enabling layer in hardware */
+        spin_lock_irqsave(&layer->enq_slock, flags);
+        layer->state = MXR_LAYER_STREAMING_START;
+        spin_unlock_irqrestore(&layer->enq_slock, flags);
+
+        return 0;
+}
+
+static void mxr_watchdog(unsigned long arg)
+{
+        struct mxr_layer *layer = (struct mxr_layer *) arg;
+        struct mxr_device *mdev = layer->mdev;
+        unsigned long flags;
+
+        mxr_err(mdev, "watchdog fired for layer %s\n", layer->vfd.name);
+
+        spin_lock_irqsave(&layer->enq_slock, flags);
+
+        if (layer->update_buf == layer->shadow_buf)
+                layer->update_buf = NULL;
+        if (layer->update_buf) {
+                vb2_buffer_done(&layer->update_buf->vb, VB2_BUF_STATE_ERROR);
+                layer->update_buf = NULL;
+        }
+        if (layer->shadow_buf) {
+                vb2_buffer_done(&layer->shadow_buf->vb, VB2_BUF_STATE_ERROR);
+                layer->shadow_buf = NULL;
+        }
+        spin_unlock_irqrestore(&layer->enq_slock, flags);
+}
+
+static int stop_streaming(struct vb2_queue *vq)
+{
+        struct mxr_layer *layer = vb2_get_drv_priv(vq);
+        struct mxr_device *mdev = layer->mdev;
+        unsigned long flags;
+        struct timer_list watchdog;
+        struct mxr_buffer *buf, *buf_tmp;
+
+        mxr_dbg(mdev, "%s\n", __func__);
+
+        spin_lock_irqsave(&layer->enq_slock, flags);
+
+        /* reset list */
+        layer->state = MXR_LAYER_STREAMING_FINISH;
+
+        /* set all buffer to be done */
+        list_for_each_entry_safe(buf, buf_tmp, &layer->enq_list, list) {
+                list_del(&buf->list);
+                vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+        }
+
+        spin_unlock_irqrestore(&layer->enq_slock, flags);
+
+        /* give 1 seconds to complete to complete last buffers */
+        setup_timer_on_stack(&watchdog, mxr_watchdog,
+                (unsigned long)layer);
+        mod_timer(&watchdog, jiffies + msecs_to_jiffies(1000));
+
+        /* wait until all buffers are goes to done state */
+        vb2_wait_for_all_buffers(vq);
+
+        /* stop timer if all synchronization is done */
+        del_timer_sync(&watchdog);
+        destroy_timer_on_stack(&watchdog);
+
+        /* stopping hardware */
+        spin_lock_irqsave(&layer->enq_slock, flags);
+        layer->state = MXR_LAYER_IDLE;
+        spin_unlock_irqrestore(&layer->enq_slock, flags);
+
+        /* disabling layer in hardware */
+        layer->ops.stream_set(layer, MXR_DISABLE);
+        /* remove one streamer */
+        mxr_streamer_put(mdev);
+        /* allow changes in output configuration */
+        mxr_output_put(mdev);
+        return 0;
+}
+
+static struct vb2_ops mxr_video_qops = {
+        .queue_setup = queue_setup,
+        .buf_queue = buf_queue,
+        .wait_prepare = wait_unlock,
+        .wait_finish = wait_lock,
+        .start_streaming = start_streaming,
+        .stop_streaming = stop_streaming,
+};
+
+/* FIXME: try to put this functions to mxr_base_layer_create */
+int mxr_base_layer_register(struct mxr_layer *layer)
+{
+        struct mxr_device *mdev = layer->mdev;
+        int ret;
+
+        ret = video_register_device(&layer->vfd, VFL_TYPE_GRABBER, -1);
+        if (ret)
+                mxr_err(mdev, "failed to register video device\n");
+        else
+                mxr_info(mdev, "registered layer %s as /dev/video%d\n",
+                        layer->vfd.name, layer->vfd.num);
+        return ret;
+}
+
+void mxr_base_layer_unregister(struct mxr_layer *layer)
+{
+        video_unregister_device(&layer->vfd);
+}
+
+void mxr_layer_release(struct mxr_layer *layer)
+{
+        if (layer->ops.release)
+                layer->ops.release(layer);
+}
+
+void mxr_base_layer_release(struct mxr_layer *layer)
+{
+        kfree(layer);
+}
+
+static void mxr_vfd_release(struct video_device *vdev)
+{
+        printk(KERN_INFO "video device release\n");
+}
+
+struct mxr_layer *mxr_base_layer_create(struct mxr_device *mdev,
+        int idx, char *name, struct mxr_layer_ops *ops)
+{
+        struct mxr_layer *layer;
+
+        layer = kzalloc(sizeof *layer, GFP_KERNEL);
+        if (layer == NULL) {
+                mxr_err(mdev, "not enough memory for layer.\n");
+                goto fail;
+        }
+
+        layer->mdev = mdev;
+        layer->idx = idx;
+        layer->ops = *ops;
+
+        spin_lock_init(&layer->enq_slock);
+        INIT_LIST_HEAD(&layer->enq_list);
+        mutex_init(&layer->mutex);
+
+        layer->vfd = (struct video_device) {
+                .minor = -1,
+                .release = mxr_vfd_release,
+                .fops = &mxr_fops,
+                .ioctl_ops = &mxr_ioctl_ops,
+        };
+        strlcpy(layer->vfd.name, name, sizeof(layer->vfd.name));
+        /* let framework control PRIORITY */
+        set_bit(V4L2_FL_USE_FH_PRIO, &layer->vfd.flags);
+
+        video_set_drvdata(&layer->vfd, layer);
+        layer->vfd.lock = &layer->mutex;
+        layer->vfd.v4l2_dev = &mdev->v4l2_dev;
+
+        layer->vb_queue = (struct vb2_queue) {
+                .type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+                .io_modes = VB2_MMAP | VB2_USERPTR,
+                .drv_priv = layer,
+                .buf_struct_size = sizeof(struct mxr_buffer),
+                .ops = &mxr_video_qops,
+                .mem_ops = &vb2_dma_contig_memops,
+        };
+
+        return layer;
+
+fail:
+        return NULL;
+}
+
+static const struct mxr_format *find_format_by_fourcc(
+        struct mxr_layer *layer, unsigned long fourcc)
+{
+        int i;
+
+        for (i = 0; i < layer->fmt_array_size; ++i)
+                if (layer->fmt_array[i]->fourcc == fourcc)
+                        return layer->fmt_array[i];
+        return NULL;
+}
+
+static const struct mxr_format *find_format_by_index(
+        struct mxr_layer *layer, unsigned long index)
+{
+        if (index >= layer->fmt_array_size)
+                return NULL;
+        return layer->fmt_array[index];
+}
+
diff --git a/drivers/media/video/s5p-tv/mixer_vp_layer.c b/drivers/media/video/s5p-tv/mixer_vp_layer.c
new file mode 100644
index 00000000000..6950ed8ac1a
--- /dev/null
+++ b/drivers/media/video/s5p-tv/mixer_vp_layer.c
@@ -0,0 +1,211 @@
+/*
+ * Samsung TV Mixer driver
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *
+ * Tomasz Stanislawski, <t.stanislaws@samsung.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 Foundiation. either version 2 of the License,
+ * or (at your option) any later version
+ */
+
+#include "mixer.h"
+
+#include "regs-vp.h"
+
+#include <media/videobuf2-dma-contig.h>
+
+/* FORMAT DEFINITIONS */
+static const struct mxr_format mxr_fmt_nv12 = {
+        .name = "NV12",
+        .fourcc = V4L2_PIX_FMT_NV12,
+        .colorspace = V4L2_COLORSPACE_JPEG,
+        .num_planes = 2,
+        .plane = {
+                { .width = 1, .height = 1, .size = 1 },
+                { .width = 2, .height = 2, .size = 2 },
+        },
+        .num_subframes = 1,
+        .cookie = VP_MODE_NV12 | VP_MODE_MEM_LINEAR,
+};
+
+static const struct mxr_format mxr_fmt_nv21 = {
+        .name = "NV21",
+        .fourcc = V4L2_PIX_FMT_NV21,
+        .colorspace = V4L2_COLORSPACE_JPEG,
+        .num_planes = 2,
+        .plane = {
+                { .width = 1, .height = 1, .size = 1 },
+                { .width = 2, .height = 2, .size = 2 },
+        },
+        .num_subframes = 1,
+        .cookie = VP_MODE_NV21 | VP_MODE_MEM_LINEAR,
+};
+
+static const struct mxr_format mxr_fmt_nv12m = {
+        .name = "NV12 (mplane)",
+        .fourcc = V4L2_PIX_FMT_NV12M,
+        .colorspace = V4L2_COLORSPACE_JPEG,
+        .num_planes = 2,
+        .plane = {
+                { .width = 1, .height = 1, .size = 1 },
+                { .width = 2, .height = 2, .size = 2 },
+        },
+        .num_subframes = 2,
+        .plane2subframe = {0, 1},
+        .cookie = VP_MODE_NV12 | VP_MODE_MEM_LINEAR,
+};
+
+static const struct mxr_format mxr_fmt_nv12mt = {
+        .name = "NV12 tiled (mplane)",
+        .fourcc = V4L2_PIX_FMT_NV12MT,
+        .colorspace = V4L2_COLORSPACE_JPEG,
+        .num_planes = 2,
+        .plane = {
+                { .width = 128, .height = 32, .size = 4096 },
+                { .width = 128, .height = 32, .size = 2048 },
+        },
+        .num_subframes = 2,
+        .plane2subframe = {0, 1},
+        .cookie = VP_MODE_NV12 | VP_MODE_MEM_TILED,
+};
+
+static const struct mxr_format *mxr_video_format[] = {
+        &mxr_fmt_nv12,
+        &mxr_fmt_nv21,
+        &mxr_fmt_nv12m,
+        &mxr_fmt_nv12mt,
+};
+
+/* AUXILIARY CALLBACKS */
+
+static void mxr_vp_layer_release(struct mxr_layer *layer)
+{
+        mxr_base_layer_unregister(layer);
+        mxr_base_layer_release(layer);
+}
+
+static void mxr_vp_buffer_set(struct mxr_layer *layer,
+        struct mxr_buffer *buf)
+{
+        dma_addr_t luma_addr[2] = {0, 0};
+        dma_addr_t chroma_addr[2] = {0, 0};
+
+        if (buf == NULL) {
+                mxr_reg_vp_buffer(layer->mdev, luma_addr, chroma_addr);
+                return;
+        }
+        luma_addr[0] = vb2_dma_contig_plane_paddr(&buf->vb, 0);
+        if (layer->fmt->num_subframes == 2) {
+                chroma_addr[0] = vb2_dma_contig_plane_paddr(&buf->vb, 1);
+        } else {
+                /* FIXME: mxr_get_plane_size compute integer division,
+                 * which is slow and should not be performed in interrupt */
+                chroma_addr[0] = luma_addr[0] + mxr_get_plane_size(
+                        &layer->fmt->plane[0], layer->geo.src.full_width,
+                        layer->geo.src.full_height);
+        }
+        if (layer->fmt->cookie & VP_MODE_MEM_TILED) {
+                luma_addr[1] = luma_addr[0] + 0x40;
+                chroma_addr[1] = chroma_addr[0] + 0x40;
+        } else {
+                luma_addr[1] = luma_addr[0] + layer->geo.src.full_width;
+                chroma_addr[1] = chroma_addr[0];
+        }
+        mxr_reg_vp_buffer(layer->mdev, luma_addr, chroma_addr);
+}
+
+static void mxr_vp_stream_set(struct mxr_layer *layer, int en)
+{
+        mxr_reg_vp_layer_stream(layer->mdev, en);
+}
+
+static void mxr_vp_format_set(struct mxr_layer *layer)
+{
+        mxr_reg_vp_format(layer->mdev, layer->fmt, &layer->geo);
+}
+
+static void mxr_vp_fix_geometry(struct mxr_layer *layer)
+{
+        struct mxr_geometry *geo = &layer->geo;
+
+        /* align horizontal size to 8 pixels */
+        geo->src.full_width = ALIGN(geo->src.full_width, 8);
+        /* limit to boundary size */
+        geo->src.full_width = clamp_val(geo->src.full_width, 8, 8192);
+        geo->src.full_height = clamp_val(geo->src.full_height, 1, 8192);
+        geo->src.width = clamp_val(geo->src.width, 32, geo->src.full_width);
+        geo->src.width = min(geo->src.width, 2047U);
+        geo->src.height = clamp_val(geo->src.height, 4, geo->src.full_height);
+        geo->src.height = min(geo->src.height, 2047U);
+
+        /* setting size of output window */
+        geo->dst.width = clamp_val(geo->dst.width, 8, geo->dst.full_width);
+        geo->dst.height = clamp_val(geo->dst.height, 1, geo->dst.full_height);
+
+        /* ensure that scaling is in range 1/4x to 16x */
+        if (geo->src.width >= 4 * geo->dst.width)
+                geo->src.width = 4 * geo->dst.width;
+        if (geo->dst.width >= 16 * geo->src.width)
+                geo->dst.width = 16 * geo->src.width;
+        if (geo->src.height >= 4 * geo->dst.height)
+                geo->src.height = 4 * geo->dst.height;
+        if (geo->dst.height >= 16 * geo->src.height)
+                geo->dst.height = 16 * geo->src.height;
+
+        /* setting scaling ratio */
+        geo->x_ratio = (geo->src.width << 16) / geo->dst.width;
+        geo->y_ratio = (geo->src.height << 16) / geo->dst.height;
+
+        /* adjust offsets */
+        geo->src.x_offset = min(geo->src.x_offset,
+                geo->src.full_width - geo->src.width);
+        geo->src.y_offset = min(geo->src.y_offset,
+                geo->src.full_height - geo->src.height);
+        geo->dst.x_offset = min(geo->dst.x_offset,
+                geo->dst.full_width - geo->dst.width);
+        geo->dst.y_offset = min(geo->dst.y_offset,
+                geo->dst.full_height - geo->dst.height);
+}
+
+/* PUBLIC API */
+
+struct mxr_layer *mxr_vp_layer_create(struct mxr_device *mdev, int idx)
+{
+        struct mxr_layer *layer;
+        int ret;
+        struct mxr_layer_ops ops = {
+                .release = mxr_vp_layer_release,
+                .buffer_set = mxr_vp_buffer_set,
+                .stream_set = mxr_vp_stream_set,
+                .format_set = mxr_vp_format_set,
+                .fix_geometry = mxr_vp_fix_geometry,
+        };
+        char name[32];
+
+        sprintf(name, "video%d", idx);
+
+        layer = mxr_base_layer_create(mdev, idx, name, &ops);
+        if (layer == NULL) {
+                mxr_err(mdev, "failed to initialize layer(%d) base\n", idx);
+                goto fail;
+        }
+
+        layer->fmt_array = mxr_video_format;
+        layer->fmt_array_size = ARRAY_SIZE(mxr_video_format);
+
+        ret = mxr_base_layer_register(layer);
+        if (ret)
+                goto fail_layer;
+
+        return layer;
+
+fail_layer:
+        mxr_base_layer_release(layer);
+
+fail:
+        return NULL;
+}
+
diff --git a/drivers/media/video/s5p-tv/regs-hdmi.h b/drivers/media/video/s5p-tv/regs-hdmi.h
new file mode 100644
index 00000000000..ac93ad6f2bc
--- /dev/null
+++ b/drivers/media/video/s5p-tv/regs-hdmi.h
@@ -0,0 +1,141 @@
+/* linux/arch/arm/mach-exynos4/include/mach/regs-hdmi.h
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ * http://www.samsung.com/
+ *
+ * HDMI register header file for Samsung TVOUT driver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#ifndef SAMSUNG_REGS_HDMI_H
+#define SAMSUNG_REGS_HDMI_H
+
+/*
+ * Register part
+*/
+
+#define HDMI_CTRL_BASE(x)               ((x) + 0x00000000)
+#define HDMI_CORE_BASE(x)               ((x) + 0x00010000)
+#define HDMI_TG_BASE(x)                 ((x) + 0x00050000)
+
+/* Control registers */
+#define HDMI_INTC_CON                   HDMI_CTRL_BASE(0x0000)
+#define HDMI_INTC_FLAG                  HDMI_CTRL_BASE(0x0004)
+#define HDMI_HPD_STATUS                 HDMI_CTRL_BASE(0x000C)
+#define HDMI_PHY_RSTOUT                 HDMI_CTRL_BASE(0x0014)
+#define HDMI_PHY_VPLL                   HDMI_CTRL_BASE(0x0018)
+#define HDMI_PHY_CMU                    HDMI_CTRL_BASE(0x001C)
+#define HDMI_CORE_RSTOUT                HDMI_CTRL_BASE(0x0020)
+
+/* Core registers */
+#define HDMI_CON_0                      HDMI_CORE_BASE(0x0000)
+#define HDMI_CON_1                      HDMI_CORE_BASE(0x0004)
+#define HDMI_CON_2                      HDMI_CORE_BASE(0x0008)
+#define HDMI_SYS_STATUS                 HDMI_CORE_BASE(0x0010)
+#define HDMI_PHY_STATUS                 HDMI_CORE_BASE(0x0014)
+#define HDMI_STATUS_EN                  HDMI_CORE_BASE(0x0020)
+#define HDMI_HPD                        HDMI_CORE_BASE(0x0030)
+#define HDMI_MODE_SEL                   HDMI_CORE_BASE(0x0040)
+#define HDMI_BLUE_SCREEN_0              HDMI_CORE_BASE(0x0050)
+#define HDMI_BLUE_SCREEN_1              HDMI_CORE_BASE(0x0054)
+#define HDMI_BLUE_SCREEN_2              HDMI_CORE_BASE(0x0058)
+#define HDMI_H_BLANK_0                  HDMI_CORE_BASE(0x00A0)
+#define HDMI_H_BLANK_1                  HDMI_CORE_BASE(0x00A4)
+#define HDMI_V_BLANK_0                  HDMI_CORE_BASE(0x00B0)
+#define HDMI_V_BLANK_1                  HDMI_CORE_BASE(0x00B4)
+#define HDMI_V_BLANK_2                  HDMI_CORE_BASE(0x00B8)
+#define HDMI_H_V_LINE_0                 HDMI_CORE_BASE(0x00C0)
+#define HDMI_H_V_LINE_1                 HDMI_CORE_BASE(0x00C4)
+#define HDMI_H_V_LINE_2                 HDMI_CORE_BASE(0x00C8)
+#define HDMI_VSYNC_POL                  HDMI_CORE_BASE(0x00E4)
+#define HDMI_INT_PRO_MODE               HDMI_CORE_BASE(0x00E8)
+#define HDMI_V_BLANK_F_0                HDMI_CORE_BASE(0x0110)
+#define HDMI_V_BLANK_F_1                HDMI_CORE_BASE(0x0114)
+#define HDMI_V_BLANK_F_2                HDMI_CORE_BASE(0x0118)
+#define HDMI_H_SYNC_GEN_0               HDMI_CORE_BASE(0x0120)
+#define HDMI_H_SYNC_GEN_1               HDMI_CORE_BASE(0x0124)
+#define HDMI_H_SYNC_GEN_2               HDMI_CORE_BASE(0x0128)
+#define HDMI_V_SYNC_GEN_1_0             HDMI_CORE_BASE(0x0130)
+#define HDMI_V_SYNC_GEN_1_1             HDMI_CORE_BASE(0x0134)
+#define HDMI_V_SYNC_GEN_1_2             HDMI_CORE_BASE(0x0138)
+#define HDMI_V_SYNC_GEN_2_0             HDMI_CORE_BASE(0x0140)
+#define HDMI_V_SYNC_GEN_2_1             HDMI_CORE_BASE(0x0144)
+#define HDMI_V_SYNC_GEN_2_2             HDMI_CORE_BASE(0x0148)
+#define HDMI_V_SYNC_GEN_3_0             HDMI_CORE_BASE(0x0150)
+#define HDMI_V_SYNC_GEN_3_1             HDMI_CORE_BASE(0x0154)
+#define HDMI_V_SYNC_GEN_3_2             HDMI_CORE_BASE(0x0158)
+#define HDMI_AVI_CON                    HDMI_CORE_BASE(0x0300)
+#define HDMI_AVI_BYTE(n)                HDMI_CORE_BASE(0x0320 + 4 * (n))
+#define HDMI_DC_CONTROL                 HDMI_CORE_BASE(0x05C0)
+#define HDMI_VIDEO_PATTERN_GEN          HDMI_CORE_BASE(0x05C4)
+#define HDMI_HPD_GEN                    HDMI_CORE_BASE(0x05C8)
+
+/* Timing generator registers */
+#define HDMI_TG_CMD                     HDMI_TG_BASE(0x0000)
+#define HDMI_TG_H_FSZ_L                 HDMI_TG_BASE(0x0018)
+#define HDMI_TG_H_FSZ_H                 HDMI_TG_BASE(0x001C)
+#define HDMI_TG_HACT_ST_L               HDMI_TG_BASE(0x0020)
+#define HDMI_TG_HACT_ST_H               HDMI_TG_BASE(0x0024)
+#define HDMI_TG_HACT_SZ_L               HDMI_TG_BASE(0x0028)
+#define HDMI_TG_HACT_SZ_H               HDMI_TG_BASE(0x002C)
+#define HDMI_TG_V_FSZ_L                 HDMI_TG_BASE(0x0030)
+#define HDMI_TG_V_FSZ_H                 HDMI_TG_BASE(0x0034)
+#define HDMI_TG_VSYNC_L                 HDMI_TG_BASE(0x0038)
+#define HDMI_TG_VSYNC_H                 HDMI_TG_BASE(0x003C)
+#define HDMI_TG_VSYNC2_L                HDMI_TG_BASE(0x0040)
+#define HDMI_TG_VSYNC2_H                HDMI_TG_BASE(0x0044)
+#define HDMI_TG_VACT_ST_L               HDMI_TG_BASE(0x0048)
+#define HDMI_TG_VACT_ST_H               HDMI_TG_BASE(0x004C)
+#define HDMI_TG_VACT_SZ_L               HDMI_TG_BASE(0x0050)
+#define HDMI_TG_VACT_SZ_H               HDMI_TG_BASE(0x0054)
+#define HDMI_TG_FIELD_CHG_L             HDMI_TG_BASE(0x0058)
+#define HDMI_TG_FIELD_CHG_H             HDMI_TG_BASE(0x005C)
+#define HDMI_TG_VACT_ST2_L              HDMI_TG_BASE(0x0060)
+#define HDMI_TG_VACT_ST2_H              HDMI_TG_BASE(0x0064)
+#define HDMI_TG_VSYNC_TOP_HDMI_L        HDMI_TG_BASE(0x0078)
+#define HDMI_TG_VSYNC_TOP_HDMI_H        HDMI_TG_BASE(0x007C)
+#define HDMI_TG_VSYNC_BOT_HDMI_L        HDMI_TG_BASE(0x0080)
+#define HDMI_TG_VSYNC_BOT_HDMI_H        HDMI_TG_BASE(0x0084)
+#define HDMI_TG_FIELD_TOP_HDMI_L        HDMI_TG_BASE(0x0088)
+#define HDMI_TG_FIELD_TOP_HDMI_H        HDMI_TG_BASE(0x008C)
+#define HDMI_TG_FIELD_BOT_HDMI_L        HDMI_TG_BASE(0x0090)
+#define HDMI_TG_FIELD_BOT_HDMI_H        HDMI_TG_BASE(0x0094)
+
+/*
+ * Bit definition part
+ */
+
+/* HDMI_INTC_CON */
+#define HDMI_INTC_EN_GLOBAL             (1 << 6)
+#define HDMI_INTC_EN_HPD_PLUG           (1 << 3)
+#define HDMI_INTC_EN_HPD_UNPLUG         (1 << 2)
+
+/* HDMI_INTC_FLAG */
+#define HDMI_INTC_FLAG_HPD_PLUG         (1 << 3)
+#define HDMI_INTC_FLAG_HPD_UNPLUG       (1 << 2)
+
+/* HDMI_PHY_RSTOUT */
+#define HDMI_PHY_SW_RSTOUT              (1 << 0)
+
+/* HDMI_CORE_RSTOUT */
+#define HDMI_CORE_SW_RSTOUT             (1 << 0)
+
+/* HDMI_CON_0 */
+#define HDMI_BLUE_SCR_EN                (1 << 5)
+#define HDMI_EN                         (1 << 0)
+
+/* HDMI_PHY_STATUS */
+#define HDMI_PHY_STATUS_READY           (1 << 0)
+
+/* HDMI_MODE_SEL */
+#define HDMI_MODE_HDMI_EN               (1 << 1)
+#define HDMI_MODE_DVI_EN                (1 << 0)
+#define HDMI_MODE_MASK                  (3 << 0)
+
+/* HDMI_TG_CMD */
+#define HDMI_TG_EN                      (1 << 0)
+
+#endif /* SAMSUNG_REGS_HDMI_H */
diff --git a/drivers/media/video/s5p-tv/regs-mixer.h b/drivers/media/video/s5p-tv/regs-mixer.h
new file mode 100644
index 00000000000..3c8442609c1
--- /dev/null
+++ b/drivers/media/video/s5p-tv/regs-mixer.h
@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ * http://www.samsung.com/
+ *
+ * Mixer register header file for Samsung Mixer driver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+#ifndef SAMSUNG_REGS_MIXER_H
+#define SAMSUNG_REGS_MIXER_H
+
+/*
+ * Register part
+ */
+#define MXR_STATUS                      0x0000
+#define MXR_CFG                         0x0004
+#define MXR_INT_EN                      0x0008
+#define MXR_INT_STATUS                  0x000C
+#define MXR_LAYER_CFG                   0x0010
+#define MXR_VIDEO_CFG                   0x0014
+#define MXR_GRAPHIC0_CFG                0x0020
+#define MXR_GRAPHIC0_BASE               0x0024
+#define MXR_GRAPHIC0_SPAN               0x0028
+#define MXR_GRAPHIC0_SXY                0x002C
+#define MXR_GRAPHIC0_WH                 0x0030
+#define MXR_GRAPHIC0_DXY                0x0034
+#define MXR_GRAPHIC0_BLANK              0x0038
+#define MXR_GRAPHIC1_CFG                0x0040
+#define MXR_GRAPHIC1_BASE               0x0044
+#define MXR_GRAPHIC1_SPAN               0x0048
+#define MXR_GRAPHIC1_SXY                0x004C
+#define MXR_GRAPHIC1_WH                 0x0050
+#define MXR_GRAPHIC1_DXY                0x0054
+#define MXR_GRAPHIC1_BLANK              0x0058
+#define MXR_BG_CFG                      0x0060
+#define MXR_BG_COLOR0                   0x0064
+#define MXR_BG_COLOR1                   0x0068
+#define MXR_BG_COLOR2                   0x006C
+
+/* for parametrized access to layer registers */
+#define MXR_GRAPHIC_CFG(i)              (0x0020 + (i) * 0x20)
+#define MXR_GRAPHIC_BASE(i)             (0x0024 + (i) * 0x20)
+#define MXR_GRAPHIC_SPAN(i)             (0x0028 + (i) * 0x20)
+#define MXR_GRAPHIC_SXY(i)              (0x002C + (i) * 0x20)
+#define MXR_GRAPHIC_WH(i)               (0x0030 + (i) * 0x20)
+#define MXR_GRAPHIC_DXY(i)              (0x0034 + (i) * 0x20)
+
+/*
+ * Bit definition part
+ */
+
+/* generates mask for range of bits */
+#define MXR_MASK(high_bit, low_bit) \
+        (((2 << ((high_bit) - (low_bit))) - 1) << (low_bit))
+
+#define MXR_MASK_VAL(val, high_bit, low_bit) \
+        (((val) << (low_bit)) & MXR_MASK(high_bit, low_bit))
+
+/* bits for MXR_STATUS */
+#define MXR_STATUS_16_BURST             (1 << 7)
+#define MXR_STATUS_BURST_MASK           (1 << 7)
+#define MXR_STATUS_SYNC_ENABLE          (1 << 2)
+#define MXR_STATUS_REG_RUN              (1 << 0)
+
+/* bits for MXR_CFG */
+#define MXR_CFG_OUT_YUV444              (0 << 8)
+#define MXR_CFG_OUT_RGB888              (1 << 8)
+#define MXR_CFG_DST_SDO                 (0 << 7)
+#define MXR_CFG_DST_HDMI                (1 << 7)
+#define MXR_CFG_DST_MASK                (1 << 7)
+#define MXR_CFG_SCAN_HD_720             (0 << 6)
+#define MXR_CFG_SCAN_HD_1080            (1 << 6)
+#define MXR_CFG_GRP1_ENABLE             (1 << 5)
+#define MXR_CFG_GRP0_ENABLE             (1 << 4)
+#define MXR_CFG_VP_ENABLE               (1 << 3)
+#define MXR_CFG_SCAN_INTERLACE          (0 << 2)
+#define MXR_CFG_SCAN_PROGRASSIVE        (1 << 2)
+#define MXR_CFG_SCAN_NTSC               (0 << 1)
+#define MXR_CFG_SCAN_PAL                (1 << 1)
+#define MXR_CFG_SCAN_SD                 (0 << 0)
+#define MXR_CFG_SCAN_HD                 (1 << 0)
+#define MXR_CFG_SCAN_MASK               0x47
+
+/* bits for MXR_GRAPHICn_CFG */
+#define MXR_GRP_CFG_COLOR_KEY_DISABLE   (1 << 21)
+#define MXR_GRP_CFG_BLEND_PRE_MUL       (1 << 20)
+#define MXR_GRP_CFG_FORMAT_VAL(x)       MXR_MASK_VAL(x, 11, 8)
+#define MXR_GRP_CFG_FORMAT_MASK         MXR_GRP_CFG_FORMAT_VAL(~0)
+#define MXR_GRP_CFG_ALPHA_VAL(x)        MXR_MASK_VAL(x, 7, 0)
+
+/* bits for MXR_GRAPHICn_WH */
+#define MXR_GRP_WH_H_SCALE(x)           MXR_MASK_VAL(x, 28, 28)
+#define MXR_GRP_WH_V_SCALE(x)           MXR_MASK_VAL(x, 12, 12)
+#define MXR_GRP_WH_WIDTH(x)             MXR_MASK_VAL(x, 26, 16)
+#define MXR_GRP_WH_HEIGHT(x)            MXR_MASK_VAL(x, 10, 0)
+
+/* bits for MXR_GRAPHICn_SXY */
+#define MXR_GRP_SXY_SX(x)               MXR_MASK_VAL(x, 26, 16)
+#define MXR_GRP_SXY_SY(x)               MXR_MASK_VAL(x, 10, 0)
+
+/* bits for MXR_GRAPHICn_DXY */
+#define MXR_GRP_DXY_DX(x)               MXR_MASK_VAL(x, 26, 16)
+#define MXR_GRP_DXY_DY(x)               MXR_MASK_VAL(x, 10, 0)
+
+/* bits for MXR_INT_EN */
+#define MXR_INT_EN_VSYNC                (1 << 11)
+#define MXR_INT_EN_ALL                  (0x0f << 8)
+
+/* bit for MXR_INT_STATUS */
+#define MXR_INT_CLEAR_VSYNC             (1 << 11)
+#define MXR_INT_STATUS_VSYNC            (1 << 0)
+
+/* bit for MXR_LAYER_CFG */
+#define MXR_LAYER_CFG_GRP1_VAL(x)       MXR_MASK_VAL(x, 11, 8)
+#define MXR_LAYER_CFG_GRP0_VAL(x)       MXR_MASK_VAL(x, 7, 4)
+#define MXR_LAYER_CFG_VP_VAL(x)         MXR_MASK_VAL(x, 3, 0)
+
+#endif /* SAMSUNG_REGS_MIXER_H */
+
diff --git a/drivers/media/video/s5p-tv/regs-sdo.h b/drivers/media/video/s5p-tv/regs-sdo.h
new file mode 100644
index 00000000000..7f7c2b8ac14
--- /dev/null
+++ b/drivers/media/video/s5p-tv/regs-sdo.h
@@ -0,0 +1,63 @@
+/* drivers/media/video/s5p-tv/regs-sdo.h
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * SDO register description file
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef SAMSUNG_REGS_SDO_H
+#define SAMSUNG_REGS_SDO_H
+
+/*
+ * Register part
+ */
+
+#define SDO_CLKCON                      0x0000
+#define SDO_CONFIG                      0x0008
+#define SDO_VBI                         0x0014
+#define SDO_DAC                         0x003C
+#define SDO_CCCON                       0x0180
+#define SDO_IRQ                         0x0280
+#define SDO_IRQMASK                     0x0284
+#define SDO_VERSION                     0x03D8
+
+/*
+ * Bit definition part
+ */
+
+/* SDO Clock Control Register (SDO_CLKCON) */
+#define SDO_TVOUT_SW_RESET              (1 << 4)
+#define SDO_TVOUT_CLOCK_READY           (1 << 1)
+#define SDO_TVOUT_CLOCK_ON              (1 << 0)
+
+/* SDO Video Standard Configuration Register (SDO_CONFIG) */
+#define SDO_PROGRESSIVE                 (1 << 4)
+#define SDO_NTSC_M                      0
+#define SDO_PAL_M                       1
+#define SDO_PAL_BGHID                   2
+#define SDO_PAL_N                       3
+#define SDO_PAL_NC                      4
+#define SDO_NTSC_443                    8
+#define SDO_PAL_60                      9
+#define SDO_STANDARD_MASK               0xf
+
+/* SDO VBI Configuration Register (SDO_VBI) */
+#define SDO_CVBS_WSS_INS                (1 << 14)
+#define SDO_CVBS_CLOSED_CAPTION_MASK    (3 << 12)
+
+/* SDO DAC Configuration Register (SDO_DAC) */
+#define SDO_POWER_ON_DAC                (1 << 0)
+
+/* SDO Color Compensation On/Off Control (SDO_CCCON) */
+#define SDO_COMPENSATION_BHS_ADJ_OFF    (1 << 4)
+#define SDO_COMPENSATION_CVBS_COMP_OFF  (1 << 0)
+
+/* SDO Interrupt Request Register (SDO_IRQ) */
+#define SDO_VSYNC_IRQ_PEND              (1 << 0)
+
+#endif /* SAMSUNG_REGS_SDO_H */
diff --git a/drivers/media/video/s5p-tv/regs-vp.h b/drivers/media/video/s5p-tv/regs-vp.h
new file mode 100644
index 00000000000..6c63984e11e
--- /dev/null
+++ b/drivers/media/video/s5p-tv/regs-vp.h
@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * Video processor register header file for Samsung Mixer driver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef SAMSUNG_REGS_VP_H
+#define SAMSUNG_REGS_VP_H
+
+/*
+ * Register part
+ */
+
+#define VP_ENABLE                       0x0000
+#define VP_SRESET                       0x0004
+#define VP_SHADOW_UPDATE                0x0008
+#define VP_FIELD_ID                     0x000C
+#define VP_MODE                         0x0010
+#define VP_IMG_SIZE_Y                   0x0014
+#define VP_IMG_SIZE_C                   0x0018
+#define VP_PER_RATE_CTRL                0x001C
+#define VP_TOP_Y_PTR                    0x0028
+#define VP_BOT_Y_PTR                    0x002C
+#define VP_TOP_C_PTR                    0x0030
+#define VP_BOT_C_PTR                    0x0034
+#define VP_ENDIAN_MODE                  0x03CC
+#define VP_SRC_H_POSITION               0x0044
+#define VP_SRC_V_POSITION               0x0048
+#define VP_SRC_WIDTH                    0x004C
+#define VP_SRC_HEIGHT                   0x0050
+#define VP_DST_H_POSITION               0x0054
+#define VP_DST_V_POSITION               0x0058
+#define VP_DST_WIDTH                    0x005C
+#define VP_DST_HEIGHT                   0x0060
+#define VP_H_RATIO                      0x0064
+#define VP_V_RATIO                      0x0068
+#define VP_POLY8_Y0_LL                  0x006C
+#define VP_POLY4_Y0_LL                  0x00EC
+#define VP_POLY4_C0_LL                  0x012C
+
+/*
+ * Bit definition part
+ */
+
+/* generates mask for range of bits */
+
+#define VP_MASK(high_bit, low_bit) \
+        (((2 << ((high_bit) - (low_bit))) - 1) << (low_bit))
+
+#define VP_MASK_VAL(val, high_bit, low_bit) \
+        (((val) << (low_bit)) & VP_MASK(high_bit, low_bit))
+
+ /* VP_ENABLE */
+#define VP_ENABLE_ON                    (1 << 0)
+
+/* VP_SRESET */
+#define VP_SRESET_PROCESSING            (1 << 0)
+
+/* VP_SHADOW_UPDATE */
+#define VP_SHADOW_UPDATE_ENABLE         (1 << 0)
+
+/* VP_MODE */
+#define VP_MODE_NV12                    (0 << 6)
+#define VP_MODE_NV21                    (1 << 6)
+#define VP_MODE_LINE_SKIP               (1 << 5)
+#define VP_MODE_MEM_LINEAR              (0 << 4)
+#define VP_MODE_MEM_TILED               (1 << 4)
+#define VP_MODE_FMT_MASK                (5 << 4)
+#define VP_MODE_FIELD_ID_AUTO_TOGGLING  (1 << 2)
+#define VP_MODE_2D_IPC                  (1 << 1)
+
+/* VP_IMG_SIZE_Y */
+/* VP_IMG_SIZE_C */
+#define VP_IMG_HSIZE(x)                 VP_MASK_VAL(x, 29, 16)
+#define VP_IMG_VSIZE(x)                 VP_MASK_VAL(x, 13, 0)
+
+/* VP_SRC_H_POSITION */
+#define VP_SRC_H_POSITION_VAL(x)        VP_MASK_VAL(x, 14, 4)
+
+/* VP_ENDIAN_MODE */
+#define VP_ENDIAN_MODE_LITTLE           (1 << 0)
+
+#endif /* SAMSUNG_REGS_VP_H */
diff --git a/drivers/media/video/s5p-tv/sdo_drv.c b/drivers/media/video/s5p-tv/sdo_drv.c
new file mode 100644
index 00000000000..4dddd6bd635
--- /dev/null
+++ b/drivers/media/video/s5p-tv/sdo_drv.c
@@ -0,0 +1,479 @@
+/*
+ * Samsung Standard Definition Output (SDO) driver
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *
+ * Tomasz Stanislawski, <t.stanislaws@samsung.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 Foundiation. either version 2 of the License,
+ * or (at your option) any later version
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+
+#include <media/v4l2-subdev.h>
+
+#include "regs-sdo.h"
+
+MODULE_AUTHOR("Tomasz Stanislawski, <t.stanislaws@samsung.com>");
+MODULE_DESCRIPTION("Samsung Standard Definition Output (SDO)");
+MODULE_LICENSE("GPL");
+
+#define SDO_DEFAULT_STD V4L2_STD_PAL
+
+struct sdo_format {
+        v4l2_std_id id;
+        /* all modes are 720 pixels wide */
+        unsigned int height;
+        unsigned int cookie;
+};
+
+struct sdo_device {
+        /** pointer to device parent */
+        struct device *dev;
+        /** base address of SDO registers */
+        void __iomem *regs;
+        /** SDO interrupt */
+        unsigned int irq;
+        /** DAC source clock */
+        struct clk *sclk_dac;
+        /** DAC clock */
+        struct clk *dac;
+        /** DAC physical interface */
+        struct clk *dacphy;
+        /** clock for control of VPLL */
+        struct clk *fout_vpll;
+        /** regulator for SDO IP power */
+        struct regulator *vdac;
+        /** regulator for SDO plug detection */
+        struct regulator *vdet;
+        /** subdev used as device interface */
+        struct v4l2_subdev sd;
+        /** current format */
+        const struct sdo_format *fmt;
+};
+
+static inline struct sdo_device *sd_to_sdev(struct v4l2_subdev *sd)
+{
+        return container_of(sd, struct sdo_device, sd);
+}
+
+static inline
+void sdo_write_mask(struct sdo_device *sdev, u32 reg_id, u32 value, u32 mask)
+{
+        u32 old = readl(sdev->regs + reg_id);
+        value = (value & mask) | (old & ~mask);
+        writel(value, sdev->regs + reg_id);
+}
+
+static inline
+void sdo_write(struct sdo_device *sdev, u32 reg_id, u32 value)
+{
+        writel(value, sdev->regs + reg_id);
+}
+
+static inline
+u32 sdo_read(struct sdo_device *sdev, u32 reg_id)
+{
+        return readl(sdev->regs + reg_id);
+}
+
+static irqreturn_t sdo_irq_handler(int irq, void *dev_data)
+{
+        struct sdo_device *sdev = dev_data;
+
+        /* clear interrupt */
+        sdo_write_mask(sdev, SDO_IRQ, ~0, SDO_VSYNC_IRQ_PEND);
+        return IRQ_HANDLED;
+}
+
+static void sdo_reg_debug(struct sdo_device *sdev)
+{
+#define DBGREG(reg_id) \
+        dev_info(sdev->dev, #reg_id " = %08x\n", \
+                sdo_read(sdev, reg_id))
+
+        DBGREG(SDO_CLKCON);
+        DBGREG(SDO_CONFIG);
+        DBGREG(SDO_VBI);
+        DBGREG(SDO_DAC);
+        DBGREG(SDO_IRQ);
+        DBGREG(SDO_IRQMASK);
+        DBGREG(SDO_VERSION);
+}
+
+static const struct sdo_format sdo_format[] = {
+        { V4L2_STD_PAL_N,       .height = 576, .cookie = SDO_PAL_N },
+        { V4L2_STD_PAL_Nc,      .height = 576, .cookie = SDO_PAL_NC },
+        { V4L2_STD_PAL_M,       .height = 480, .cookie = SDO_PAL_M },
+        { V4L2_STD_PAL_60,      .height = 480, .cookie = SDO_PAL_60 },
+        { V4L2_STD_NTSC_443,    .height = 480, .cookie = SDO_NTSC_443 },
+        { V4L2_STD_PAL,         .height = 576, .cookie = SDO_PAL_BGHID },
+        { V4L2_STD_NTSC_M,      .height = 480, .cookie = SDO_NTSC_M },
+};
+
+static const struct sdo_format *sdo_find_format(v4l2_std_id id)
+{
+        int i;
+        for (i = 0; i < ARRAY_SIZE(sdo_format); ++i)
+                if (sdo_format[i].id & id)
+                        return &sdo_format[i];
+        return NULL;
+}
+
+static int sdo_g_tvnorms_output(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+        *std = V4L2_STD_NTSC_M | V4L2_STD_PAL_M | V4L2_STD_PAL |
+                V4L2_STD_PAL_N | V4L2_STD_PAL_Nc |
+                V4L2_STD_NTSC_443 | V4L2_STD_PAL_60;
+        return 0;
+}
+
+static int sdo_s_std_output(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+        struct sdo_device *sdev = sd_to_sdev(sd);
+        const struct sdo_format *fmt;
+        fmt = sdo_find_format(std);
+        if (fmt == NULL)
+                return -EINVAL;
+        sdev->fmt = fmt;
+        return 0;
+}
+
+static int sdo_g_std_output(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+        *std = sd_to_sdev(sd)->fmt->id;
+        return 0;
+}
+
+static int sdo_g_mbus_fmt(struct v4l2_subdev *sd,
+        struct v4l2_mbus_framefmt *fmt)
+{
+        struct sdo_device *sdev = sd_to_sdev(sd);
+
+        if (!sdev->fmt)
+                return -ENXIO;
+        /* all modes are 720 pixels wide */
+        fmt->width = 720;
+        fmt->height = sdev->fmt->height;
+        fmt->code = V4L2_MBUS_FMT_FIXED;
+        fmt->field = V4L2_FIELD_INTERLACED;
+        return 0;
+}
+
+static int sdo_s_power(struct v4l2_subdev *sd, int on)
+{
+        struct sdo_device *sdev = sd_to_sdev(sd);
+        struct device *dev = sdev->dev;
+        int ret;
+
+        dev_info(dev, "sdo_s_power(%d)\n", on);
+
+        if (on)
+                ret = pm_runtime_get_sync(dev);
+        else
+                ret = pm_runtime_put_sync(dev);
+
+        /* only values < 0 indicate errors */
+        return IS_ERR_VALUE(ret) ? ret : 0;
+}
+
+static int sdo_streamon(struct sdo_device *sdev)
+{
+        /* set proper clock for Timing Generator */
+        clk_set_rate(sdev->fout_vpll, 54000000);
+        dev_info(sdev->dev, "fout_vpll.rate = %lu\n",
+        clk_get_rate(sdev->fout_vpll));
+        /* enable clock in SDO */
+        sdo_write_mask(sdev, SDO_CLKCON, ~0, SDO_TVOUT_CLOCK_ON);
+        clk_enable(sdev->dacphy);
+        /* enable DAC */
+        sdo_write_mask(sdev, SDO_DAC, ~0, SDO_POWER_ON_DAC);
+        sdo_reg_debug(sdev);
+        return 0;
+}
+
+static int sdo_streamoff(struct sdo_device *sdev)
+{
+        int tries;
+
+        sdo_write_mask(sdev, SDO_DAC, 0, SDO_POWER_ON_DAC);
+        clk_disable(sdev->dacphy);
+        sdo_write_mask(sdev, SDO_CLKCON, 0, SDO_TVOUT_CLOCK_ON);
+        for (tries = 100; tries; --tries) {
+                if (sdo_read(sdev, SDO_CLKCON) & SDO_TVOUT_CLOCK_READY)
+                        break;
+                mdelay(1);
+        }
+        if (tries == 0)
+                dev_err(sdev->dev, "failed to stop streaming\n");
+        return tries ? 0 : -EIO;
+}
+
+static int sdo_s_stream(struct v4l2_subdev *sd, int on)
+{
+        struct sdo_device *sdev = sd_to_sdev(sd);
+        return on ? sdo_streamon(sdev) : sdo_streamoff(sdev);
+}
+
+static const struct v4l2_subdev_core_ops sdo_sd_core_ops = {
+        .s_power = sdo_s_power,
+};
+
+static const struct v4l2_subdev_video_ops sdo_sd_video_ops = {
+        .s_std_output = sdo_s_std_output,
+        .g_std_output = sdo_g_std_output,
+        .g_tvnorms_output = sdo_g_tvnorms_output,
+        .g_mbus_fmt = sdo_g_mbus_fmt,
+        .s_stream = sdo_s_stream,
+};
+
+static const struct v4l2_subdev_ops sdo_sd_ops = {
+        .core = &sdo_sd_core_ops,
+        .video = &sdo_sd_video_ops,
+};
+
+static int sdo_runtime_suspend(struct device *dev)
+{
+        struct v4l2_subdev *sd = dev_get_drvdata(dev);
+        struct sdo_device *sdev = sd_to_sdev(sd);
+
+        dev_info(dev, "suspend\n");
+        regulator_disable(sdev->vdet);
+        regulator_disable(sdev->vdac);
+        clk_disable(sdev->sclk_dac);
+        return 0;
+}
+
+static int sdo_runtime_resume(struct device *dev)
+{
+        struct v4l2_subdev *sd = dev_get_drvdata(dev);
+        struct sdo_device *sdev = sd_to_sdev(sd);
+
+        dev_info(dev, "resume\n");
+        clk_enable(sdev->sclk_dac);
+        regulator_enable(sdev->vdac);
+        regulator_enable(sdev->vdet);
+
+        /* software reset */
+        sdo_write_mask(sdev, SDO_CLKCON, ~0, SDO_TVOUT_SW_RESET);
+        mdelay(10);
+        sdo_write_mask(sdev, SDO_CLKCON, 0, SDO_TVOUT_SW_RESET);
+
+        /* setting TV mode */
+        sdo_write_mask(sdev, SDO_CONFIG, sdev->fmt->cookie, SDO_STANDARD_MASK);
+        /* XXX: forcing interlaced mode using undocumented bit */
+        sdo_write_mask(sdev, SDO_CONFIG, 0, SDO_PROGRESSIVE);
+        /* turn all VBI off */
+        sdo_write_mask(sdev, SDO_VBI, 0, SDO_CVBS_WSS_INS |
+                SDO_CVBS_CLOSED_CAPTION_MASK);
+        /* turn all post processing off */
+        sdo_write_mask(sdev, SDO_CCCON, ~0, SDO_COMPENSATION_BHS_ADJ_OFF |
+                SDO_COMPENSATION_CVBS_COMP_OFF);
+        sdo_reg_debug(sdev);
+        return 0;
+}
+
+static const struct dev_pm_ops sdo_pm_ops = {
+        .runtime_suspend = sdo_runtime_suspend,
+        .runtime_resume  = sdo_runtime_resume,
+};
+
+static int __devinit sdo_probe(struct platform_device *pdev)
+{
+        struct device *dev = &pdev->dev;
+        struct sdo_device *sdev;
+        struct resource *res;
+        int ret = 0;
+        struct clk *sclk_vpll;
+
+        dev_info(dev, "probe start\n");
+        sdev = kzalloc(sizeof *sdev, GFP_KERNEL);
+        if (!sdev) {
+                dev_err(dev, "not enough memory.\n");
+                ret = -ENOMEM;
+                goto fail;
+        }
+        sdev->dev = dev;
+
+        /* mapping registers */
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (res == NULL) {
+                dev_err(dev, "get memory resource failed.\n");
+                ret = -ENXIO;
+                goto fail_sdev;
+        }
+
+        sdev->regs = ioremap(res->start, resource_size(res));
+        if (sdev->regs == NULL) {
+                dev_err(dev, "register mapping failed.\n");
+                ret = -ENXIO;
+                goto fail_sdev;
+        }
+
+        /* acquiring interrupt */
+        res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+        if (res == NULL) {
+                dev_err(dev, "get interrupt resource failed.\n");
+                ret = -ENXIO;
+                goto fail_regs;
+        }
+        ret = request_irq(res->start, sdo_irq_handler, 0, "s5p-sdo", sdev);
+        if (ret) {
+                dev_err(dev, "request interrupt failed.\n");
+                goto fail_regs;
+        }
+        sdev->irq = res->start;
+
+        /* acquire clocks */
+        sdev->sclk_dac = clk_get(dev, "sclk_dac");
+        if (IS_ERR_OR_NULL(sdev->sclk_dac)) {
+                dev_err(dev, "failed to get clock 'sclk_dac'\n");
+                ret = -ENXIO;
+                goto fail_irq;
+        }
+        sdev->dac = clk_get(dev, "dac");
+        if (IS_ERR_OR_NULL(sdev->dac)) {
+                dev_err(dev, "failed to get clock 'dac'\n");
+                ret = -ENXIO;
+                goto fail_sclk_dac;
+        }
+        sdev->dacphy = clk_get(dev, "dacphy");
+        if (IS_ERR_OR_NULL(sdev->dacphy)) {
+                dev_err(dev, "failed to get clock 'dacphy'\n");
+                ret = -ENXIO;
+                goto fail_dac;
+        }
+        sclk_vpll = clk_get(dev, "sclk_vpll");
+        if (IS_ERR_OR_NULL(sclk_vpll)) {
+                dev_err(dev, "failed to get clock 'sclk_vpll'\n");
+                ret = -ENXIO;
+                goto fail_dacphy;
+        }
+        clk_set_parent(sdev->sclk_dac, sclk_vpll);
+        clk_put(sclk_vpll);
+        sdev->fout_vpll = clk_get(dev, "fout_vpll");
+        if (IS_ERR_OR_NULL(sdev->fout_vpll)) {
+                dev_err(dev, "failed to get clock 'fout_vpll'\n");
+                goto fail_dacphy;
+        }
+        dev_info(dev, "fout_vpll.rate = %lu\n", clk_get_rate(sclk_vpll));
+
+        /* acquire regulator */
+        sdev->vdac = regulator_get(dev, "vdd33a_dac");
+        if (IS_ERR_OR_NULL(sdev->vdac)) {
+                dev_err(dev, "failed to get regulator 'vdac'\n");
+                goto fail_fout_vpll;
+        }
+        sdev->vdet = regulator_get(dev, "vdet");
+        if (IS_ERR_OR_NULL(sdev->vdet)) {
+                dev_err(dev, "failed to get regulator 'vdet'\n");
+                goto fail_vdac;
+        }
+
+        /* enable gate for dac clock, because mixer uses it */
+        clk_enable(sdev->dac);
+
+        /* configure power management */
+        pm_runtime_enable(dev);
+
+        /* configuration of interface subdevice */
+        v4l2_subdev_init(&sdev->sd, &sdo_sd_ops);
+        sdev->sd.owner = THIS_MODULE;
+        strlcpy(sdev->sd.name, "s5p-sdo", sizeof sdev->sd.name);
+
+        /* set default format */
+        sdev->fmt = sdo_find_format(SDO_DEFAULT_STD);
+        BUG_ON(sdev->fmt == NULL);
+
+        /* keeping subdev in device's private for use by other drivers */
+        dev_set_drvdata(dev, &sdev->sd);
+
+        dev_info(dev, "probe succeeded\n");
+        return 0;
+
+fail_vdac:
+        regulator_put(sdev->vdac);
+fail_fout_vpll:
+        clk_put(sdev->fout_vpll);
+fail_dacphy:
+        clk_put(sdev->dacphy);
+fail_dac:
+        clk_put(sdev->dac);
+fail_sclk_dac:
+        clk_put(sdev->sclk_dac);
+fail_irq:
+        free_irq(sdev->irq, sdev);
+fail_regs:
+        iounmap(sdev->regs);
+fail_sdev:
+        kfree(sdev);
+fail:
+        dev_info(dev, "probe failed\n");
+        return ret;
+}
+
+static int __devexit sdo_remove(struct platform_device *pdev)
+{
+        struct v4l2_subdev *sd = dev_get_drvdata(&pdev->dev);
+        struct sdo_device *sdev = sd_to_sdev(sd);
+
+        pm_runtime_disable(&pdev->dev);
+        clk_disable(sdev->dac);
+        regulator_put(sdev->vdet);
+        regulator_put(sdev->vdac);
+        clk_put(sdev->fout_vpll);
+        clk_put(sdev->dacphy);
+        clk_put(sdev->dac);
+        clk_put(sdev->sclk_dac);
+        free_irq(sdev->irq, sdev);
+        iounmap(sdev->regs);
+        kfree(sdev);
+
+        dev_info(&pdev->dev, "remove successful\n");
+        return 0;
+}
+
+static struct platform_driver sdo_driver __refdata = {
+        .probe = sdo_probe,
+        .remove = __devexit_p(sdo_remove),
+        .driver = {
+                .name = "s5p-sdo",
+                .owner = THIS_MODULE,
+                .pm = &sdo_pm_ops,
+        }
+};
+
+static int __init sdo_init(void)
+{
+        int ret;
+        static const char banner[] __initdata = KERN_INFO \
+                "Samsung Standard Definition Output (SDO) driver, "
+                "(c) 2010-2011 Samsung Electronics Co., Ltd.\n";
+        printk(banner);
+
+        ret = platform_driver_register(&sdo_driver);
+        if (ret)
+                printk(KERN_ERR "SDO platform driver register failed\n");
+
+        return ret;
+}
+module_init(sdo_init);
+
+static void __exit sdo_exit(void)
+{
+        platform_driver_unregister(&sdo_driver);
+}
+module_exit(sdo_exit);
diff --git a/drivers/media/video/saa7115.c b/drivers/media/video/saa7115.c
index 0db90922ee9..f2ae405c74a 100644
--- a/drivers/media/video/saa7115.c
+++ b/drivers/media/video/saa7115.c
@@ -757,8 +757,8 @@ static int saa711x_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
         switch (ctrl->id) {
         case V4L2_CID_CHROMA_AGC:
                 /* chroma gain cluster */
-                if (state->agc->cur.val)
-                        state->gain->cur.val =
+                if (state->agc->val)
+                        state->gain->val =
                                 saa711x_read(sd, R_0F_CHROMA_GAIN_CNTL) & 0x7f;
                 break;
         }
diff --git a/drivers/media/video/saa7134/saa7134-cards.c b/drivers/media/video/saa7134/saa7134-cards.c
index e2062b240e3..0f9fb99adeb 100644
--- a/drivers/media/video/saa7134/saa7134-cards.c
+++ b/drivers/media/video/saa7134/saa7134-cards.c
@@ -4951,8 +4951,9 @@ struct saa7134_board saa7134_boards[] = {
                 .audio_clock    = 0x00187de7,
                 .tuner_type     = TUNER_XC2028,
                 .radio_type     = UNSET,
-                .tuner_addr     = ADDR_UNSET,
+                .tuner_addr     = 0x61,
                 .radio_addr     = ADDR_UNSET,
+                .mpeg           = SAA7134_MPEG_DVB,
                 .inputs = {{
                         .name   = name_tv,
                         .vmux   = 3,
@@ -6992,6 +6993,11 @@ static int saa7134_xc2028_callback(struct saa7134_dev *dev,
                         msleep(10);
                         saa7134_set_gpio(dev, 18, 1);
                 break;
+                case SAA7134_BOARD_VIDEOMATE_T750:
+                        saa7134_set_gpio(dev, 20, 0);
+                        msleep(10);
+                        saa7134_set_gpio(dev, 20, 1);
+                break;
                 }
         return 0;
         }
@@ -7451,6 +7457,11 @@ int saa7134_board_init1(struct saa7134_dev *dev)
                 saa_andorl(SAA7134_GPIO_GPMODE0 >> 2,   0x0e050000, 0x0c050000);
                 saa_andorl(SAA7134_GPIO_GPSTATUS0 >> 2, 0x0e050000, 0x0c050000);
                 break;
+        case SAA7134_BOARD_VIDEOMATE_T750:
+                /* enable the analog tuner */
+                saa_andorl(SAA7134_GPIO_GPMODE0 >> 2,   0x00008000, 0x00008000);
+                saa_andorl(SAA7134_GPIO_GPSTATUS0 >> 2, 0x00008000, 0x00008000);
+                break;
         }
         return 0;
 }
diff --git a/drivers/media/video/saa7134/saa7134-core.c b/drivers/media/video/saa7134/saa7134-core.c
index f9be737ba6f..ca65cda3e10 100644
--- a/drivers/media/video/saa7134/saa7134-core.c
+++ b/drivers/media/video/saa7134/saa7134-core.c
@@ -39,6 +39,8 @@
 MODULE_DESCRIPTION("v4l2 driver module for saa7130/34 based TV cards");
 MODULE_AUTHOR("Gerd Knorr <kraxel@bytesex.org> [SuSE Labs]");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(SAA7134_VERSION);
+
 
 /* ------------------------------------------------------------------ */
 
@@ -1332,14 +1334,8 @@ static struct pci_driver saa7134_pci_driver = {
 static int __init saa7134_init(void)
 {
         INIT_LIST_HEAD(&saa7134_devlist);
-        printk(KERN_INFO "saa7130/34: v4l2 driver version %d.%d.%d loaded\n",
-               (SAA7134_VERSION_CODE >> 16) & 0xff,
-               (SAA7134_VERSION_CODE >>  8) & 0xff,
-               SAA7134_VERSION_CODE & 0xff);
-#ifdef SNAPSHOT
-        printk(KERN_INFO "saa7130/34: snapshot date %04d-%02d-%02d\n",
-               SNAPSHOT/10000, (SNAPSHOT/100)%100, SNAPSHOT%100);
-#endif
+        printk(KERN_INFO "saa7130/34: v4l2 driver version %s loaded\n",
+               SAA7134_VERSION);
         return pci_register_driver(&saa7134_pci_driver);
 }
 
diff --git a/drivers/media/video/saa7134/saa7134-dvb.c b/drivers/media/video/saa7134/saa7134-dvb.c
index 996a206c6d7..1e4ef166988 100644
--- a/drivers/media/video/saa7134/saa7134-dvb.c
+++ b/drivers/media/video/saa7134/saa7134-dvb.c
@@ -56,6 +56,7 @@
 #include "lgs8gxx.h"
 
 #include "zl10353.h"
+#include "qt1010.h"
 
 #include "zl10036.h"
 #include "zl10039.h"
@@ -939,6 +940,18 @@ static struct zl10353_config behold_x7_config = {
         .disable_i2c_gate_ctrl = 1,
 };
 
+static struct zl10353_config videomate_t750_zl10353_config = {
+        .demod_address         = 0x0f,
+        .no_tuner              = 1,
+        .parallel_ts           = 1,
+        .disable_i2c_gate_ctrl = 1,
+};
+
+static struct qt1010_config videomate_t750_qt1010_config = {
+        .i2c_address = 0x62
+};
+
+
 /* ==================================================================
  * tda10086 based DVB-S cards, helper functions
  */
@@ -1650,6 +1663,18 @@ static int dvb_init(struct saa7134_dev *dev)
                                         __func__);
 
                 break;
+        case SAA7134_BOARD_VIDEOMATE_T750:
+                fe0->dvb.frontend = dvb_attach(zl10353_attach,
+                                                &videomate_t750_zl10353_config,
+                                                &dev->i2c_adap);
+                if (fe0->dvb.frontend != NULL) {
+                        if (dvb_attach(qt1010_attach,
+                                        fe0->dvb.frontend,
+                                        &dev->i2c_adap,
+                                        &videomate_t750_qt1010_config) == NULL)
+                                wprintk("error attaching QT1010\n");
+                }
+                break;
         case SAA7134_BOARD_ZOLID_HYBRID_PCI:
                 fe0->dvb.frontend = dvb_attach(tda10048_attach,
                                                &zolid_tda10048_config,
diff --git a/drivers/media/video/saa7134/saa7134-empress.c b/drivers/media/video/saa7134/saa7134-empress.c
index 18294db38a0..dde361a9194 100644
--- a/drivers/media/video/saa7134/saa7134-empress.c
+++ b/drivers/media/video/saa7134/saa7134-empress.c
@@ -172,7 +172,6 @@ static int empress_querycap(struct file *file, void  *priv,
         strlcpy(cap->card, saa7134_boards[dev->board].name,
                 sizeof(cap->card));
         sprintf(cap->bus_info, "PCI:%s", pci_name(dev->pci));
-        cap->version = SAA7134_VERSION_CODE;
         cap->capabilities =
                 V4L2_CAP_VIDEO_CAPTURE |
                 V4L2_CAP_READWRITE |
diff --git a/drivers/media/video/saa7134/saa7134-video.c b/drivers/media/video/saa7134/saa7134-video.c
index 776ba2dd7f9..9cf7914f6f9 100644
--- a/drivers/media/video/saa7134/saa7134-video.c
+++ b/drivers/media/video/saa7134/saa7134-video.c
@@ -1810,7 +1810,6 @@ static int saa7134_querycap(struct file *file, void  *priv,
         strlcpy(cap->card, saa7134_boards[dev->board].name,
                 sizeof(cap->card));
         sprintf(cap->bus_info, "PCI:%s", pci_name(dev->pci));
-        cap->version = SAA7134_VERSION_CODE;
         cap->capabilities =
                 V4L2_CAP_VIDEO_CAPTURE |
                 V4L2_CAP_VBI_CAPTURE |
@@ -2307,7 +2306,6 @@ static int radio_querycap(struct file *file, void *priv,
         strcpy(cap->driver, "saa7134");
         strlcpy(cap->card, saa7134_boards[dev->board].name, sizeof(cap->card));
         sprintf(cap->bus_info, "PCI:%s", pci_name(dev->pci));
-        cap->version = SAA7134_VERSION_CODE;
         cap->capabilities = V4L2_CAP_TUNER;
         return 0;
 }
diff --git a/drivers/media/video/saa7134/saa7134.h b/drivers/media/video/saa7134/saa7134.h
index 28eb1039832..bc8d6bba8ee 100644
--- a/drivers/media/video/saa7134/saa7134.h
+++ b/drivers/media/video/saa7134/saa7134.h
@@ -19,8 +19,7 @@
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
-#include <linux/version.h>
-#define SAA7134_VERSION_CODE KERNEL_VERSION(0, 2, 16)
+#define SAA7134_VERSION "0, 2, 17"
 
 #include <linux/pci.h>
 #include <linux/i2c.h>
diff --git a/drivers/media/video/saa7164/saa7164-encoder.c b/drivers/media/video/saa7164/saa7164-encoder.c
index 400364569c8..2fd38a01887 100644
--- a/drivers/media/video/saa7164/saa7164-encoder.c
+++ b/drivers/media/video/saa7164/saa7164-encoder.c
@@ -1246,7 +1246,6 @@ static unsigned int fops_poll(struct file *file, poll_table *wait)
         struct saa7164_encoder_fh *fh =
                 (struct saa7164_encoder_fh *)file->private_data;
         struct saa7164_port *port = fh->port;
-        struct saa7164_user_buffer *ubuf;
         unsigned int mask = 0;
 
         port->last_poll_msecs_diff = port->last_poll_msecs;
@@ -1278,10 +1277,7 @@ static unsigned int fops_poll(struct file *file, poll_table *wait)
         }
 
         /* Pull the first buffer from the used list */
-        ubuf = list_first_entry(&port->list_buf_used.list,
-                struct saa7164_user_buffer, list);
-
-        if (ubuf)
+        if (!list_empty(&port->list_buf_used.list))
                 mask |= POLLIN | POLLRDNORM;
 
         return mask;
diff --git a/drivers/media/video/saa7164/saa7164-vbi.c b/drivers/media/video/saa7164/saa7164-vbi.c
index bc1fcedba87..e2e03415871 100644
--- a/drivers/media/video/saa7164/saa7164-vbi.c
+++ b/drivers/media/video/saa7164/saa7164-vbi.c
@@ -1192,7 +1192,6 @@ static unsigned int fops_poll(struct file *file, poll_table *wait)
 {
         struct saa7164_vbi_fh *fh = (struct saa7164_vbi_fh *)file->private_data;
         struct saa7164_port *port = fh->port;
-        struct saa7164_user_buffer *ubuf;
         unsigned int mask = 0;
 
         port->last_poll_msecs_diff = port->last_poll_msecs;
@@ -1224,10 +1223,7 @@ static unsigned int fops_poll(struct file *file, poll_table *wait)
         }
 
         /* Pull the first buffer from the used list */
-        ubuf = list_first_entry(&port->list_buf_used.list,
-                struct saa7164_user_buffer, list);
-
-        if (ubuf)
+        if (!list_empty(&port->list_buf_used.list))
                 mask |= POLLIN | POLLRDNORM;
 
         return mask;
diff --git a/drivers/media/video/saa7164/saa7164.h b/drivers/media/video/saa7164/saa7164.h
index 16745d2fb34..6678bf1e781 100644
--- a/drivers/media/video/saa7164/saa7164.h
+++ b/drivers/media/video/saa7164/saa7164.h
@@ -48,7 +48,6 @@
 #include <linux/i2c.h>
 #include <linux/i2c-algo-bit.h>
 #include <linux/kdev_t.h>
-#include <linux/version.h>
 #include <linux/mutex.h>
 #include <linux/crc32.h>
 #include <linux/kthread.h>
diff --git a/drivers/media/video/sh_mobile_ceu_camera.c b/drivers/media/video/sh_mobile_ceu_camera.c
index 3ae5c9c58cb..e54089802b6 100644
--- a/drivers/media/video/sh_mobile_ceu_camera.c
+++ b/drivers/media/video/sh_mobile_ceu_camera.c
@@ -27,7 +27,6 @@
 #include <linux/mm.h>
 #include <linux/moduleparam.h>
 #include <linux/time.h>
-#include <linux/version.h>
 #include <linux/slab.h>
 #include <linux/device.h>
 #include <linux/platform_device.h>
@@ -39,6 +38,7 @@
 #include <media/v4l2-dev.h>
 #include <media/soc_camera.h>
 #include <media/sh_mobile_ceu.h>
+#include <media/sh_mobile_csi2.h>
 #include <media/videobuf2-dma-contig.h>
 #include <media/v4l2-mediabus.h>
 #include <media/soc_mediabus.h>
@@ -96,6 +96,7 @@ struct sh_mobile_ceu_buffer {
 struct sh_mobile_ceu_dev {
         struct soc_camera_host ici;
         struct soc_camera_device *icd;
+        struct platform_device *csi2_pdev;
 
         unsigned int irq;
         void __iomem *base;
@@ -205,7 +206,7 @@ static int sh_mobile_ceu_soft_reset(struct sh_mobile_ceu_dev *pcdev)
 
 
         if (2 != success) {
-                dev_warn(&icd->dev, "soft reset time out\n");
+                dev_warn(icd->pdev, "soft reset time out\n");
                 return -EIO;
         }
 
@@ -220,7 +221,7 @@ static int sh_mobile_ceu_videobuf_setup(struct vb2_queue *vq,
                         unsigned long sizes[], void *alloc_ctxs[])
 {
         struct soc_camera_device *icd = container_of(vq, struct soc_camera_device, vb2_vidq);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
                                                 icd->current_fmt->host_fmt);
@@ -242,7 +243,7 @@ static int sh_mobile_ceu_videobuf_setup(struct vb2_queue *vq,
                         *count = pcdev->video_limit / PAGE_ALIGN(sizes[0]);
         }
 
-        dev_dbg(icd->dev.parent, "count=%d, size=%lu\n", *count, sizes[0]);
+        dev_dbg(icd->parent, "count=%d, size=%lu\n", *count, sizes[0]);
 
         return 0;
 }
@@ -351,7 +352,7 @@ static int sh_mobile_ceu_videobuf_prepare(struct vb2_buffer *vb)
 
         buf = to_ceu_vb(vb);
 
-        dev_dbg(icd->dev.parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
+        dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
                 vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
 
         /* Added list head initialization on alloc */
@@ -371,7 +372,7 @@ static int sh_mobile_ceu_videobuf_prepare(struct vb2_buffer *vb)
         size = icd->user_height * bytes_per_line;
 
         if (vb2_plane_size(vb, 0) < size) {
-                dev_err(icd->dev.parent, "Buffer too small (%lu < %lu)\n",
+                dev_err(icd->parent, "Buffer too small (%lu < %lu)\n",
                         vb2_plane_size(vb, 0), size);
                 return -ENOBUFS;
         }
@@ -384,11 +385,11 @@ static int sh_mobile_ceu_videobuf_prepare(struct vb2_buffer *vb)
 static void sh_mobile_ceu_videobuf_queue(struct vb2_buffer *vb)
 {
         struct soc_camera_device *icd = container_of(vb->vb2_queue, struct soc_camera_device, vb2_vidq);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         struct sh_mobile_ceu_buffer *buf = to_ceu_vb(vb);
 
-        dev_dbg(icd->dev.parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
+        dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
                 vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
 
         spin_lock_irq(&pcdev->lock);
@@ -409,7 +410,7 @@ static void sh_mobile_ceu_videobuf_queue(struct vb2_buffer *vb)
 static void sh_mobile_ceu_videobuf_release(struct vb2_buffer *vb)
 {
         struct soc_camera_device *icd = container_of(vb->vb2_queue, struct soc_camera_device, vb2_vidq);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_buffer *buf = to_ceu_vb(vb);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
@@ -421,8 +422,12 @@ static void sh_mobile_ceu_videobuf_release(struct vb2_buffer *vb)
                 pcdev->active = NULL;
         }
 
-        /* Doesn't hurt also if the list is empty */
-        list_del_init(&buf->queue);
+        /*
+         * Doesn't hurt also if the list is empty, but it hurts, if queuing the
+         * buffer failed, and .buf_init() hasn't been called
+         */
+        if (buf->queue.next)
+                list_del_init(&buf->queue);
 
         spin_unlock_irq(&pcdev->lock);
 }
@@ -437,7 +442,7 @@ static int sh_mobile_ceu_videobuf_init(struct vb2_buffer *vb)
 static int sh_mobile_ceu_stop_streaming(struct vb2_queue *q)
 {
         struct soc_camera_device *icd = container_of(q, struct soc_camera_device, vb2_vidq);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         struct list_head *buf_head, *tmp;
 
@@ -499,25 +504,48 @@ out:
         return IRQ_HANDLED;
 }
 
+static struct v4l2_subdev *find_csi2(struct sh_mobile_ceu_dev *pcdev)
+{
+        struct v4l2_subdev *sd;
+
+        if (!pcdev->csi2_pdev)
+                return NULL;
+
+        v4l2_device_for_each_subdev(sd, &pcdev->ici.v4l2_dev)
+                if (&pcdev->csi2_pdev->dev == v4l2_get_subdevdata(sd))
+                        return sd;
+
+        return NULL;
+}
+
 /* Called with .video_lock held */
 static int sh_mobile_ceu_add_device(struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
+        struct v4l2_subdev *csi2_sd;
         int ret;
 
         if (pcdev->icd)
                 return -EBUSY;
 
-        dev_info(icd->dev.parent,
+        dev_info(icd->parent,
                  "SuperH Mobile CEU driver attached to camera %d\n",
                  icd->devnum);
 
         pm_runtime_get_sync(ici->v4l2_dev.dev);
 
         ret = sh_mobile_ceu_soft_reset(pcdev);
-        if (!ret)
+
+        csi2_sd = find_csi2(pcdev);
+
+        ret = v4l2_subdev_call(csi2_sd, core, s_power, 1);
+        if (ret != -ENODEV && ret != -ENOIOCTLCMD && ret < 0) {
+                pm_runtime_put_sync(ici->v4l2_dev.dev);
+        } else {
                 pcdev->icd = icd;
+                ret = 0;
+        }
 
         return ret;
 }
@@ -525,11 +553,13 @@ static int sh_mobile_ceu_add_device(struct soc_camera_device *icd)
 /* Called with .video_lock held */
 static void sh_mobile_ceu_remove_device(struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
+        struct v4l2_subdev *csi2_sd = find_csi2(pcdev);
 
         BUG_ON(icd != pcdev->icd);
 
+        v4l2_subdev_call(csi2_sd, core, s_power, 0);
         /* disable capture, disable interrupts */
         ceu_write(pcdev, CEIER, 0);
         sh_mobile_ceu_soft_reset(pcdev);
@@ -545,7 +575,7 @@ static void sh_mobile_ceu_remove_device(struct soc_camera_device *icd)
 
         pm_runtime_put_sync(ici->v4l2_dev.dev);
 
-        dev_info(icd->dev.parent,
+        dev_info(icd->parent,
                  "SuperH Mobile CEU driver detached from camera %d\n",
                  icd->devnum);
 
@@ -585,14 +615,14 @@ static u16 calc_scale(unsigned int src, unsigned int *dst)
 /* rect is guaranteed to not exceed the scaled camera rectangle */
 static void sh_mobile_ceu_set_rect(struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_cam *cam = icd->host_priv;
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         unsigned int height, width, cdwdr_width, in_width, in_height;
         unsigned int left_offset, top_offset;
         u32 camor;
 
-        dev_geo(icd->dev.parent, "Crop %ux%u@%u:%u\n",
+        dev_geo(icd->parent, "Crop %ux%u@%u:%u\n",
                 icd->user_width, icd->user_height, cam->ceu_left, cam->ceu_top);
 
         left_offset     = cam->ceu_left;
@@ -641,7 +671,7 @@ static void sh_mobile_ceu_set_rect(struct soc_camera_device *icd)
         }
 
         /* CSI2 special configuration */
-        if (pcdev->pdata->csi2_dev) {
+        if (pcdev->pdata->csi2) {
                 in_width = ((in_width - 2) * 2);
                 left_offset *= 2;
         }
@@ -649,7 +679,7 @@ static void sh_mobile_ceu_set_rect(struct soc_camera_device *icd)
         /* Set CAMOR, CAPWR, CFSZR, take care of CDWDR */
         camor = left_offset | (top_offset << 16);
 
-        dev_geo(icd->dev.parent,
+        dev_geo(icd->parent,
                 "CAMOR 0x%x, CAPWR 0x%x, CFSZR 0x%x, CDWDR 0x%x\n", camor,
                 (in_height << 16) | in_width, (height << 16) | width,
                 cdwdr_width);
@@ -697,7 +727,7 @@ static void capture_restore(struct sh_mobile_ceu_dev *pcdev, u32 capsr)
 static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd,
                                        __u32 pixfmt)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         int ret;
         unsigned long camera_flags, common_flags, value;
@@ -783,7 +813,7 @@ static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd,
         value |= pcdev->is_16bit ? 1 << 12 : 0;
 
         /* CSI2 mode */
-        if (pcdev->pdata->csi2_dev)
+        if (pcdev->pdata->csi2)
                 value |= 3 << 12;
 
         ceu_write(pcdev, CAMCR, value);
@@ -806,7 +836,7 @@ static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd,
         sh_mobile_ceu_set_rect(icd);
         mdelay(1);
 
-        dev_geo(icd->dev.parent, "CFLCR 0x%x\n", pcdev->cflcr);
+        dev_geo(icd->parent, "CFLCR 0x%x\n", pcdev->cflcr);
         ceu_write(pcdev, CFLCR, pcdev->cflcr);
 
         /*
@@ -829,7 +859,7 @@ static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd,
         ceu_write(pcdev, CDOCR, value);
         ceu_write(pcdev, CFWCR, 0); /* keep "datafetch firewall" disabled */
 
-        dev_dbg(icd->dev.parent, "S_FMT successful for %c%c%c%c %ux%u\n",
+        dev_dbg(icd->parent, "S_FMT successful for %c%c%c%c %ux%u\n",
                 pixfmt & 0xff, (pixfmt >> 8) & 0xff,
                 (pixfmt >> 16) & 0xff, (pixfmt >> 24) & 0xff,
                 icd->user_width, icd->user_height);
@@ -843,7 +873,7 @@ static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd,
 static int sh_mobile_ceu_try_bus_param(struct soc_camera_device *icd,
                                        unsigned char buswidth)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         unsigned long camera_flags, common_flags;
 
@@ -901,7 +931,7 @@ static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int
                                      struct soc_camera_format_xlate *xlate)
 {
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        struct device *dev = icd->dev.parent;
+        struct device *dev = icd->parent;
         struct soc_camera_host *ici = to_soc_camera_host(dev);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         int ret, k, n;
@@ -921,7 +951,7 @@ static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int
                 return 0;
         }
 
-        if (!pcdev->pdata->csi2_dev) {
+        if (!pcdev->pdata->csi2) {
                 ret = sh_mobile_ceu_try_bus_param(icd, fmt->bits_per_sample);
                 if (ret < 0)
                         return 0;
@@ -1244,7 +1274,7 @@ static int client_s_fmt(struct soc_camera_device *icd,
 {
         struct sh_mobile_ceu_cam *cam = icd->host_priv;
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        struct device *dev = icd->dev.parent;
+        struct device *dev = icd->parent;
         unsigned int width = mf->width, height = mf->height, tmp_w, tmp_h;
         unsigned int max_width, max_height;
         struct v4l2_cropcap cap;
@@ -1313,7 +1343,7 @@ static int client_scale(struct soc_camera_device *icd,
                         bool ceu_can_scale)
 {
         struct sh_mobile_ceu_cam *cam = icd->host_priv;
-        struct device *dev = icd->dev.parent;
+        struct device *dev = icd->parent;
         struct v4l2_mbus_framefmt mf_tmp = *mf;
         unsigned int scale_h, scale_v;
         int ret;
@@ -1363,13 +1393,13 @@ static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
                                   struct v4l2_crop *a)
 {
         struct v4l2_rect *rect = &a->c;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct device *dev = icd->parent;
+        struct soc_camera_host *ici = to_soc_camera_host(dev);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         struct v4l2_crop cam_crop;
         struct sh_mobile_ceu_cam *cam = icd->host_priv;
         struct v4l2_rect *cam_rect = &cam_crop.c;
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        struct device *dev = icd->dev.parent;
         struct v4l2_mbus_framefmt mf;
         unsigned int scale_cam_h, scale_cam_v, scale_ceu_h, scale_ceu_v,
                 out_width, out_height;
@@ -1511,7 +1541,7 @@ static void calculate_client_output(struct soc_camera_device *icd,
                 struct v4l2_pix_format *pix, struct v4l2_mbus_framefmt *mf)
 {
         struct sh_mobile_ceu_cam *cam = icd->host_priv;
-        struct device *dev = icd->dev.parent;
+        struct device *dev = icd->parent;
         struct v4l2_rect *cam_subrect = &cam->subrect;
         unsigned int scale_v, scale_h;
 
@@ -1555,12 +1585,12 @@ static void calculate_client_output(struct soc_camera_device *icd,
 static int sh_mobile_ceu_set_fmt(struct soc_camera_device *icd,
                                  struct v4l2_format *f)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct device *dev = icd->parent;
+        struct soc_camera_host *ici = to_soc_camera_host(dev);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         struct sh_mobile_ceu_cam *cam = icd->host_priv;
         struct v4l2_pix_format *pix = &f->fmt.pix;
         struct v4l2_mbus_framefmt mf;
-        struct device *dev = icd->dev.parent;
         __u32 pixfmt = pix->pixelformat;
         const struct soc_camera_format_xlate *xlate;
         /* Keep Compiler Happy */
@@ -1684,12 +1714,12 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
         int width, height;
         int ret;
 
-        dev_geo(icd->dev.parent, "TRY_FMT(pix=0x%x, %ux%u)\n",
+        dev_geo(icd->parent, "TRY_FMT(pix=0x%x, %ux%u)\n",
                  pixfmt, pix->width, pix->height);
 
         xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
         if (!xlate) {
-                dev_warn(icd->dev.parent, "Format %x not found\n", pixfmt);
+                dev_warn(icd->parent, "Format %x not found\n", pixfmt);
                 return -EINVAL;
         }
 
@@ -1701,11 +1731,6 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
         width = pix->width;
         height = pix->height;
 
-        pix->bytesperline = soc_mbus_bytes_per_line(width, xlate->host_fmt);
-        if ((int)pix->bytesperline < 0)
-                return pix->bytesperline;
-        pix->sizeimage = height * pix->bytesperline;
-
         /* limit to sensor capabilities */
         mf.width        = pix->width;
         mf.height       = pix->height;
@@ -1741,7 +1766,7 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
                                                          try_mbus_fmt, &mf);
                         if (ret < 0) {
                                 /* Shouldn't actually happen... */
-                                dev_err(icd->dev.parent,
+                                dev_err(icd->parent,
                                         "FIXME: client try_fmt() = %d\n", ret);
                                 return ret;
                         }
@@ -1753,7 +1778,7 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
                         pix->height = height;
         }
 
-        dev_geo(icd->dev.parent, "%s(): return %d, fmt 0x%x, %ux%u\n",
+        dev_geo(icd->parent, "%s(): return %d, fmt 0x%x, %ux%u\n",
                 __func__, ret, pix->pixelformat, pix->width, pix->height);
 
         return ret;
@@ -1763,7 +1788,7 @@ static int sh_mobile_ceu_set_livecrop(struct soc_camera_device *icd,
                                       struct v4l2_crop *a)
 {
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         u32 out_width = icd->user_width, out_height = icd->user_height;
         int ret;
@@ -1775,13 +1800,13 @@ static int sh_mobile_ceu_set_livecrop(struct soc_camera_device *icd,
         /* Stop the client */
         ret = v4l2_subdev_call(sd, video, s_stream, 0);
         if (ret < 0)
-                dev_warn(icd->dev.parent,
+                dev_warn(icd->parent,
                          "Client failed to stop the stream: %d\n", ret);
         else
                 /* Do the crop, if it fails, there's nothing more we can do */
                 sh_mobile_ceu_set_crop(icd, a);
 
-        dev_geo(icd->dev.parent, "Output after crop: %ux%u\n", icd->user_width, icd->user_height);
+        dev_geo(icd->parent, "Output after crop: %ux%u\n", icd->user_width, icd->user_height);
 
         if (icd->user_width != out_width || icd->user_height != out_height) {
                 struct v4l2_format f = {
@@ -1827,7 +1852,6 @@ static int sh_mobile_ceu_querycap(struct soc_camera_host *ici,
                                   struct v4l2_capability *cap)
 {
         strlcpy(cap->card, "SuperH_Mobile_CEU", sizeof(cap->card));
-        cap->version = KERNEL_VERSION(0, 0, 5);
         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
         return 0;
 }
@@ -1848,7 +1872,7 @@ static int sh_mobile_ceu_init_videobuf(struct vb2_queue *q,
 static int sh_mobile_ceu_get_ctrl(struct soc_camera_device *icd,
                                   struct v4l2_control *ctrl)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         u32 val;
 
@@ -1864,7 +1888,7 @@ static int sh_mobile_ceu_get_ctrl(struct soc_camera_device *icd,
 static int sh_mobile_ceu_set_ctrl(struct soc_camera_device *icd,
                                   struct v4l2_control *ctrl)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
         switch (ctrl->id) {
@@ -1950,7 +1974,7 @@ static int __devinit sh_mobile_ceu_probe(struct platform_device *pdev)
                 .completion = COMPLETION_INITIALIZER_ONSTACK(wait.completion),
                 .notifier.notifier_call = bus_notify,
         };
-        struct device *csi2;
+        struct sh_mobile_ceu_companion *csi2;
 
         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
         irq = platform_get_irq(pdev, 0);
@@ -2023,26 +2047,61 @@ static int __devinit sh_mobile_ceu_probe(struct platform_device *pdev)
         pcdev->ici.drv_name = dev_name(&pdev->dev);
         pcdev->ici.ops = &sh_mobile_ceu_host_ops;
 
+        pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+        if (IS_ERR(pcdev->alloc_ctx)) {
+                err = PTR_ERR(pcdev->alloc_ctx);
+                goto exit_free_clk;
+        }
+
+        err = soc_camera_host_register(&pcdev->ici);
+        if (err)
+                goto exit_free_ctx;
+
         /* CSI2 interfacing */
-        csi2 = pcdev->pdata->csi2_dev;
+        csi2 = pcdev->pdata->csi2;
         if (csi2) {
-                wait.dev = csi2;
+                struct platform_device *csi2_pdev =
+                        platform_device_alloc("sh-mobile-csi2", csi2->id);
+                struct sh_csi2_pdata *csi2_pdata = csi2->platform_data;
+
+                if (!csi2_pdev) {
+                        err = -ENOMEM;
+                        goto exit_host_unregister;
+                }
+
+                pcdev->csi2_pdev                = csi2_pdev;
+
+                err = platform_device_add_data(csi2_pdev, csi2_pdata, sizeof(*csi2_pdata));
+                if (err < 0)
+                        goto exit_pdev_put;
+
+                csi2_pdata                      = csi2_pdev->dev.platform_data;
+                csi2_pdata->v4l2_dev            = &pcdev->ici.v4l2_dev;
+
+                csi2_pdev->resource             = csi2->resource;
+                csi2_pdev->num_resources        = csi2->num_resources;
+
+                err = platform_device_add(csi2_pdev);
+                if (err < 0)
+                        goto exit_pdev_put;
+
+                wait.dev = &csi2_pdev->dev;
 
                 err = bus_register_notifier(&platform_bus_type, &wait.notifier);
                 if (err < 0)
-                        goto exit_free_clk;
+                        goto exit_pdev_unregister;
 
                 /*
                  * From this point the driver module will not unload, until
                  * we complete the completion.
                  */
 
-                if (!csi2->driver) {
+                if (!csi2_pdev->dev.driver) {
                         complete(&wait.completion);
                         /* Either too late, or probing failed */
                         bus_unregister_notifier(&platform_bus_type, &wait.notifier);
                         err = -ENXIO;
-                        goto exit_free_clk;
+                        goto exit_pdev_unregister;
                 }
 
                 /*
@@ -2051,34 +2110,28 @@ static int __devinit sh_mobile_ceu_probe(struct platform_device *pdev)
                  * the "owner" is safe!
                  */
 
-                err = try_module_get(csi2->driver->owner);
+                err = try_module_get(csi2_pdev->dev.driver->owner);
 
                 /* Let notifier complete, if it has been locked */
                 complete(&wait.completion);
                 bus_unregister_notifier(&platform_bus_type, &wait.notifier);
                 if (!err) {
                         err = -ENODEV;
-                        goto exit_free_clk;
+                        goto exit_pdev_unregister;
                 }
         }
 
-        pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
-        if (IS_ERR(pcdev->alloc_ctx)) {
-                err = PTR_ERR(pcdev->alloc_ctx);
-                goto exit_module_put;
-        }
-
-        err = soc_camera_host_register(&pcdev->ici);
-        if (err)
-                goto exit_free_ctx;
-
         return 0;
 
+exit_pdev_unregister:
+        platform_device_del(pcdev->csi2_pdev);
+exit_pdev_put:
+        pcdev->csi2_pdev->resource = NULL;
+        platform_device_put(pcdev->csi2_pdev);
+exit_host_unregister:
+        soc_camera_host_unregister(&pcdev->ici);
 exit_free_ctx:
         vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
-exit_module_put:
-        if (csi2 && csi2->driver)
-                module_put(csi2->driver->owner);
 exit_free_clk:
         pm_runtime_disable(&pdev->dev);
         free_irq(pcdev->irq, pcdev);
@@ -2098,7 +2151,7 @@ static int __devexit sh_mobile_ceu_remove(struct platform_device *pdev)
         struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
         struct sh_mobile_ceu_dev *pcdev = container_of(soc_host,
                                         struct sh_mobile_ceu_dev, ici);
-        struct device *csi2 = pcdev->pdata->csi2_dev;
+        struct platform_device *csi2_pdev = pcdev->csi2_pdev;
 
         soc_camera_host_unregister(soc_host);
         pm_runtime_disable(&pdev->dev);
@@ -2107,8 +2160,13 @@ static int __devexit sh_mobile_ceu_remove(struct platform_device *pdev)
                 dma_release_declared_memory(&pdev->dev);
         iounmap(pcdev->base);
         vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
-        if (csi2 && csi2->driver)
-                module_put(csi2->driver->owner);
+        if (csi2_pdev && csi2_pdev->dev.driver) {
+                struct module *csi2_drv = csi2_pdev->dev.driver->owner;
+                platform_device_del(csi2_pdev);
+                csi2_pdev->resource = NULL;
+                platform_device_put(csi2_pdev);
+                module_put(csi2_drv);
+        }
         kfree(pcdev);
 
         return 0;
@@ -2158,4 +2216,5 @@ module_exit(sh_mobile_ceu_exit);
 MODULE_DESCRIPTION("SuperH Mobile CEU driver");
 MODULE_AUTHOR("Magnus Damm");
 MODULE_LICENSE("GPL");
+MODULE_VERSION("0.0.6");
 MODULE_ALIAS("platform:sh_mobile_ceu");
diff --git a/drivers/media/video/sh_mobile_csi2.c b/drivers/media/video/sh_mobile_csi2.c
index 98b87481fa9..2893a0134c7 100644
--- a/drivers/media/video/sh_mobile_csi2.c
+++ b/drivers/media/video/sh_mobile_csi2.c
@@ -16,6 +16,7 @@
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 
+#include <media/sh_mobile_ceu.h>
 #include <media/sh_mobile_csi2.h>
 #include <media/soc_camera.h>
 #include <media/v4l2-common.h>
@@ -33,7 +34,6 @@
 struct sh_csi2 {
         struct v4l2_subdev              subdev;
         struct list_head                list;
-        struct notifier_block           notifier;
         unsigned int                    irq;
         void __iomem                    *base;
         struct platform_device          *pdev;
@@ -132,13 +132,6 @@ static struct v4l2_subdev_video_ops sh_csi2_subdev_video_ops = {
         .try_mbus_fmt   = sh_csi2_try_fmt,
 };
 
-static struct v4l2_subdev_core_ops sh_csi2_subdev_core_ops;
-
-static struct v4l2_subdev_ops sh_csi2_subdev_ops = {
-        .core   = &sh_csi2_subdev_core_ops,
-        .video  = &sh_csi2_subdev_video_ops,
-};
-
 static void sh_csi2_hwinit(struct sh_csi2 *priv)
 {
         struct sh_csi2_pdata *pdata = priv->pdev->dev.platform_data;
@@ -186,65 +179,84 @@ static unsigned long sh_csi2_query_bus_param(struct soc_camera_device *icd)
         return soc_camera_apply_sensor_flags(icl, flags);
 }
 
-static int sh_csi2_notify(struct notifier_block *nb,
-                          unsigned long action, void *data)
+static int sh_csi2_client_connect(struct sh_csi2 *priv)
 {
-        struct device *dev = data;
-        struct soc_camera_device *icd = to_soc_camera_dev(dev);
-        struct v4l2_device *v4l2_dev = dev_get_drvdata(dev->parent);
-        struct sh_csi2 *priv =
-                container_of(nb, struct sh_csi2, notifier);
         struct sh_csi2_pdata *pdata = priv->pdev->dev.platform_data;
-        int ret, i;
+        struct v4l2_subdev *sd, *csi2_sd = &priv->subdev;
+        struct soc_camera_device *icd = NULL;
+        struct device *dev = v4l2_get_subdevdata(&priv->subdev);
+        int i;
+
+        v4l2_device_for_each_subdev(sd, csi2_sd->v4l2_dev)
+                if (sd->grp_id) {
+                        icd = (struct soc_camera_device *)sd->grp_id;
+                        break;
+                }
+
+        if (!icd)
+                return -EINVAL;
 
         for (i = 0; i < pdata->num_clients; i++)
                 if (&pdata->clients[i].pdev->dev == icd->pdev)
                         break;
 
-        dev_dbg(dev, "%s(%p): action = %lu, found #%d\n", __func__, dev, action, i);
+        dev_dbg(dev, "%s(%p): found #%d\n", __func__, dev, i);
 
         if (i == pdata->num_clients)
-                return NOTIFY_DONE;
+                return -ENODEV;
 
-        switch (action) {
-        case BUS_NOTIFY_BOUND_DRIVER:
-                snprintf(priv->subdev.name, V4L2_SUBDEV_NAME_SIZE, "%s%s",
-                         dev_name(v4l2_dev->dev), ".mipi-csi");
-                priv->subdev.grp_id = (long)icd;
-                ret = v4l2_device_register_subdev(v4l2_dev, &priv->subdev);
-                dev_dbg(dev, "%s(%p): ret(register_subdev) = %d\n", __func__, priv, ret);
-                if (ret < 0)
-                        return NOTIFY_DONE;
+        priv->client = pdata->clients + i;
 
-                priv->client = pdata->clients + i;
+        priv->set_bus_param             = icd->ops->set_bus_param;
+        priv->query_bus_param           = icd->ops->query_bus_param;
+        icd->ops->set_bus_param         = sh_csi2_set_bus_param;
+        icd->ops->query_bus_param       = sh_csi2_query_bus_param;
 
-                priv->set_bus_param             = icd->ops->set_bus_param;
-                priv->query_bus_param           = icd->ops->query_bus_param;
-                icd->ops->set_bus_param         = sh_csi2_set_bus_param;
-                icd->ops->query_bus_param       = sh_csi2_query_bus_param;
+        csi2_sd->grp_id = (long)icd;
 
-                pm_runtime_get_sync(v4l2_get_subdevdata(&priv->subdev));
+        pm_runtime_get_sync(dev);
 
-                sh_csi2_hwinit(priv);
-                break;
-        case BUS_NOTIFY_UNBIND_DRIVER:
-                priv->client = NULL;
+        sh_csi2_hwinit(priv);
 
-                /* Driver is about to be unbound */
-                icd->ops->set_bus_param         = priv->set_bus_param;
-                icd->ops->query_bus_param       = priv->query_bus_param;
-                priv->set_bus_param             = NULL;
-                priv->query_bus_param           = NULL;
+        return 0;
+}
 
-                v4l2_device_unregister_subdev(&priv->subdev);
+static void sh_csi2_client_disconnect(struct sh_csi2 *priv)
+{
+        struct soc_camera_device *icd = (struct soc_camera_device *)priv->subdev.grp_id;
 
-                pm_runtime_put(v4l2_get_subdevdata(&priv->subdev));
-                break;
-        }
+        priv->client = NULL;
+        priv->subdev.grp_id = 0;
 
-        return NOTIFY_OK;
+        /* Driver is about to be unbound */
+        icd->ops->set_bus_param         = priv->set_bus_param;
+        icd->ops->query_bus_param       = priv->query_bus_param;
+        priv->set_bus_param             = NULL;
+        priv->query_bus_param           = NULL;
+
+        pm_runtime_put(v4l2_get_subdevdata(&priv->subdev));
 }
 
+static int sh_csi2_s_power(struct v4l2_subdev *sd, int on)
+{
+        struct sh_csi2 *priv = container_of(sd, struct sh_csi2, subdev);
+
+        if (on)
+                return sh_csi2_client_connect(priv);
+
+        sh_csi2_client_disconnect(priv);
+        return 0;
+}
+
+static struct v4l2_subdev_core_ops sh_csi2_subdev_core_ops = {
+        .s_power        = sh_csi2_s_power,
+};
+
+static struct v4l2_subdev_ops sh_csi2_subdev_ops = {
+        .core   = &sh_csi2_subdev_core_ops,
+        .video  = &sh_csi2_subdev_video_ops,
+};
+
 static __devinit int sh_csi2_probe(struct platform_device *pdev)
 {
         struct resource *res;
@@ -274,14 +286,6 @@ static __devinit int sh_csi2_probe(struct platform_device *pdev)
                 return -ENOMEM;
 
         priv->irq = irq;
-        priv->notifier.notifier_call = sh_csi2_notify;
-
-        /* We MUST attach after the MIPI sensor */
-        ret = bus_register_notifier(&soc_camera_bus_type, &priv->notifier);
-        if (ret < 0) {
-                dev_err(&pdev->dev, "CSI2 cannot register notifier\n");
-                goto ernotify;
-        }
 
         if (!request_mem_region(res->start, resource_size(res), pdev->name)) {
                 dev_err(&pdev->dev, "CSI2 register region already claimed\n");
@@ -297,11 +301,17 @@ static __devinit int sh_csi2_probe(struct platform_device *pdev)
         }
 
         priv->pdev = pdev;
+        platform_set_drvdata(pdev, priv);
 
         v4l2_subdev_init(&priv->subdev, &sh_csi2_subdev_ops);
         v4l2_set_subdevdata(&priv->subdev, &pdev->dev);
 
-        platform_set_drvdata(pdev, priv);
+        snprintf(priv->subdev.name, V4L2_SUBDEV_NAME_SIZE, "%s.mipi-csi",
+                 dev_name(pdata->v4l2_dev->dev));
+        ret = v4l2_device_register_subdev(pdata->v4l2_dev, &priv->subdev);
+        dev_dbg(&pdev->dev, "%s(%p): ret(register_subdev) = %d\n", __func__, priv, ret);
+        if (ret < 0)
+                goto esdreg;
 
         pm_runtime_enable(&pdev->dev);
 
@@ -309,11 +319,11 @@ static __devinit int sh_csi2_probe(struct platform_device *pdev)
 
         return 0;
 
+esdreg:
+        iounmap(priv->base);
 eremap:
         release_mem_region(res->start, resource_size(res));
 ereqreg:
-        bus_unregister_notifier(&soc_camera_bus_type, &priv->notifier);
-ernotify:
         kfree(priv);
 
         return ret;
@@ -324,7 +334,7 @@ static __devexit int sh_csi2_remove(struct platform_device *pdev)
         struct sh_csi2 *priv = platform_get_drvdata(pdev);
         struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
-        bus_unregister_notifier(&soc_camera_bus_type, &priv->notifier);
+        v4l2_device_unregister_subdev(&priv->subdev);
         pm_runtime_disable(&pdev->dev);
         iounmap(priv->base);
         release_mem_region(res->start, resource_size(res));
@@ -335,8 +345,9 @@ static __devexit int sh_csi2_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver __refdata sh_csi2_pdrv = {
-        .remove  = __devexit_p(sh_csi2_remove),
-        .driver  = {
+        .remove = __devexit_p(sh_csi2_remove),
+        .probe  = sh_csi2_probe,
+        .driver = {
                 .name   = "sh-mobile-csi2",
                 .owner  = THIS_MODULE,
         },
@@ -344,7 +355,7 @@ static struct platform_driver __refdata sh_csi2_pdrv = {
 
 static int __init sh_csi2_init(void)
 {
-        return platform_driver_probe(&sh_csi2_pdrv, sh_csi2_probe);
+        return platform_driver_register(&sh_csi2_pdrv);
 }
 
 static void __exit sh_csi2_exit(void)
diff --git a/drivers/media/video/sh_vou.c b/drivers/media/video/sh_vou.c
index 07cf0c6c7c1..6a729879d89 100644
--- a/drivers/media/video/sh_vou.c
+++ b/drivers/media/video/sh_vou.c
@@ -19,7 +19,6 @@
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/slab.h>
-#include <linux/version.h>
 #include <linux/videodev2.h>
 
 #include <media/sh_vou.h>
@@ -393,7 +392,6 @@ static int sh_vou_querycap(struct file *file, void  *priv,
         dev_dbg(vou_file->vbq.dev, "%s()\n", __func__);
 
         strlcpy(cap->card, "SuperH VOU", sizeof(cap->card));
-        cap->version = KERNEL_VERSION(0, 1, 0);
         cap->capabilities = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
         return 0;
 }
@@ -1490,4 +1488,5 @@ module_exit(sh_vou_exit);
 MODULE_DESCRIPTION("SuperH VOU driver");
 MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
 MODULE_LICENSE("GPL v2");
+MODULE_VERSION("0.1.0");
 MODULE_ALIAS("platform:sh-vou");
diff --git a/drivers/media/video/sn9c102/sn9c102.h b/drivers/media/video/sn9c102/sn9c102.h
index cbfc44433b9..22ea211ab54 100644
--- a/drivers/media/video/sn9c102/sn9c102.h
+++ b/drivers/media/video/sn9c102/sn9c102.h
@@ -21,7 +21,6 @@
 #ifndef _SN9C102_H_
 #define _SN9C102_H_
 
-#include <linux/version.h>
 #include <linux/usb.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-common.h>
diff --git a/drivers/media/video/sn9c102/sn9c102_core.c b/drivers/media/video/sn9c102/sn9c102_core.c
index 0e07c493e6f..16cb07c5c27 100644
--- a/drivers/media/video/sn9c102/sn9c102_core.c
+++ b/drivers/media/video/sn9c102/sn9c102_core.c
@@ -33,6 +33,7 @@
 #include <linux/stat.h>
 #include <linux/mm.h>
 #include <linux/vmalloc.h>
+#include <linux/version.h>
 #include <linux/page-flags.h>
 #include <asm/byteorder.h>
 #include <asm/page.h>
@@ -47,8 +48,7 @@
 #define SN9C102_MODULE_AUTHOR   "(C) 2004-2007 Luca Risolia"
 #define SN9C102_AUTHOR_EMAIL    "<luca.risolia@studio.unibo.it>"
 #define SN9C102_MODULE_LICENSE  "GPL"
-#define SN9C102_MODULE_VERSION  "1:1.47pre49"
-#define SN9C102_MODULE_VERSION_CODE  KERNEL_VERSION(1, 1, 47)
+#define SN9C102_MODULE_VERSION  "1:1.48"
 
 /*****************************************************************************/
 
@@ -2158,7 +2158,7 @@ sn9c102_vidioc_querycap(struct sn9c102_device* cam, void __user * arg)
 {
         struct v4l2_capability cap = {
                 .driver = "sn9c102",
-                .version = SN9C102_MODULE_VERSION_CODE,
+                .version = LINUX_VERSION_CODE,
                 .capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
                                 V4L2_CAP_STREAMING,
         };
@@ -3187,16 +3187,8 @@ static long sn9c102_ioctl_v4l2(struct file *filp,
         case VIDIOC_S_AUDIO:
                 return sn9c102_vidioc_s_audio(cam, arg);
 
-        case VIDIOC_G_STD:
-        case VIDIOC_S_STD:
-        case VIDIOC_QUERYSTD:
-        case VIDIOC_ENUMSTD:
-        case VIDIOC_QUERYMENU:
-        case VIDIOC_ENUM_FRAMEINTERVALS:
-                return -EINVAL;
-
         default:
-                return -EINVAL;
+                return -ENOTTY;
 
         }
 }
diff --git a/drivers/media/video/soc_camera.c b/drivers/media/video/soc_camera.c
index 4e4d4122d9a..5bdfe7e16bc 100644
--- a/drivers/media/video/soc_camera.c
+++ b/drivers/media/video/soc_camera.c
@@ -60,14 +60,14 @@ static int soc_camera_power_set(struct soc_camera_device *icd,
                 ret = regulator_bulk_enable(icl->num_regulators,
                                             icl->regulators);
                 if (ret < 0) {
-                        dev_err(&icd->dev, "Cannot enable regulators\n");
+                        dev_err(icd->pdev, "Cannot enable regulators\n");
                         return ret;
                 }
 
                 if (icl->power)
                         ret = icl->power(icd->pdev, power_on);
                 if (ret < 0) {
-                        dev_err(&icd->dev,
+                        dev_err(icd->pdev,
                                 "Platform failed to power-on the camera.\n");
 
                         regulator_bulk_disable(icl->num_regulators,
@@ -79,7 +79,7 @@ static int soc_camera_power_set(struct soc_camera_device *icd,
                 if (icl->power)
                         ret = icl->power(icd->pdev, 0);
                 if (ret < 0) {
-                        dev_err(&icd->dev,
+                        dev_err(icd->pdev,
                                 "Platform failed to power-off the camera.\n");
                         return ret;
                 }
@@ -87,7 +87,7 @@ static int soc_camera_power_set(struct soc_camera_device *icd,
                 ret = regulator_bulk_disable(icl->num_regulators,
                                              icl->regulators);
                 if (ret < 0) {
-                        dev_err(&icd->dev, "Cannot disable regulators\n");
+                        dev_err(icd->pdev, "Cannot disable regulators\n");
                         return ret;
                 }
         }
@@ -147,11 +147,11 @@ EXPORT_SYMBOL(soc_camera_apply_sensor_flags);
 static int soc_camera_try_fmt(struct soc_camera_device *icd,
                               struct v4l2_format *f)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct v4l2_pix_format *pix = &f->fmt.pix;
         int ret;
 
-        dev_dbg(&icd->dev, "TRY_FMT(%c%c%c%c, %ux%u)\n",
+        dev_dbg(icd->pdev, "TRY_FMT(%c%c%c%c, %ux%u)\n",
                 pixfmtstr(pix->pixelformat), pix->width, pix->height);
 
         pix->bytesperline = 0;
@@ -199,22 +199,15 @@ static int soc_camera_try_fmt_vid_cap(struct file *file, void *priv,
 static int soc_camera_enum_input(struct file *file, void *priv,
                                  struct v4l2_input *inp)
 {
-        struct soc_camera_device *icd = file->private_data;
-        int ret = 0;
-
         if (inp->index != 0)
                 return -EINVAL;
 
-        if (icd->ops->enum_input)
-                ret = icd->ops->enum_input(icd, inp);
-        else {
-                /* default is camera */
-                inp->type = V4L2_INPUT_TYPE_CAMERA;
-                inp->std  = V4L2_STD_UNKNOWN;
-                strcpy(inp->name, "Camera");
-        }
+        /* default is camera */
+        inp->type = V4L2_INPUT_TYPE_CAMERA;
+        inp->std  = V4L2_STD_UNKNOWN;
+        strcpy(inp->name, "Camera");
 
-        return ret;
+        return 0;
 }
 
 static int soc_camera_g_input(struct file *file, void *priv, unsigned int *i)
@@ -244,7 +237,7 @@ static int soc_camera_enum_fsizes(struct file *file, void *fh,
                                          struct v4l2_frmsizeenum *fsize)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
         return ici->ops->enum_fsizes(icd, fsize);
 }
@@ -254,7 +247,7 @@ 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->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
         WARN_ON(priv != file->private_data);
 
@@ -281,7 +274,7 @@ 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->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
         WARN_ON(priv != file->private_data);
 
@@ -295,7 +288,7 @@ 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->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
         WARN_ON(priv != file->private_data);
 
@@ -312,7 +305,7 @@ 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->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
         WARN_ON(priv != file->private_data);
 
@@ -329,7 +322,7 @@ static int soc_camera_dqbuf(struct file *file, void *priv,
 static int soc_camera_init_user_formats(struct soc_camera_device *icd)
 {
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         unsigned int i, fmts = 0, raw_fmts = 0;
         int ret;
         enum v4l2_mbus_pixelcode code;
@@ -363,7 +356,7 @@ static int soc_camera_init_user_formats(struct soc_camera_device *icd)
         if (!icd->user_formats)
                 return -ENOMEM;
 
-        dev_dbg(&icd->dev, "Found %d supported formats.\n", fmts);
+        dev_dbg(icd->pdev, "Found %d supported formats.\n", fmts);
 
         /* Second pass - actually fill data formats */
         fmts = 0;
@@ -395,7 +388,7 @@ egfmt:
 /* Always entered with .video_lock held */
 static void soc_camera_free_user_formats(struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
         if (ici->ops->put_formats)
                 ici->ops->put_formats(icd);
@@ -409,11 +402,11 @@ static void soc_camera_free_user_formats(struct soc_camera_device *icd)
 static int soc_camera_set_fmt(struct soc_camera_device *icd,
                               struct v4l2_format *f)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct v4l2_pix_format *pix = &f->fmt.pix;
         int ret;
 
-        dev_dbg(&icd->dev, "S_FMT(%c%c%c%c, %ux%u)\n",
+        dev_dbg(icd->pdev, "S_FMT(%c%c%c%c, %ux%u)\n",
                 pixfmtstr(pix->pixelformat), pix->width, pix->height);
 
         /* We always call try_fmt() before set_fmt() or set_crop() */
@@ -426,7 +419,7 @@ static int soc_camera_set_fmt(struct soc_camera_device *icd,
                 return ret;
         } else if (!icd->current_fmt ||
                    icd->current_fmt->host_fmt->fourcc != pix->pixelformat) {
-                dev_err(&icd->dev,
+                dev_err(icd->pdev,
                         "Host driver hasn't set up current format correctly!\n");
                 return -EINVAL;
         }
@@ -440,7 +433,7 @@ static int soc_camera_set_fmt(struct soc_camera_device *icd,
         if (ici->ops->init_videobuf)
                 icd->vb_vidq.field = pix->field;
 
-        dev_dbg(&icd->dev, "set width: %d height: %d\n",
+        dev_dbg(icd->pdev, "set width: %d height: %d\n",
                 icd->user_width, icd->user_height);
 
         /* set physical bus parameters */
@@ -450,9 +443,7 @@ static int soc_camera_set_fmt(struct soc_camera_device *icd,
 static int soc_camera_open(struct file *file)
 {
         struct video_device *vdev = video_devdata(file);
-        struct soc_camera_device *icd = container_of(vdev->parent,
-                                                     struct soc_camera_device,
-                                                     dev);
+        struct soc_camera_device *icd = dev_get_drvdata(vdev->parent);
         struct soc_camera_link *icl = to_soc_camera_link(icd);
         struct soc_camera_host *ici;
         int ret;
@@ -461,10 +452,10 @@ static int soc_camera_open(struct file *file)
                 /* No device driver attached */
                 return -ENODEV;
 
-        ici = to_soc_camera_host(icd->dev.parent);
+        ici = to_soc_camera_host(icd->parent);
 
         if (!try_module_get(ici->ops->owner)) {
-                dev_err(&icd->dev, "Couldn't lock capture bus driver.\n");
+                dev_err(icd->pdev, "Couldn't lock capture bus driver.\n");
                 return -EINVAL;
         }
 
@@ -495,7 +486,7 @@ static int soc_camera_open(struct file *file)
 
                 ret = ici->ops->add(icd);
                 if (ret < 0) {
-                        dev_err(&icd->dev, "Couldn't activate the camera: %d\n", ret);
+                        dev_err(icd->pdev, "Couldn't activate the camera: %d\n", ret);
                         goto eiciadd;
                 }
 
@@ -524,7 +515,7 @@ static int soc_camera_open(struct file *file)
         }
 
         file->private_data = icd;
-        dev_dbg(&icd->dev, "camera device open\n");
+        dev_dbg(icd->pdev, "camera device open\n");
 
         return 0;
 
@@ -549,7 +540,7 @@ epower:
 static int soc_camera_close(struct file *file)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
         icd->use_count--;
         if (!icd->use_count) {
@@ -570,7 +561,7 @@ static int soc_camera_close(struct file *file)
 
         module_put(ici->ops->owner);
 
-        dev_dbg(&icd->dev, "camera device close\n");
+        dev_dbg(icd->pdev, "camera device close\n");
 
         return 0;
 }
@@ -581,7 +572,7 @@ static ssize_t soc_camera_read(struct file *file, char __user *buf,
         struct soc_camera_device *icd = file->private_data;
         int err = -EINVAL;
 
-        dev_err(&icd->dev, "camera device read not implemented\n");
+        dev_err(icd->pdev, "camera device read not implemented\n");
 
         return err;
 }
@@ -589,10 +580,10 @@ static ssize_t soc_camera_read(struct file *file, char __user *buf,
 static int soc_camera_mmap(struct file *file, struct vm_area_struct *vma)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         int err;
 
-        dev_dbg(&icd->dev, "mmap called, vma=0x%08lx\n", (unsigned long)vma);
+        dev_dbg(icd->pdev, "mmap called, vma=0x%08lx\n", (unsigned long)vma);
 
         if (icd->streamer != file)
                 return -EBUSY;
@@ -602,7 +593,7 @@ static int soc_camera_mmap(struct file *file, struct vm_area_struct *vma)
         else
                 err = vb2_mmap(&icd->vb2_vidq, vma);
 
-        dev_dbg(&icd->dev, "vma start=0x%08lx, size=%ld, ret=%d\n",
+        dev_dbg(icd->pdev, "vma start=0x%08lx, size=%ld, ret=%d\n",
                 (unsigned long)vma->vm_start,
                 (unsigned long)vma->vm_end - (unsigned long)vma->vm_start,
                 err);
@@ -613,13 +604,13 @@ static int soc_camera_mmap(struct file *file, struct vm_area_struct *vma)
 static unsigned int soc_camera_poll(struct file *file, poll_table *pt)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
         if (icd->streamer != file)
                 return -EBUSY;
 
         if (ici->ops->init_videobuf && list_empty(&icd->vb_vidq.stream)) {
-                dev_err(&icd->dev, "Trying to poll with no queued buffers!\n");
+                dev_err(icd->pdev, "Trying to poll with no queued buffers!\n");
                 return POLLERR;
         }
 
@@ -659,15 +650,15 @@ static int soc_camera_s_fmt_vid_cap(struct file *file, void *priv,
         WARN_ON(priv != file->private_data);
 
         if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-                dev_warn(&icd->dev, "Wrong buf-type %d\n", f->type);
+                dev_warn(icd->pdev, "Wrong buf-type %d\n", f->type);
                 return -EINVAL;
         }
 
         if (icd->streamer && icd->streamer != file)
                 return -EBUSY;
 
-        if (is_streaming(to_soc_camera_host(icd->dev.parent), icd)) {
-                dev_err(&icd->dev, "S_FMT denied: queue initialised\n");
+        if (is_streaming(to_soc_camera_host(icd->parent), icd)) {
+                dev_err(icd->pdev, "S_FMT denied: queue initialised\n");
                 return -EBUSY;
         }
 
@@ -716,7 +707,7 @@ static int soc_camera_g_fmt_vid_cap(struct file *file, void *priv,
         pix->field              = icd->field;
         pix->pixelformat        = icd->current_fmt->host_fmt->fourcc;
         pix->colorspace         = icd->colorspace;
-        dev_dbg(&icd->dev, "current_fmt->fourcc: 0x%08x\n",
+        dev_dbg(icd->pdev, "current_fmt->fourcc: 0x%08x\n",
                 icd->current_fmt->host_fmt->fourcc);
         return 0;
 }
@@ -725,7 +716,7 @@ static int soc_camera_querycap(struct file *file, void  *priv,
                                struct v4l2_capability *cap)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
         WARN_ON(priv != file->private_data);
 
@@ -737,7 +728,7 @@ 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->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
         int ret;
 
@@ -766,7 +757,7 @@ 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->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
         WARN_ON(priv != file->private_data);
 
@@ -794,7 +785,7 @@ static int soc_camera_queryctrl(struct file *file, void *priv,
                                 struct v4l2_queryctrl *qc)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         int i;
 
         WARN_ON(priv != file->private_data);
@@ -825,7 +816,7 @@ static int soc_camera_g_ctrl(struct file *file, void *priv,
                              struct v4l2_control *ctrl)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
         int ret;
 
@@ -844,7 +835,7 @@ static int soc_camera_s_ctrl(struct file *file, void *priv,
                              struct v4l2_control *ctrl)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
         int ret;
 
@@ -863,7 +854,7 @@ static int soc_camera_cropcap(struct file *file, void *fh,
                               struct v4l2_cropcap *a)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
         return ici->ops->cropcap(icd, a);
 }
@@ -872,7 +863,7 @@ static int soc_camera_g_crop(struct file *file, void *fh,
                              struct v4l2_crop *a)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         int ret;
 
         ret = ici->ops->get_crop(icd, a);
@@ -889,7 +880,7 @@ static int soc_camera_s_crop(struct file *file, void *fh,
                              struct v4l2_crop *a)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct v4l2_rect *rect = &a->c;
         struct v4l2_crop current_crop;
         int ret;
@@ -897,7 +888,7 @@ static int soc_camera_s_crop(struct file *file, void *fh,
         if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
                 return -EINVAL;
 
-        dev_dbg(&icd->dev, "S_CROP(%ux%u@%u:%u)\n",
+        dev_dbg(icd->pdev, "S_CROP(%ux%u@%u:%u)\n",
                 rect->width, rect->height, rect->left, rect->top);
 
         /* If get_crop fails, we'll let host and / or client drivers decide */
@@ -905,7 +896,7 @@ static int soc_camera_s_crop(struct file *file, void *fh,
 
         /* Prohibit window size change with initialised buffers */
         if (ret < 0) {
-                dev_err(&icd->dev,
+                dev_err(icd->pdev,
                         "S_CROP denied: getting current crop failed\n");
         } else if ((a->c.width == current_crop.c.width &&
                     a->c.height == current_crop.c.height) ||
@@ -915,7 +906,7 @@ static int soc_camera_s_crop(struct file *file, void *fh,
         } else if (ici->ops->set_livecrop) {
                 ret = ici->ops->set_livecrop(icd, a);
         } else {
-                dev_err(&icd->dev,
+                dev_err(icd->pdev,
                         "S_CROP denied: queue initialised and sizes differ\n");
                 ret = -EBUSY;
         }
@@ -927,7 +918,7 @@ static int soc_camera_g_parm(struct file *file, void *fh,
                              struct v4l2_streamparm *a)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
         if (ici->ops->get_parm)
                 return ici->ops->get_parm(icd, a);
@@ -939,7 +930,7 @@ static int soc_camera_s_parm(struct file *file, void *fh,
                              struct v4l2_streamparm *a)
 {
         struct soc_camera_device *icd = file->private_data;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
         if (ici->ops->set_parm)
                 return ici->ops->set_parm(icd, a);
@@ -976,6 +967,8 @@ static int soc_camera_s_register(struct file *file, void *fh,
 }
 #endif
 
+static int soc_camera_probe(struct soc_camera_device *icd);
+
 /* So far this function cannot fail */
 static void scan_add_host(struct soc_camera_host *ici)
 {
@@ -986,15 +979,9 @@ static void scan_add_host(struct soc_camera_host *ici)
         list_for_each_entry(icd, &devices, list) {
                 if (icd->iface == ici->nr) {
                         int ret;
-                        icd->dev.parent = ici->v4l2_dev.dev;
-                        dev_set_name(&icd->dev, "%u-%u", icd->iface,
-                                     icd->devnum);
-                        ret = device_register(&icd->dev);
-                        if (ret < 0) {
-                                icd->dev.parent = NULL;
-                                dev_err(&icd->dev,
-                                        "Cannot register device: %d\n", ret);
-                        }
+
+                        icd->parent = ici->v4l2_dev.dev;
+                        ret = soc_camera_probe(icd);
                 }
         }
 
@@ -1006,12 +993,12 @@ static int soc_camera_init_i2c(struct soc_camera_device *icd,
                                struct soc_camera_link *icl)
 {
         struct i2c_client *client;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct i2c_adapter *adap = i2c_get_adapter(icl->i2c_adapter_id);
         struct v4l2_subdev *subdev;
 
         if (!adap) {
-                dev_err(&icd->dev, "Cannot get I2C adapter #%d. No driver?\n",
+                dev_err(icd->pdev, "Cannot get I2C adapter #%d. No driver?\n",
                         icl->i2c_adapter_id);
                 goto ei2cga;
         }
@@ -1026,7 +1013,7 @@ static int soc_camera_init_i2c(struct soc_camera_device *icd,
         client = v4l2_get_subdevdata(subdev);
 
         /* Use to_i2c_client(dev) to recover the i2c client */
-        dev_set_drvdata(&icd->dev, &client->dev);
+        icd->control = &client->dev;
 
         return 0;
 ei2cnd:
@@ -1040,7 +1027,8 @@ static void soc_camera_free_i2c(struct soc_camera_device *icd)
         struct i2c_client *client =
                 to_i2c_client(to_soc_camera_control(icd));
         struct i2c_adapter *adap = client->adapter;
-        dev_set_drvdata(&icd->dev, NULL);
+
+        icd->control = NULL;
         v4l2_device_unregister_subdev(i2c_get_clientdata(client));
         i2c_unregister_device(client);
         i2c_put_adapter(adap);
@@ -1053,17 +1041,16 @@ static void soc_camera_free_i2c(struct soc_camera_device *icd)
 static int soc_camera_video_start(struct soc_camera_device *icd);
 static int video_dev_create(struct soc_camera_device *icd);
 /* Called during host-driver probe */
-static int soc_camera_probe(struct device *dev)
+static int soc_camera_probe(struct soc_camera_device *icd)
 {
-        struct soc_camera_device *icd = to_soc_camera_dev(dev);
-        struct soc_camera_host *ici = to_soc_camera_host(dev->parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct soc_camera_link *icl = to_soc_camera_link(icd);
         struct device *control = NULL;
         struct v4l2_subdev *sd;
         struct v4l2_mbus_framefmt mf;
         int ret;
 
-        dev_info(dev, "Probing %s\n", dev_name(dev));
+        dev_info(icd->pdev, "Probing %s\n", dev_name(icd->pdev));
 
         ret = regulator_bulk_get(icd->pdev, icl->num_regulators,
                                  icl->regulators);
@@ -1099,7 +1086,7 @@ static int soc_camera_probe(struct device *dev)
                 if (icl->module_name)
                         ret = request_module(icl->module_name);
 
-                ret = icl->add_device(icl, &icd->dev);
+                ret = icl->add_device(icd);
                 if (ret < 0)
                         goto eadddev;
 
@@ -1110,7 +1097,7 @@ static int soc_camera_probe(struct device *dev)
                 control = to_soc_camera_control(icd);
                 if (!control || !control->driver || !dev_get_drvdata(control) ||
                     !try_module_get(control->driver->owner)) {
-                        icl->del_device(icl);
+                        icl->del_device(icd);
                         goto enodrv;
                 }
         }
@@ -1125,8 +1112,6 @@ static int soc_camera_probe(struct device *dev)
 
         icd->field = V4L2_FIELD_ANY;
 
-        icd->vdev->lock = &icd->video_lock;
-
         /*
          * ..._video_start() will create a device node, video_register_device()
          * itself is protected against concurrent open() calls, but we also have
@@ -1146,11 +1131,6 @@ static int soc_camera_probe(struct device *dev)
                 icd->field              = mf.field;
         }
 
-        /* Do we have to sysfs_remove_link() before device_unregister()? */
-        if (sysfs_create_link(&icd->dev.kobj, &to_soc_camera_control(icd)->kobj,
-                              "control"))
-                dev_warn(&icd->dev, "Failed creating the control symlink\n");
-
         ici->ops->remove(icd);
 
         soc_camera_power_set(icd, icl, 0);
@@ -1166,7 +1146,7 @@ eiufmt:
         if (icl->board_info) {
                 soc_camera_free_i2c(icd);
         } else {
-                icl->del_device(icl);
+                icl->del_device(icd);
                 module_put(control->driver->owner);
         }
 enodrv:
@@ -1186,13 +1166,12 @@ ereg:
  * This is called on device_unregister, which only means we have to disconnect
  * from the host, but not remove ourselves from the device list
  */
-static int soc_camera_remove(struct device *dev)
+static int soc_camera_remove(struct soc_camera_device *icd)
 {
-        struct soc_camera_device *icd = to_soc_camera_dev(dev);
         struct soc_camera_link *icl = to_soc_camera_link(icd);
         struct video_device *vdev = icd->vdev;
 
-        BUG_ON(!dev->parent);
+        BUG_ON(!icd->parent);
 
         if (vdev) {
                 video_unregister_device(vdev);
@@ -1202,10 +1181,9 @@ static int soc_camera_remove(struct device *dev)
         if (icl->board_info) {
                 soc_camera_free_i2c(icd);
         } else {
-                struct device_driver *drv = to_soc_camera_control(icd) ?
-                        to_soc_camera_control(icd)->driver : NULL;
+                struct device_driver *drv = to_soc_camera_control(icd)->driver;
                 if (drv) {
-                        icl->del_device(icl);
+                        icl->del_device(icd);
                         module_put(drv->owner);
                 }
         }
@@ -1216,49 +1194,6 @@ static int soc_camera_remove(struct device *dev)
         return 0;
 }
 
-static int soc_camera_suspend(struct device *dev, pm_message_t state)
-{
-        struct soc_camera_device *icd = to_soc_camera_dev(dev);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
-        int ret = 0;
-
-        if (ici->ops->suspend)
-                ret = ici->ops->suspend(icd, state);
-
-        return ret;
-}
-
-static int soc_camera_resume(struct device *dev)
-{
-        struct soc_camera_device *icd = to_soc_camera_dev(dev);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
-        int ret = 0;
-
-        if (ici->ops->resume)
-                ret = ici->ops->resume(icd);
-
-        return ret;
-}
-
-struct bus_type soc_camera_bus_type = {
-        .name           = "soc-camera",
-        .probe          = soc_camera_probe,
-        .remove         = soc_camera_remove,
-        .suspend        = soc_camera_suspend,
-        .resume         = soc_camera_resume,
-};
-EXPORT_SYMBOL_GPL(soc_camera_bus_type);
-
-static struct device_driver ic_drv = {
-        .name   = "camera",
-        .bus    = &soc_camera_bus_type,
-        .owner  = THIS_MODULE,
-};
-
-static void dummy_release(struct device *dev)
-{
-}
-
 static int default_cropcap(struct soc_camera_device *icd,
                            struct v4l2_cropcap *a)
 {
@@ -1317,13 +1252,6 @@ static int default_enum_fsizes(struct soc_camera_device *icd,
         return 0;
 }
 
-static void soc_camera_device_init(struct device *dev, void *pdata)
-{
-        dev->platform_data      = pdata;
-        dev->bus                = &soc_camera_bus_type;
-        dev->release            = dummy_release;
-}
-
 int soc_camera_host_register(struct soc_camera_host *ici)
 {
         struct soc_camera_host *ix;
@@ -1389,24 +1317,9 @@ void soc_camera_host_unregister(struct soc_camera_host *ici)
         mutex_lock(&list_lock);
 
         list_del(&ici->list);
-
-        list_for_each_entry(icd, &devices, list) {
-                if (icd->iface == ici->nr) {
-                        void *pdata = icd->dev.platform_data;
-                        /* The bus->remove will be called */
-                        device_unregister(&icd->dev);
-                        /*
-                         * Not before device_unregister(), .remove
-                         * needs parent to call ici->ops->remove().
-                         * If the host module is loaded again, device_register()
-                         * would complain "already initialised," since 2.6.32
-                         * this is also needed to prevent use-after-free of the
-                         * device private data.
-                         */
-                        memset(&icd->dev, 0, sizeof(icd->dev));
-                        soc_camera_device_init(&icd->dev, pdata);
-                }
-        }
+        list_for_each_entry(icd, &devices, list)
+                if (icd->iface == ici->nr && to_soc_camera_control(icd))
+                        soc_camera_remove(icd);
 
         mutex_unlock(&list_lock);
 
@@ -1448,11 +1361,6 @@ static int soc_camera_device_register(struct soc_camera_device *icd)
         return 0;
 }
 
-static void soc_camera_device_unregister(struct soc_camera_device *icd)
-{
-        list_del(&icd->list);
-}
-
 static const struct v4l2_ioctl_ops soc_camera_ioctl_ops = {
         .vidioc_querycap         = soc_camera_querycap,
         .vidioc_g_fmt_vid_cap    = soc_camera_g_fmt_vid_cap,
@@ -1487,7 +1395,7 @@ static const struct v4l2_ioctl_ops soc_camera_ioctl_ops = {
 
 static int video_dev_create(struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct video_device *vdev = video_device_alloc();
 
         if (!vdev)
@@ -1495,12 +1403,13 @@ static int video_dev_create(struct soc_camera_device *icd)
 
         strlcpy(vdev->name, ici->drv_name, sizeof(vdev->name));
 
-        vdev->parent            = &icd->dev;
+        vdev->parent            = icd->pdev;
         vdev->current_norm      = V4L2_STD_UNKNOWN;
         vdev->fops              = &soc_camera_fops;
         vdev->ioctl_ops         = &soc_camera_ioctl_ops;
         vdev->release           = video_device_release;
         vdev->tvnorms           = V4L2_STD_UNKNOWN;
+        vdev->lock              = &icd->video_lock;
 
         icd->vdev = vdev;
 
@@ -1515,7 +1424,7 @@ static int soc_camera_video_start(struct soc_camera_device *icd)
         const struct device_type *type = icd->vdev->dev.type;
         int ret;
 
-        if (!icd->dev.parent)
+        if (!icd->parent)
                 return -ENODEV;
 
         if (!icd->ops ||
@@ -1525,7 +1434,7 @@ static int soc_camera_video_start(struct soc_camera_device *icd)
 
         ret = video_register_device(icd->vdev, VFL_TYPE_GRABBER, -1);
         if (ret < 0) {
-                dev_err(&icd->dev, "video_register_device failed: %d\n", ret);
+                dev_err(icd->pdev, "video_register_device failed: %d\n", ret);
                 return ret;
         }
 
@@ -1549,6 +1458,7 @@ static int __devinit soc_camera_pdrv_probe(struct platform_device *pdev)
                 return -ENOMEM;
 
         icd->iface = icl->bus_id;
+        icd->link = icl;
         icd->pdev = &pdev->dev;
         platform_set_drvdata(pdev, icd);
 
@@ -1556,8 +1466,6 @@ static int __devinit soc_camera_pdrv_probe(struct platform_device *pdev)
         if (ret < 0)
                 goto escdevreg;
 
-        soc_camera_device_init(&icd->dev, icl);
-
         icd->user_width         = DEFAULT_WIDTH;
         icd->user_height        = DEFAULT_HEIGHT;
 
@@ -1581,7 +1489,7 @@ static int __devexit soc_camera_pdrv_remove(struct platform_device *pdev)
         if (!icd)
                 return -EINVAL;
 
-        soc_camera_device_unregister(icd);
+        list_del(&icd->list);
 
         kfree(icd);
 
@@ -1598,31 +1506,12 @@ static struct platform_driver __refdata soc_camera_pdrv = {
 
 static int __init soc_camera_init(void)
 {
-        int ret = bus_register(&soc_camera_bus_type);
-        if (ret)
-                return ret;
-        ret = driver_register(&ic_drv);
-        if (ret)
-                goto edrvr;
-
-        ret = platform_driver_probe(&soc_camera_pdrv, soc_camera_pdrv_probe);
-        if (ret)
-                goto epdr;
-
-        return 0;
-
-epdr:
-        driver_unregister(&ic_drv);
-edrvr:
-        bus_unregister(&soc_camera_bus_type);
-        return ret;
+        return platform_driver_probe(&soc_camera_pdrv, soc_camera_pdrv_probe);
 }
 
 static void __exit soc_camera_exit(void)
 {
         platform_driver_unregister(&soc_camera_pdrv);
-        driver_unregister(&ic_drv);
-        bus_unregister(&soc_camera_bus_type);
 }
 
 module_init(soc_camera_init);
diff --git a/drivers/media/video/soc_camera_platform.c b/drivers/media/video/soc_camera_platform.c
index bf406e89c99..8069cd6bc5e 100644
--- a/drivers/media/video/soc_camera_platform.c
+++ b/drivers/media/video/soc_camera_platform.c
@@ -146,7 +146,7 @@ static int soc_camera_platform_probe(struct platform_device *pdev)
         if (!p)
                 return -EINVAL;
 
-        if (!p->dev) {
+        if (!p->icd) {
                 dev_err(&pdev->dev,
                         "Platform has not set soc_camera_device pointer!\n");
                 return -EINVAL;
@@ -156,16 +156,16 @@ static int soc_camera_platform_probe(struct platform_device *pdev)
         if (!priv)
                 return -ENOMEM;
 
-        icd = to_soc_camera_dev(p->dev);
+        icd = p->icd;
 
         /* soc-camera convention: control's drvdata points to the subdev */
         platform_set_drvdata(pdev, &priv->subdev);
         /* Set the control device reference */
-        dev_set_drvdata(&icd->dev, &pdev->dev);
+        icd->control = &pdev->dev;
 
         icd->ops = &soc_camera_platform_ops;
 
-        ici = to_soc_camera_host(icd->dev.parent);
+        ici = to_soc_camera_host(icd->parent);
 
         v4l2_subdev_init(&priv->subdev, &platform_subdev_ops);
         v4l2_set_subdevdata(&priv->subdev, p);
@@ -188,7 +188,7 @@ static int soc_camera_platform_remove(struct platform_device *pdev)
 {
         struct soc_camera_platform_priv *priv = get_priv(pdev);
         struct soc_camera_platform_info *p = pdev->dev.platform_data;
-        struct soc_camera_device *icd = to_soc_camera_dev(p->dev);
+        struct soc_camera_device *icd = p->icd;
 
         v4l2_device_unregister_subdev(&priv->subdev);
         icd->ops = NULL;
diff --git a/drivers/media/video/sr030pc30.c b/drivers/media/video/sr030pc30.c
index c901721a1db..8afb0e8a2e0 100644
--- a/drivers/media/video/sr030pc30.c
+++ b/drivers/media/video/sr030pc30.c
@@ -726,8 +726,10 @@ static int sr030pc30_s_power(struct v4l2_subdev *sd, int on)
         const struct sr030pc30_platform_data *pdata = info->pdata;
         int ret;
 
-        if (WARN(pdata == NULL, "No platform data!\n"))
-                return -ENOMEM;
+        if (pdata == NULL) {
+                WARN(1, "No platform data!\n");
+                return -EINVAL;
+        }
 
         /*
          * Put sensor into power sleep mode before switching off
@@ -746,6 +748,7 @@ static int sr030pc30_s_power(struct v4l2_subdev *sd, int on)
         if (on) {
                 ret = sr030pc30_base_config(sd);
         } else {
+                ret = 0;
                 info->curr_win = NULL;
                 info->curr_fmt = NULL;
         }
diff --git a/drivers/media/video/tda7432.c b/drivers/media/video/tda7432.c
index 3941f954daf..bd218545da9 100644
--- a/drivers/media/video/tda7432.c
+++ b/drivers/media/video/tda7432.c
@@ -49,10 +49,11 @@ static int maxvol;
 static int loudness; /* disable loudness by default */
 static int debug;        /* insmod parameter */
 module_param(debug, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "Set debugging level from 0 to 3. Default is off(0).");
 module_param(loudness, int, S_IRUGO);
-MODULE_PARM_DESC(maxvol,"Set maximium volume to +20db (0), default is 0db(1)");
+MODULE_PARM_DESC(loudness, "Turn loudness on(1) else off(0). Default is off(0).");
 module_param(maxvol, int, S_IRUGO | S_IWUSR);
-
+MODULE_PARM_DESC(maxvol, "Set maximium volume to +20dB(0) else +0dB(1). Default is +20dB(0).");
 
 
 /* Structure of address and subaddresses for the tda7432 */
diff --git a/drivers/media/video/timblogiw.c b/drivers/media/video/timblogiw.c
index fc611ebeb82..84cd1b65b76 100644
--- a/drivers/media/video/timblogiw.c
+++ b/drivers/media/video/timblogiw.c
@@ -20,7 +20,6 @@
  * Timberdale FPGA LogiWin Video In
  */
 
-#include <linux/version.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/dmaengine.h>
diff --git a/drivers/media/video/tlg2300/pd-common.h b/drivers/media/video/tlg2300/pd-common.h
index 46066bdc73f..56564e6aaac 100644
--- a/drivers/media/video/tlg2300/pd-common.h
+++ b/drivers/media/video/tlg2300/pd-common.h
@@ -1,7 +1,6 @@
 #ifndef PD_COMMON_H
 #define PD_COMMON_H
 
-#include <linux/version.h>
 #include <linux/fs.h>
 #include <linux/wait.h>
 #include <linux/list.h>
diff --git a/drivers/media/video/tlg2300/pd-main.c b/drivers/media/video/tlg2300/pd-main.c
index 99c81a9a4f4..129f135d5a5 100644
--- a/drivers/media/video/tlg2300/pd-main.c
+++ b/drivers/media/video/tlg2300/pd-main.c
@@ -531,3 +531,4 @@ module_exit(poseidon_exit);
 MODULE_AUTHOR("Telegent Systems");
 MODULE_DESCRIPTION("For tlg2300-based USB device ");
 MODULE_LICENSE("GPL");
+MODULE_VERSION("0.0.2");
diff --git a/drivers/media/video/tlg2300/pd-radio.c b/drivers/media/video/tlg2300/pd-radio.c
index fae84c2a0c3..4fad1dfb92c 100644
--- a/drivers/media/video/tlg2300/pd-radio.c
+++ b/drivers/media/video/tlg2300/pd-radio.c
@@ -6,7 +6,6 @@
 #include <linux/usb.h>
 #include <linux/i2c.h>
 #include <media/v4l2-dev.h>
-#include <linux/version.h>
 #include <linux/mm.h>
 #include <linux/mutex.h>
 #include <media/v4l2-ioctl.h>
@@ -149,7 +148,6 @@ static int vidioc_querycap(struct file *file, void *priv,
         strlcpy(v->driver, "tele-radio", sizeof(v->driver));
         strlcpy(v->card, "Telegent Poseidon", sizeof(v->card));
         usb_make_path(p->udev, v->bus_info, sizeof(v->bus_info));
-        v->version = KERNEL_VERSION(0, 0, 1);
         v->capabilities = V4L2_CAP_TUNER | V4L2_CAP_RADIO;
         return 0;
 }
diff --git a/drivers/media/video/tuner-core.c b/drivers/media/video/tuner-core.c
index a03945ab9f0..11cc980b0cd 100644
--- a/drivers/media/video/tuner-core.c
+++ b/drivers/media/video/tuner-core.c
@@ -39,6 +39,7 @@
 #include "tda9887.h"
 #include "xc5000.h"
 #include "tda18271.h"
+#include "xc4000.h"
 
 #define UNSET (-1U)
 
@@ -391,6 +392,23 @@ static void set_type(struct i2c_client *c, unsigned int type,
                 tune_now = 0;
                 break;
         }
+        case TUNER_XC4000:
+        {
+                struct xc4000_config xc4000_cfg = {
+                        .i2c_address      = t->i2c->addr,
+                        /* FIXME: the correct parameters will be set */
+                        /* only when the digital dvb_attach() occurs */
+                        .default_pm       = 0,
+                        .dvb_amplitude    = 0,
+                        .set_smoothedcvbs = 0,
+                        .if_khz           = 0
+                };
+                if (!dvb_attach(xc4000_attach,
+                                &t->fe, t->i2c->adapter, &xc4000_cfg))
+                        goto attach_failed;
+                tune_now = 0;
+                break;
+        }
         default:
                 if (!dvb_attach(simple_tuner_attach, &t->fe,
                                 t->i2c->adapter, t->i2c->addr, t->type))
diff --git a/drivers/media/video/tw9910.c b/drivers/media/video/tw9910.c
index 0347bbe3645..742482e3001 100644
--- a/drivers/media/video/tw9910.c
+++ b/drivers/media/video/tw9910.c
@@ -552,16 +552,6 @@ static int tw9910_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
         return ret;
 }
 
-static int tw9910_enum_input(struct soc_camera_device *icd,
-                             struct v4l2_input *inp)
-{
-        inp->type = V4L2_INPUT_TYPE_TUNER;
-        inp->std  = V4L2_STD_UNKNOWN;
-        strcpy(inp->name, "Video");
-
-        return 0;
-}
-
 static int tw9910_g_chip_ident(struct v4l2_subdev *sd,
                                struct v4l2_dbg_chip_ident *id)
 {
@@ -846,13 +836,9 @@ static int tw9910_video_probe(struct soc_camera_device *icd,
         struct tw9910_priv *priv = to_tw9910(client);
         s32 id;
 
-        /*
-         * We must have a parent by now. And it cannot be a wrong one.
-         * So this entire test is completely redundant.
-         */
-        if (!icd->dev.parent ||
-            to_soc_camera_host(icd->dev.parent)->nr != icd->iface)
-                return -ENODEV;
+        /* We must have a parent by now. And it cannot be a wrong one. */
+        BUG_ON(!icd->parent ||
+               to_soc_camera_host(icd->parent)->nr != icd->iface);
 
         /*
          * tw9910 only use 8 or 16 bit bus width
@@ -891,7 +877,6 @@ static int tw9910_video_probe(struct soc_camera_device *icd,
 static struct soc_camera_ops tw9910_ops = {
         .set_bus_param          = tw9910_set_bus_param,
         .query_bus_param        = tw9910_query_bus_param,
-        .enum_input             = tw9910_enum_input,
 };
 
 static struct v4l2_subdev_core_ops tw9910_subdev_core_ops = {
diff --git a/drivers/media/video/usbvision/usbvision-video.c b/drivers/media/video/usbvision/usbvision-video.c
index ea8ea8a48df..5a74f5e07d7 100644
--- a/drivers/media/video/usbvision/usbvision-video.c
+++ b/drivers/media/video/usbvision/usbvision-video.c
@@ -45,7 +45,6 @@
  *
  */
 
-#include <linux/version.h>
 #include <linux/kernel.h>
 #include <linux/list.h>
 #include <linux/timer.h>
@@ -77,15 +76,7 @@
 #define DRIVER_ALIAS "USBVision"
 #define DRIVER_DESC "USBVision USB Video Device Driver for Linux"
 #define DRIVER_LICENSE "GPL"
-#define USBVISION_DRIVER_VERSION_MAJOR 0
-#define USBVISION_DRIVER_VERSION_MINOR 9
-#define USBVISION_DRIVER_VERSION_PATCHLEVEL 10
-#define USBVISION_DRIVER_VERSION KERNEL_VERSION(USBVISION_DRIVER_VERSION_MAJOR,\
-USBVISION_DRIVER_VERSION_MINOR,\
-USBVISION_DRIVER_VERSION_PATCHLEVEL)
-#define USBVISION_VERSION_STRING __stringify(USBVISION_DRIVER_VERSION_MAJOR) \
-"." __stringify(USBVISION_DRIVER_VERSION_MINOR) \
-"." __stringify(USBVISION_DRIVER_VERSION_PATCHLEVEL)
+#define USBVISION_VERSION_STRING "0.9.11"
 
 #define ENABLE_HEXDUMP  0       /* Enable if you need it */
 
@@ -516,7 +507,6 @@ static int vidioc_querycap(struct file *file, void  *priv,
                 usbvision_device_data[usbvision->dev_model].model_string,
                 sizeof(vc->card));
         usb_make_path(usbvision->dev, vc->bus_info, sizeof(vc->bus_info));
-        vc->version = USBVISION_DRIVER_VERSION;
         vc->capabilities = V4L2_CAP_VIDEO_CAPTURE |
                 V4L2_CAP_AUDIO |
                 V4L2_CAP_READWRITE |
diff --git a/drivers/media/video/uvc/uvc_ctrl.c b/drivers/media/video/uvc/uvc_ctrl.c
index 2c8954ec685..10c2364f3e8 100644
--- a/drivers/media/video/uvc/uvc_ctrl.c
+++ b/drivers/media/video/uvc/uvc_ctrl.c
@@ -1664,8 +1664,8 @@ int uvc_ctrl_add_mapping(struct uvc_video_chain *chain,
                 return -EINVAL;
         }
 
-        /* Search for the matching (GUID/CS) control in the given device */
-        list_for_each_entry(entity, &dev->entities, list) {
+        /* Search for the matching (GUID/CS) control on the current chain */
+        list_for_each_entry(entity, &chain->entities, chain) {
                 unsigned int i;
 
                 if (UVC_ENTITY_TYPE(entity) != UVC_VC_EXTENSION_UNIT ||
diff --git a/drivers/media/video/uvc/uvc_driver.c b/drivers/media/video/uvc/uvc_driver.c
index b6eae48d7fb..d29f9c2d085 100644
--- a/drivers/media/video/uvc/uvc_driver.c
+++ b/drivers/media/video/uvc/uvc_driver.c
@@ -31,6 +31,7 @@
 #include <linux/videodev2.h>
 #include <linux/vmalloc.h>
 #include <linux/wait.h>
+#include <linux/version.h>
 #include <asm/atomic.h>
 #include <asm/unaligned.h>
 
@@ -1857,7 +1858,7 @@ static int uvc_probe(struct usb_interface *intf,
                         sizeof(dev->mdev.serial));
         strcpy(dev->mdev.bus_info, udev->devpath);
         dev->mdev.hw_revision = le16_to_cpu(udev->descriptor.bcdDevice);
-        dev->mdev.driver_version = DRIVER_VERSION_NUMBER;
+        dev->mdev.driver_version = LINUX_VERSION_CODE;
         if (media_device_register(&dev->mdev) < 0)
                 goto error;
 
@@ -2130,6 +2131,15 @@ static struct usb_device_id uvc_ids[] = {
           .bInterfaceProtocol   = 0,
           .driver_info          = UVC_QUIRK_PROBE_MINMAX
                                 | UVC_QUIRK_BUILTIN_ISIGHT },
+        /* Foxlink ("HP Webcam" on HP Mini 5103) */
+        { .match_flags          = USB_DEVICE_ID_MATCH_DEVICE
+                                | USB_DEVICE_ID_MATCH_INT_INFO,
+          .idVendor             = 0x05c8,
+          .idProduct            = 0x0403,
+          .bInterfaceClass      = USB_CLASS_VIDEO,
+          .bInterfaceSubClass   = 1,
+          .bInterfaceProtocol   = 0,
+          .driver_info          = UVC_QUIRK_FIX_BANDWIDTH },
         /* Genesys Logic USB 2.0 PC Camera */
         { .match_flags          = USB_DEVICE_ID_MATCH_DEVICE
                                 | USB_DEVICE_ID_MATCH_INT_INFO,
diff --git a/drivers/media/video/uvc/uvc_v4l2.c b/drivers/media/video/uvc/uvc_v4l2.c
index dde6533e8e6..ea71d5f1f6d 100644
--- a/drivers/media/video/uvc/uvc_v4l2.c
+++ b/drivers/media/video/uvc/uvc_v4l2.c
@@ -83,7 +83,7 @@ static int uvc_ioctl_ctrl_map(struct uvc_video_chain *chain,
         default:
                 uvc_trace(UVC_TRACE_CONTROL, "Unsupported V4L2 control type "
                           "%u.\n", xmap->v4l2_type);
-                ret = -EINVAL;
+                ret = -ENOTTY;
                 goto done;
         }
 
@@ -571,7 +571,7 @@ static long uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
                 strlcpy(cap->card, vdev->name, sizeof cap->card);
                 usb_make_path(stream->dev->udev,
                               cap->bus_info, sizeof(cap->bus_info));
-                cap->version = DRIVER_VERSION_NUMBER;
+                cap->version = LINUX_VERSION_CODE;
                 if (stream->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
                         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE
                                           | V4L2_CAP_STREAMING;
diff --git a/drivers/media/video/uvc/uvcvideo.h b/drivers/media/video/uvc/uvcvideo.h
index 20107fd3574..df32a43ca86 100644
--- a/drivers/media/video/uvc/uvcvideo.h
+++ b/drivers/media/video/uvc/uvcvideo.h
@@ -183,8 +183,7 @@ struct uvc_xu_control {
  * Driver specific constants.
  */
 
-#define DRIVER_VERSION_NUMBER   KERNEL_VERSION(1, 1, 0)
-#define DRIVER_VERSION          "v1.1.0"
+#define DRIVER_VERSION          "1.1.1"
 
 /* Number of isochronous URBs. */
 #define UVC_URBS                5
diff --git a/drivers/media/video/v4l2-common.c b/drivers/media/video/v4l2-common.c
index 06b9f9f8201..5c6100fb407 100644
--- a/drivers/media/video/v4l2-common.c
+++ b/drivers/media/video/v4l2-common.c
@@ -105,6 +105,9 @@ int v4l2_ctrl_check(struct v4l2_ext_control *ctrl, struct v4l2_queryctrl *qctrl,
                     menu_items[ctrl->value][0] == '\0')
                         return -EINVAL;
         }
+        if (qctrl->type == V4L2_CTRL_TYPE_BITMASK &&
+                        (ctrl->value & ~qctrl->maximum))
+                return -ERANGE;
         return 0;
 }
 EXPORT_SYMBOL(v4l2_ctrl_check);
diff --git a/drivers/media/video/v4l2-compat-ioctl32.c b/drivers/media/video/v4l2-compat-ioctl32.c
index 7c2694738b3..61979b70f38 100644
--- a/drivers/media/video/v4l2-compat-ioctl32.c
+++ b/drivers/media/video/v4l2-compat-ioctl32.c
@@ -662,6 +662,32 @@ static int put_v4l2_ext_controls32(struct v4l2_ext_controls *kp, struct v4l2_ext
         return 0;
 }
 
+struct v4l2_event32 {
+        __u32                           type;
+        union {
+                __u8                    data[64];
+        } u;
+        __u32                           pending;
+        __u32                           sequence;
+        struct compat_timespec          timestamp;
+        __u32                           id;
+        __u32                           reserved[8];
+};
+
+static int put_v4l2_event32(struct v4l2_event *kp, struct v4l2_event32 __user *up)
+{
+        if (!access_ok(VERIFY_WRITE, up, sizeof(struct v4l2_event32)) ||
+                put_user(kp->type, &up->type) ||
+                copy_to_user(&up->u, &kp->u, sizeof(kp->u)) ||
+                put_user(kp->pending, &up->pending) ||
+                put_user(kp->sequence, &up->sequence) ||
+                put_compat_timespec(&kp->timestamp, &up->timestamp) ||
+                put_user(kp->id, &up->id) ||
+                copy_to_user(up->reserved, kp->reserved, 8 * sizeof(__u32)))
+                        return -EFAULT;
+        return 0;
+}
+
 #define VIDIOC_G_FMT32          _IOWR('V',  4, struct v4l2_format32)
 #define VIDIOC_S_FMT32          _IOWR('V',  5, struct v4l2_format32)
 #define VIDIOC_QUERYBUF32       _IOWR('V',  9, struct v4l2_buffer32)
@@ -675,6 +701,7 @@ static int put_v4l2_ext_controls32(struct v4l2_ext_controls *kp, struct v4l2_ext
 #define VIDIOC_G_EXT_CTRLS32    _IOWR('V', 71, struct v4l2_ext_controls32)
 #define VIDIOC_S_EXT_CTRLS32    _IOWR('V', 72, struct v4l2_ext_controls32)
 #define VIDIOC_TRY_EXT_CTRLS32  _IOWR('V', 73, struct v4l2_ext_controls32)
+#define VIDIOC_DQEVENT32        _IOR ('V', 89, struct v4l2_event32)
 
 #define VIDIOC_OVERLAY32        _IOW ('V', 14, s32)
 #define VIDIOC_STREAMON32       _IOW ('V', 18, s32)
@@ -693,6 +720,7 @@ static long do_video_ioctl(struct file *file, unsigned int cmd, unsigned long ar
                 struct v4l2_input v2i;
                 struct v4l2_standard v2s;
                 struct v4l2_ext_controls v2ecs;
+                struct v4l2_event v2ev;
                 unsigned long vx;
                 int vi;
         } karg;
@@ -715,6 +743,7 @@ static long do_video_ioctl(struct file *file, unsigned int cmd, unsigned long ar
         case VIDIOC_G_EXT_CTRLS32: cmd = VIDIOC_G_EXT_CTRLS; break;
         case VIDIOC_S_EXT_CTRLS32: cmd = VIDIOC_S_EXT_CTRLS; break;
         case VIDIOC_TRY_EXT_CTRLS32: cmd = VIDIOC_TRY_EXT_CTRLS; break;
+        case VIDIOC_DQEVENT32: cmd = VIDIOC_DQEVENT; break;
         case VIDIOC_OVERLAY32: cmd = VIDIOC_OVERLAY; break;
         case VIDIOC_STREAMON32: cmd = VIDIOC_STREAMON; break;
         case VIDIOC_STREAMOFF32: cmd = VIDIOC_STREAMOFF; break;
@@ -778,6 +807,9 @@ static long do_video_ioctl(struct file *file, unsigned int cmd, unsigned long ar
                 err = get_v4l2_ext_controls32(&karg.v2ecs, up);
                 compatible_arg = 0;
                 break;
+        case VIDIOC_DQEVENT:
+                compatible_arg = 0;
+                break;
         }
         if (err)
                 return err;
@@ -818,6 +850,10 @@ static long do_video_ioctl(struct file *file, unsigned int cmd, unsigned long ar
                 err = put_v4l2_framebuffer32(&karg.v2fb, up);
                 break;
 
+        case VIDIOC_DQEVENT:
+                err = put_v4l2_event32(&karg.v2ev, up);
+                break;
+
         case VIDIOC_G_FMT:
         case VIDIOC_S_FMT:
         case VIDIOC_TRY_FMT:
@@ -920,6 +956,7 @@ long v4l2_compat_ioctl32(struct file *file, unsigned int cmd, unsigned long arg)
         case VIDIOC_S_DV_TIMINGS:
         case VIDIOC_G_DV_TIMINGS:
         case VIDIOC_DQEVENT:
+        case VIDIOC_DQEVENT32:
         case VIDIOC_SUBSCRIBE_EVENT:
         case VIDIOC_UNSUBSCRIBE_EVENT:
                 ret = do_video_ioctl(file, cmd, arg);
diff --git a/drivers/media/video/v4l2-ctrls.c b/drivers/media/video/v4l2-ctrls.c
index 2412f08527a..06b6014d4fb 100644
--- a/drivers/media/video/v4l2-ctrls.c
+++ b/drivers/media/video/v4l2-ctrls.c
@@ -23,17 +23,39 @@
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 #include <media/v4l2-dev.h>
 
+#define has_op(master, op) \
+        (master->ops && master->ops->op)
+#define call_op(master, op) \
+        (has_op(master, op) ? master->ops->op(master) : 0)
+
 /* Internal temporary helper struct, one for each v4l2_ext_control */
-struct ctrl_helper {
+struct v4l2_ctrl_helper {
+        /* Pointer to the control reference of the master control */
+        struct v4l2_ctrl_ref *mref;
         /* The control corresponding to the v4l2_ext_control ID field. */
         struct v4l2_ctrl *ctrl;
-        /* Used internally to mark whether this control was already
-           processed. */
-        bool handled;
+        /* v4l2_ext_control index of the next control belonging to the
+           same cluster, or 0 if there isn't any. */
+        u32 next;
 };
 
+/* Small helper function to determine if the autocluster is set to manual
+   mode. In that case the is_volatile flag should be ignored. */
+static bool is_cur_manual(const struct v4l2_ctrl *master)
+{
+        return master->is_auto && master->cur.val == master->manual_mode_value;
+}
+
+/* Same as above, but this checks the against the new value instead of the
+   current value. */
+static bool is_new_manual(const struct v4l2_ctrl *master)
+{
+        return master->is_auto && master->val == master->manual_mode_value;
+}
+
 /* Returns NULL or a character pointer array containing the menu for
    the given control ID. The pointer array ends with a NULL pointer.
    An empty string signifies a menu entry that is invalid. This allows
@@ -181,7 +203,7 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
         };
         static const char * const mpeg_stream_vbi_fmt[] = {
                 "No VBI",
-                "Private packet, IVTV format",
+                "Private Packet, IVTV Format",
                 NULL
         };
         static const char * const camera_power_line_frequency[] = {
@@ -204,18 +226,130 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
                 "Negative",
                 "Emboss",
                 "Sketch",
-                "Sky blue",
-                "Grass green",
-                "Skin whiten",
+                "Sky Blue",
+                "Grass Green",
+                "Skin Whiten",
                 "Vivid",
                 NULL
         };
         static const char * const tune_preemphasis[] = {
-                "No preemphasis",
+                "No Preemphasis",
                 "50 useconds",
                 "75 useconds",
                 NULL,
         };
+        static const char * const header_mode[] = {
+                "Separate Buffer",
+                "Joined With 1st Frame",
+                NULL,
+        };
+        static const char * const multi_slice[] = {
+                "Single",
+                "Max Macroblocks",
+                "Max Bytes",
+                NULL,
+        };
+        static const char * const entropy_mode[] = {
+                "CAVLC",
+                "CABAC",
+                NULL,
+        };
+        static const char * const mpeg_h264_level[] = {
+                "1",
+                "1b",
+                "1.1",
+                "1.2",
+                "1.3",
+                "2",
+                "2.1",
+                "2.2",
+                "3",
+                "3.1",
+                "3.2",
+                "4",
+                "4.1",
+                "4.2",
+                "5",
+                "5.1",
+                NULL,
+        };
+        static const char * const h264_loop_filter[] = {
+                "Enabled",
+                "Disabled",
+                "Disabled at Slice Boundary",
+                NULL,
+        };
+        static const char * const h264_profile[] = {
+                "Baseline",
+                "Constrained Baseline",
+                "Main",
+                "Extended",
+                "High",
+                "High 10",
+                "High 422",
+                "High 444 Predictive",
+                "High 10 Intra",
+                "High 422 Intra",
+                "High 444 Intra",
+                "CAVLC 444 Intra",
+                "Scalable Baseline",
+                "Scalable High",
+                "Scalable High Intra",
+                "Multiview High",
+                NULL,
+        };
+        static const char * const vui_sar_idc[] = {
+                "Unspecified",
+                "1:1",
+                "12:11",
+                "10:11",
+                "16:11",
+                "40:33",
+                "24:11",
+                "20:11",
+                "32:11",
+                "80:33",
+                "18:11",
+                "15:11",
+                "64:33",
+                "160:99",
+                "4:3",
+                "3:2",
+                "2:1",
+                "Extended SAR",
+                NULL,
+        };
+        static const char * const mpeg_mpeg4_level[] = {
+                "0",
+                "0b",
+                "1",
+                "2",
+                "3",
+                "3b",
+                "4",
+                "5",
+                NULL,
+        };
+        static const char * const mpeg4_profile[] = {
+                "Simple",
+                "Adcanved Simple",
+                "Core",
+                "Simple Scalable",
+                "Advanced Coding Efficency",
+                NULL,
+        };
+
+        static const char * const flash_led_mode[] = {
+                "Off",
+                "Flash",
+                "Torch",
+                NULL,
+        };
+        static const char * const flash_strobe_source[] = {
+                "Software",
+                "External",
+                NULL,
+        };
 
         switch (id) {
         case V4L2_CID_MPEG_AUDIO_SAMPLING_FREQ:
@@ -256,6 +390,28 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
                 return colorfx;
         case V4L2_CID_TUNE_PREEMPHASIS:
                 return tune_preemphasis;
+        case V4L2_CID_FLASH_LED_MODE:
+                return flash_led_mode;
+        case V4L2_CID_FLASH_STROBE_SOURCE:
+                return flash_strobe_source;
+        case V4L2_CID_MPEG_VIDEO_HEADER_MODE:
+                return header_mode;
+        case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE:
+                return multi_slice;
+        case V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE:
+                return entropy_mode;
+        case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
+                return mpeg_h264_level;
+        case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE:
+                return h264_loop_filter;
+        case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
+                return h264_profile;
+        case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_IDC:
+                return vui_sar_idc;
+        case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:
+                return mpeg_mpeg4_level;
+        case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:
+                return mpeg4_profile;
         default:
                 return NULL;
         }
@@ -307,6 +463,8 @@ const char *v4l2_ctrl_get_name(u32 id)
         case V4L2_CID_CHROMA_GAIN:              return "Chroma Gain";
         case V4L2_CID_ILLUMINATORS_1:           return "Illuminator 1";
         case V4L2_CID_ILLUMINATORS_2:           return "Illuminator 2";
+        case V4L2_CID_MIN_BUFFERS_FOR_CAPTURE:  return "Minimum Number of Capture Buffers";
+        case V4L2_CID_MIN_BUFFERS_FOR_OUTPUT:   return "Minimum Number of Output Buffers";
 
         /* MPEG controls */
         /* Keep the order of the 'case's the same as in videodev2.h! */
@@ -343,6 +501,48 @@ const char *v4l2_ctrl_get_name(u32 id)
         case V4L2_CID_MPEG_VIDEO_TEMPORAL_DECIMATION: return "Video Temporal Decimation";
         case V4L2_CID_MPEG_VIDEO_MUTE:          return "Video Mute";
         case V4L2_CID_MPEG_VIDEO_MUTE_YUV:      return "Video Mute YUV";
+        case V4L2_CID_MPEG_VIDEO_DECODER_SLICE_INTERFACE:       return "Decoder Slice Interface";
+        case V4L2_CID_MPEG_VIDEO_DECODER_MPEG4_DEBLOCK_FILTER:  return "MPEG4 Loop Filter Enable";
+        case V4L2_CID_MPEG_VIDEO_CYCLIC_INTRA_REFRESH_MB:       return "The Number of Intra Refresh MBs";
+        case V4L2_CID_MPEG_VIDEO_FRAME_RC_ENABLE:               return "Frame Level Rate Control Enable";
+        case V4L2_CID_MPEG_VIDEO_MB_RC_ENABLE:                  return "H264 MB Level Rate Control";
+        case V4L2_CID_MPEG_VIDEO_HEADER_MODE:                   return "Sequence Header Mode";
+        case V4L2_CID_MPEG_VIDEO_MAX_REF_PIC:                   return "The Max Number of Reference Picture";
+        case V4L2_CID_MPEG_VIDEO_H263_I_FRAME_QP:               return "H263 I-Frame QP Value";
+        case V4L2_CID_MPEG_VIDEO_H263_P_FRAME_QP:               return "H263 P frame QP Value";
+        case V4L2_CID_MPEG_VIDEO_H263_B_FRAME_QP:               return "H263 B frame QP Value";
+        case V4L2_CID_MPEG_VIDEO_H263_MIN_QP:                   return "H263 Minimum QP Value";
+        case V4L2_CID_MPEG_VIDEO_H263_MAX_QP:                   return "H263 Maximum QP Value";
+        case V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP:               return "H264 I-Frame QP Value";
+        case V4L2_CID_MPEG_VIDEO_H264_P_FRAME_QP:               return "H264 P frame QP Value";
+        case V4L2_CID_MPEG_VIDEO_H264_B_FRAME_QP:               return "H264 B frame QP Value";
+        case V4L2_CID_MPEG_VIDEO_H264_MAX_QP:                   return "H264 Maximum QP Value";
+        case V4L2_CID_MPEG_VIDEO_H264_MIN_QP:                   return "H264 Minimum QP Value";
+        case V4L2_CID_MPEG_VIDEO_H264_8X8_TRANSFORM:            return "H264 8x8 Transform Enable";
+        case V4L2_CID_MPEG_VIDEO_H264_CPB_SIZE:                 return "H264 CPB Buffer Size";
+        case V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE:             return "H264 Entorpy Mode";
+        case V4L2_CID_MPEG_VIDEO_H264_I_PERIOD:                 return "H264 I Period";
+        case V4L2_CID_MPEG_VIDEO_H264_LEVEL:                    return "H264 Level";
+        case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA:        return "H264 Loop Filter Alpha Offset";
+        case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA:         return "H264 Loop Filter Beta Offset";
+        case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE:         return "H264 Loop Filter Mode";
+        case V4L2_CID_MPEG_VIDEO_H264_PROFILE:                  return "H264 Profile";
+        case V4L2_CID_MPEG_VIDEO_H264_VUI_EXT_SAR_HEIGHT:       return "Vertical Size of SAR";
+        case V4L2_CID_MPEG_VIDEO_H264_VUI_EXT_SAR_WIDTH:        return "Horizontal Size of SAR";
+        case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_ENABLE:           return "Aspect Ratio VUI Enable";
+        case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_IDC:              return "VUI Aspect Ratio IDC";
+        case V4L2_CID_MPEG_VIDEO_MPEG4_I_FRAME_QP:              return "MPEG4 I-Frame QP Value";
+        case V4L2_CID_MPEG_VIDEO_MPEG4_P_FRAME_QP:              return "MPEG4 P frame QP Value";
+        case V4L2_CID_MPEG_VIDEO_MPEG4_B_FRAME_QP:              return "MPEG4 B frame QP Value";
+        case V4L2_CID_MPEG_VIDEO_MPEG4_MIN_QP:                  return "MPEG4 Minimum QP Value";
+        case V4L2_CID_MPEG_VIDEO_MPEG4_MAX_QP:                  return "MPEG4 Maximum QP Value";
+        case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:                   return "MPEG4 Level";
+        case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:                 return "MPEG4 Profile";
+        case V4L2_CID_MPEG_VIDEO_MPEG4_QPEL:                    return "Quarter Pixel Search Enable";
+        case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_BYTES:         return "The Maximum Bytes Per Slice";
+        case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_MB:            return "The Number of MB in a Slice";
+        case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE:              return "The Slice Partitioning Method";
+        case V4L2_CID_MPEG_VIDEO_VBV_SIZE:                      return "VBV Buffer Size";
 
         /* CAMERA controls */
         /* Keep the order of the 'case's the same as in videodev2.h! */
@@ -389,6 +589,21 @@ const char *v4l2_ctrl_get_name(u32 id)
         case V4L2_CID_TUNE_POWER_LEVEL:         return "Tune Power Level";
         case V4L2_CID_TUNE_ANTENNA_CAPACITOR:   return "Tune Antenna Capacitor";
 
+        /* Flash controls */
+        case V4L2_CID_FLASH_CLASS:              return "Flash controls";
+        case V4L2_CID_FLASH_LED_MODE:           return "LED mode";
+        case V4L2_CID_FLASH_STROBE_SOURCE:      return "Strobe source";
+        case V4L2_CID_FLASH_STROBE:             return "Strobe";
+        case V4L2_CID_FLASH_STROBE_STOP:        return "Stop strobe";
+        case V4L2_CID_FLASH_STROBE_STATUS:      return "Strobe status";
+        case V4L2_CID_FLASH_TIMEOUT:            return "Strobe timeout";
+        case V4L2_CID_FLASH_INTENSITY:          return "Intensity, flash mode";
+        case V4L2_CID_FLASH_TORCH_INTENSITY:    return "Intensity, torch mode";
+        case V4L2_CID_FLASH_INDICATOR_INTENSITY: return "Intensity, indicator";
+        case V4L2_CID_FLASH_FAULT:              return "Faults";
+        case V4L2_CID_FLASH_CHARGE:             return "Charge";
+        case V4L2_CID_FLASH_READY:              return "Ready to strobe";
+
         default:
                 return NULL;
         }
@@ -423,12 +638,24 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
         case V4L2_CID_PILOT_TONE_ENABLED:
         case V4L2_CID_ILLUMINATORS_1:
         case V4L2_CID_ILLUMINATORS_2:
+        case V4L2_CID_FLASH_STROBE_STATUS:
+        case V4L2_CID_FLASH_CHARGE:
+        case V4L2_CID_FLASH_READY:
+        case V4L2_CID_MPEG_VIDEO_DECODER_MPEG4_DEBLOCK_FILTER:
+        case V4L2_CID_MPEG_VIDEO_DECODER_SLICE_INTERFACE:
+        case V4L2_CID_MPEG_VIDEO_FRAME_RC_ENABLE:
+        case V4L2_CID_MPEG_VIDEO_MB_RC_ENABLE:
+        case V4L2_CID_MPEG_VIDEO_H264_8X8_TRANSFORM:
+        case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_ENABLE:
+        case V4L2_CID_MPEG_VIDEO_MPEG4_QPEL:
                 *type = V4L2_CTRL_TYPE_BOOLEAN;
                 *min = 0;
                 *max = *step = 1;
                 break;
         case V4L2_CID_PAN_RESET:
         case V4L2_CID_TILT_RESET:
+        case V4L2_CID_FLASH_STROBE:
+        case V4L2_CID_FLASH_STROBE_STOP:
                 *type = V4L2_CTRL_TYPE_BUTTON;
                 *flags |= V4L2_CTRL_FLAG_WRITE_ONLY;
                 *min = *max = *step = *def = 0;
@@ -452,6 +679,17 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
         case V4L2_CID_EXPOSURE_AUTO:
         case V4L2_CID_COLORFX:
         case V4L2_CID_TUNE_PREEMPHASIS:
+        case V4L2_CID_FLASH_LED_MODE:
+        case V4L2_CID_FLASH_STROBE_SOURCE:
+        case V4L2_CID_MPEG_VIDEO_HEADER_MODE:
+        case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE:
+        case V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE:
+        case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
+        case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE:
+        case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
+        case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_IDC:
+        case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:
+        case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:
                 *type = V4L2_CTRL_TYPE_MENU;
                 break;
         case V4L2_CID_RDS_TX_PS_NAME:
@@ -462,6 +700,7 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
         case V4L2_CID_CAMERA_CLASS:
         case V4L2_CID_MPEG_CLASS:
         case V4L2_CID_FM_TX_CLASS:
+        case V4L2_CID_FLASH_CLASS:
                 *type = V4L2_CTRL_TYPE_CTRL_CLASS;
                 /* You can neither read not write these */
                 *flags |= V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY;
@@ -474,6 +713,14 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
                 /* Max is calculated as RGB888 that is 2^24 */
                 *max = 0xFFFFFF;
                 break;
+        case V4L2_CID_FLASH_FAULT:
+                *type = V4L2_CTRL_TYPE_BITMASK;
+                break;
+        case V4L2_CID_MIN_BUFFERS_FOR_CAPTURE:
+        case V4L2_CID_MIN_BUFFERS_FOR_OUTPUT:
+                *type = V4L2_CTRL_TYPE_INTEGER;
+                *flags |= V4L2_CTRL_FLAG_READ_ONLY;
+                break;
         default:
                 *type = V4L2_CTRL_TYPE_INTEGER;
                 break;
@@ -519,6 +766,10 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
         case V4L2_CID_ZOOM_RELATIVE:
                 *flags |= V4L2_CTRL_FLAG_WRITE_ONLY;
                 break;
+        case V4L2_CID_FLASH_STROBE_STATUS:
+        case V4L2_CID_FLASH_READY:
+                *flags |= V4L2_CTRL_FLAG_READ_ONLY;
+                break;
         }
 }
 EXPORT_SYMBOL(v4l2_ctrl_fill);
@@ -537,6 +788,42 @@ static bool type_is_int(const struct v4l2_ctrl *ctrl)
         }
 }
 
+static void fill_event(struct v4l2_event *ev, struct v4l2_ctrl *ctrl, u32 changes)
+{
+        memset(ev->reserved, 0, sizeof(ev->reserved));
+        ev->type = V4L2_EVENT_CTRL;
+        ev->id = ctrl->id;
+        ev->u.ctrl.changes = changes;
+        ev->u.ctrl.type = ctrl->type;
+        ev->u.ctrl.flags = ctrl->flags;
+        if (ctrl->type == V4L2_CTRL_TYPE_STRING)
+                ev->u.ctrl.value64 = 0;
+        else
+                ev->u.ctrl.value64 = ctrl->cur.val64;
+        ev->u.ctrl.minimum = ctrl->minimum;
+        ev->u.ctrl.maximum = ctrl->maximum;
+        if (ctrl->type == V4L2_CTRL_TYPE_MENU)
+                ev->u.ctrl.step = 1;
+        else
+                ev->u.ctrl.step = ctrl->step;
+        ev->u.ctrl.default_value = ctrl->default_value;
+}
+
+static void send_event(struct v4l2_fh *fh, struct v4l2_ctrl *ctrl, u32 changes)
+{
+        struct v4l2_event ev;
+        struct v4l2_subscribed_event *sev;
+
+        if (list_empty(&ctrl->ev_subs))
+                return;
+        fill_event(&ev, ctrl, changes);
+
+        list_for_each_entry(sev, &ctrl->ev_subs, node)
+                if (sev->fh && (sev->fh != fh ||
+                                (sev->flags & V4L2_EVENT_SUB_FL_ALLOW_FEEDBACK)))
+                        v4l2_event_queue_fh(sev->fh, &ev);
+}
+
 /* Helper function: copy the current control value back to the caller */
 static int cur_to_user(struct v4l2_ext_control *c,
                        struct v4l2_ctrl *ctrl)
@@ -624,22 +911,45 @@ static int new_to_user(struct v4l2_ext_control *c,
 }
 
 /* Copy the new value to the current value. */
-static void new_to_cur(struct v4l2_ctrl *ctrl)
+static void new_to_cur(struct v4l2_fh *fh, struct v4l2_ctrl *ctrl,
+                                                bool update_inactive)
 {
+        bool changed = false;
+
         if (ctrl == NULL)
                 return;
         switch (ctrl->type) {
+        case V4L2_CTRL_TYPE_BUTTON:
+                changed = true;
+                break;
         case V4L2_CTRL_TYPE_STRING:
                 /* strings are always 0-terminated */
+                changed = strcmp(ctrl->string, ctrl->cur.string);
                 strcpy(ctrl->cur.string, ctrl->string);
                 break;
         case V4L2_CTRL_TYPE_INTEGER64:
+                changed = ctrl->val64 != ctrl->cur.val64;
                 ctrl->cur.val64 = ctrl->val64;
                 break;
         default:
+                changed = ctrl->val != ctrl->cur.val;
                 ctrl->cur.val = ctrl->val;
                 break;
         }
+        if (update_inactive) {
+                ctrl->flags &= ~V4L2_CTRL_FLAG_INACTIVE;
+                if (!is_cur_manual(ctrl->cluster[0]))
+                        ctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
+        }
+        if (changed || update_inactive) {
+                /* If a control was changed that was not one of the controls
+                   modified by the application, then send the event to all. */
+                if (!ctrl->is_new)
+                        fh = NULL;
+                send_event(fh, ctrl,
+                        (changed ? V4L2_EVENT_CTRL_CH_VALUE : 0) |
+                        (update_inactive ? V4L2_EVENT_CTRL_CH_FLAGS : 0));
+        }
 }
 
 /* Copy the current value to the new value */
@@ -692,13 +1002,11 @@ static int cluster_changed(struct v4l2_ctrl *master)
         return diff;
 }
 
-/* Validate a new control */
-static int validate_new(struct v4l2_ctrl *ctrl)
+/* Validate integer-type control */
+static int validate_new_int(const struct v4l2_ctrl *ctrl, s32 *pval)
 {
-        s32 val = ctrl->val;
-        char *s = ctrl->string;
+        s32 val = *pval;
         u32 offset;
-        size_t len;
 
         switch (ctrl->type) {
         case V4L2_CTRL_TYPE_INTEGER:
@@ -711,11 +1019,11 @@ static int validate_new(struct v4l2_ctrl *ctrl)
                 offset = val - ctrl->minimum;
                 offset = ctrl->step * (offset / ctrl->step);
                 val = ctrl->minimum + offset;
-                ctrl->val = val;
+                *pval = val;
                 return 0;
 
         case V4L2_CTRL_TYPE_BOOLEAN:
-                ctrl->val = !!ctrl->val;
+                *pval = !!val;
                 return 0;
 
         case V4L2_CTRL_TYPE_MENU:
@@ -726,11 +1034,35 @@ static int validate_new(struct v4l2_ctrl *ctrl)
                         return -EINVAL;
                 return 0;
 
+        case V4L2_CTRL_TYPE_BITMASK:
+                *pval &= ctrl->maximum;
+                return 0;
+
         case V4L2_CTRL_TYPE_BUTTON:
         case V4L2_CTRL_TYPE_CTRL_CLASS:
-                ctrl->val64 = 0;
+                *pval = 0;
                 return 0;
 
+        default:
+                return -EINVAL;
+        }
+}
+
+/* Validate a new control */
+static int validate_new(const struct v4l2_ctrl *ctrl, struct v4l2_ext_control *c)
+{
+        char *s = c->string;
+        size_t len;
+
+        switch (ctrl->type) {
+        case V4L2_CTRL_TYPE_INTEGER:
+        case V4L2_CTRL_TYPE_BOOLEAN:
+        case V4L2_CTRL_TYPE_MENU:
+        case V4L2_CTRL_TYPE_BITMASK:
+        case V4L2_CTRL_TYPE_BUTTON:
+        case V4L2_CTRL_TYPE_CTRL_CLASS:
+                return validate_new_int(ctrl, &c->value);
+
         case V4L2_CTRL_TYPE_INTEGER64:
                 return 0;
 
@@ -780,6 +1112,7 @@ void v4l2_ctrl_handler_free(struct v4l2_ctrl_handler *hdl)
 {
         struct v4l2_ctrl_ref *ref, *next_ref;
         struct v4l2_ctrl *ctrl, *next_ctrl;
+        struct v4l2_subscribed_event *sev, *next_sev;
 
         if (hdl == NULL || hdl->buckets == NULL)
                 return;
@@ -793,6 +1126,8 @@ void v4l2_ctrl_handler_free(struct v4l2_ctrl_handler *hdl)
         /* Free all controls owned by the handler */
         list_for_each_entry_safe(ctrl, next_ctrl, &hdl->ctrls, node) {
                 list_del(&ctrl->node);
+                list_for_each_entry_safe(sev, next_sev, &ctrl->ev_subs, node)
+                        list_del(&sev->node);
                 kfree(ctrl);
         }
         kfree(hdl->buckets);
@@ -962,13 +1297,17 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
 
         /* Sanity checks */
         if (id == 0 || name == NULL || id >= V4L2_CID_PRIVATE_BASE ||
-            max < min ||
             (type == V4L2_CTRL_TYPE_INTEGER && step == 0) ||
+            (type == V4L2_CTRL_TYPE_BITMASK && max == 0) ||
             (type == V4L2_CTRL_TYPE_MENU && qmenu == NULL) ||
             (type == V4L2_CTRL_TYPE_STRING && max == 0)) {
                 handler_set_err(hdl, -ERANGE);
                 return NULL;
         }
+        if (type != V4L2_CTRL_TYPE_BITMASK && max < min) {
+                handler_set_err(hdl, -ERANGE);
+                return NULL;
+        }
         if ((type == V4L2_CTRL_TYPE_INTEGER ||
              type == V4L2_CTRL_TYPE_MENU ||
              type == V4L2_CTRL_TYPE_BOOLEAN) &&
@@ -976,6 +1315,10 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
                 handler_set_err(hdl, -ERANGE);
                 return NULL;
         }
+        if (type == V4L2_CTRL_TYPE_BITMASK && ((def & ~max) || min || step)) {
+                handler_set_err(hdl, -ERANGE);
+                return NULL;
+        }
 
         if (type == V4L2_CTRL_TYPE_BUTTON)
                 flags |= V4L2_CTRL_FLAG_WRITE_ONLY;
@@ -991,6 +1334,7 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
         }
 
         INIT_LIST_HEAD(&ctrl->node);
+        INIT_LIST_HEAD(&ctrl->ev_subs);
         ctrl->handler = hdl;
         ctrl->ops = ops;
         ctrl->id = id;
@@ -1132,6 +1476,9 @@ int v4l2_ctrl_add_handler(struct v4l2_ctrl_handler *hdl,
                 /* Skip handler-private controls. */
                 if (ctrl->is_private)
                         continue;
+                /* And control classes */
+                if (ctrl->type == V4L2_CTRL_TYPE_CTRL_CLASS)
+                        continue;
                 ret = handler_new_ref(hdl, ctrl);
                 if (ret)
                         break;
@@ -1147,7 +1494,7 @@ void v4l2_ctrl_cluster(unsigned ncontrols, struct v4l2_ctrl **controls)
         int i;
 
         /* The first control is the master control and it must not be NULL */
-        BUG_ON(controls[0] == NULL);
+        BUG_ON(ncontrols == 0 || controls[0] == NULL);
 
         for (i = 0; i < ncontrols; i++) {
                 if (controls[i]) {
@@ -1158,18 +1505,47 @@ void v4l2_ctrl_cluster(unsigned ncontrols, struct v4l2_ctrl **controls)
 }
 EXPORT_SYMBOL(v4l2_ctrl_cluster);
 
+void v4l2_ctrl_auto_cluster(unsigned ncontrols, struct v4l2_ctrl **controls,
+                            u8 manual_val, bool set_volatile)
+{
+        struct v4l2_ctrl *master = controls[0];
+        u32 flag;
+        int i;
+
+        v4l2_ctrl_cluster(ncontrols, controls);
+        WARN_ON(ncontrols <= 1);
+        WARN_ON(manual_val < master->minimum || manual_val > master->maximum);
+        master->is_auto = true;
+        master->manual_mode_value = manual_val;
+        master->flags |= V4L2_CTRL_FLAG_UPDATE;
+        flag = is_cur_manual(master) ? 0 : V4L2_CTRL_FLAG_INACTIVE;
+
+        for (i = 1; i < ncontrols; i++)
+                if (controls[i]) {
+                        controls[i]->is_volatile = set_volatile;
+                        controls[i]->flags |= flag;
+                }
+}
+EXPORT_SYMBOL(v4l2_ctrl_auto_cluster);
+
 /* Activate/deactivate a control. */
 void v4l2_ctrl_activate(struct v4l2_ctrl *ctrl, bool active)
 {
+        /* invert since the actual flag is called 'inactive' */
+        bool inactive = !active;
+        bool old;
+
         if (ctrl == NULL)
                 return;
 
-        if (!active)
+        if (inactive)
                 /* set V4L2_CTRL_FLAG_INACTIVE */
-                set_bit(4, &ctrl->flags);
+                old = test_and_set_bit(4, &ctrl->flags);
         else
                 /* clear V4L2_CTRL_FLAG_INACTIVE */
-                clear_bit(4, &ctrl->flags);
+                old = test_and_clear_bit(4, &ctrl->flags);
+        if (old != inactive)
+                send_event(NULL, ctrl, V4L2_EVENT_CTRL_CH_FLAGS);
 }
 EXPORT_SYMBOL(v4l2_ctrl_activate);
 
@@ -1181,15 +1557,21 @@ EXPORT_SYMBOL(v4l2_ctrl_activate);
    these controls. */
 void v4l2_ctrl_grab(struct v4l2_ctrl *ctrl, bool grabbed)
 {
+        bool old;
+
         if (ctrl == NULL)
                 return;
 
+        v4l2_ctrl_lock(ctrl);
         if (grabbed)
                 /* set V4L2_CTRL_FLAG_GRABBED */
-                set_bit(1, &ctrl->flags);
+                old = test_and_set_bit(1, &ctrl->flags);
         else
                 /* clear V4L2_CTRL_FLAG_GRABBED */
-                clear_bit(1, &ctrl->flags);
+                old = test_and_clear_bit(1, &ctrl->flags);
+        if (old != grabbed)
+                send_event(NULL, ctrl, V4L2_EVENT_CTRL_CH_FLAGS);
+        v4l2_ctrl_unlock(ctrl);
 }
 EXPORT_SYMBOL(v4l2_ctrl_grab);
 
@@ -1217,6 +1599,9 @@ static void log_ctrl(const struct v4l2_ctrl *ctrl,
         case V4L2_CTRL_TYPE_MENU:
                 printk(KERN_CONT "%s", ctrl->qmenu[ctrl->cur.val]);
                 break;
+        case V4L2_CTRL_TYPE_BITMASK:
+                printk(KERN_CONT "0x%08x", ctrl->cur.val);
+                break;
         case V4L2_CTRL_TYPE_INTEGER64:
                 printk(KERN_CONT "%lld", ctrl->cur.val64);
                 break;
@@ -1277,26 +1662,21 @@ int v4l2_ctrl_handler_setup(struct v4l2_ctrl_handler *hdl)
                 int i;
 
                 /* Skip if this control was already handled by a cluster. */
-                if (ctrl->done)
+                /* Skip button controls and read-only controls. */
+                if (ctrl->done || ctrl->type == V4L2_CTRL_TYPE_BUTTON ||
+                    (ctrl->flags & V4L2_CTRL_FLAG_READ_ONLY))
                         continue;
 
                 for (i = 0; i < master->ncontrols; i++) {
                         if (master->cluster[i]) {
                                 cur_to_new(master->cluster[i]);
                                 master->cluster[i]->is_new = 1;
+                                master->cluster[i]->done = true;
                         }
                 }
-
-                /* Skip button controls and read-only controls. */
-                if (ctrl->type == V4L2_CTRL_TYPE_BUTTON ||
-                    (ctrl->flags & V4L2_CTRL_FLAG_READ_ONLY))
-                        continue;
-                ret = master->ops->s_ctrl(master);
+                ret = call_op(master, s_ctrl);
                 if (ret)
                         break;
-                for (i = 0; i < master->ncontrols; i++)
-                        if (master->cluster[i])
-                                master->cluster[i]->done = true;
         }
         mutex_unlock(&hdl->lock);
         return ret;
@@ -1447,18 +1827,19 @@ EXPORT_SYMBOL(v4l2_subdev_querymenu);
    Find the controls in the control array and do some basic checks. */
 static int prepare_ext_ctrls(struct v4l2_ctrl_handler *hdl,
                              struct v4l2_ext_controls *cs,
-                             struct ctrl_helper *helpers,
-                             bool try)
+                             struct v4l2_ctrl_helper *helpers)
 {
+        struct v4l2_ctrl_helper *h;
+        bool have_clusters = false;
         u32 i;
 
-        for (i = 0; i < cs->count; i++) {
+        for (i = 0, h = helpers; i < cs->count; i++, h++) {
                 struct v4l2_ext_control *c = &cs->controls[i];
+                struct v4l2_ctrl_ref *ref;
                 struct v4l2_ctrl *ctrl;
                 u32 id = c->id & V4L2_CTRL_ID_MASK;
 
-                if (try)
-                        cs->error_idx = i;
+                cs->error_idx = i;
 
                 if (cs->ctrl_class && V4L2_CTRL_ID2CLASS(id) != cs->ctrl_class)
                         return -EINVAL;
@@ -1467,53 +1848,59 @@ static int prepare_ext_ctrls(struct v4l2_ctrl_handler *hdl,
                    extended controls */
                 if (id >= V4L2_CID_PRIVATE_BASE)
                         return -EINVAL;
-                ctrl = v4l2_ctrl_find(hdl, id);
-                if (ctrl == NULL)
+                ref = find_ref_lock(hdl, id);
+                if (ref == NULL)
                         return -EINVAL;
+                ctrl = ref->ctrl;
                 if (ctrl->flags & V4L2_CTRL_FLAG_DISABLED)
                         return -EINVAL;
 
-                helpers[i].ctrl = ctrl;
-                helpers[i].handled = false;
+                if (ctrl->cluster[0]->ncontrols > 1)
+                        have_clusters = true;
+                if (ctrl->cluster[0] != ctrl)
+                        ref = find_ref_lock(hdl, ctrl->cluster[0]->id);
+                /* Store the ref to the master control of the cluster */
+                h->mref = ref;
+                h->ctrl = ctrl;
+                /* Initially set next to 0, meaning that there is no other
+                   control in this helper array belonging to the same
+                   cluster */
+                h->next = 0;
         }
-        return 0;
-}
 
-typedef int (*cluster_func)(struct v4l2_ext_control *c,
-                            struct v4l2_ctrl *ctrl);
+        /* We are done if there were no controls that belong to a multi-
+           control cluster. */
+        if (!have_clusters)
+                return 0;
 
-/* Walk over all controls in v4l2_ext_controls belonging to the same cluster
-   and call the provided function. */
-static int cluster_walk(unsigned from,
-                        struct v4l2_ext_controls *cs,
-                        struct ctrl_helper *helpers,
-                        cluster_func f)
-{
-        struct v4l2_ctrl **cluster = helpers[from].ctrl->cluster;
-        int ret = 0;
-        int i;
+        /* The code below figures out in O(n) time which controls in the list
+           belong to the same cluster. */
 
-        /* Find any controls from the same cluster and call the function */
-        for (i = from; !ret && i < cs->count; i++) {
-                struct v4l2_ctrl *ctrl = helpers[i].ctrl;
+        /* This has to be done with the handler lock taken. */
+        mutex_lock(&hdl->lock);
 
-                if (!helpers[i].handled && ctrl->cluster == cluster)
-                        ret = f(&cs->controls[i], ctrl);
+        /* First zero the helper field in the master control references */
+        for (i = 0; i < cs->count; i++)
+                helpers[i].mref->helper = 0;
+        for (i = 0, h = helpers; i < cs->count; i++, h++) {
+                struct v4l2_ctrl_ref *mref = h->mref;
+
+                /* If the mref->helper is set, then it points to an earlier
+                   helper that belongs to the same cluster. */
+                if (mref->helper) {
+                        /* Set the next field of mref->helper to the current
+                           index: this means that that earlier helper now
+                           points to the next helper in the same cluster. */
+                        mref->helper->next = i;
+                        /* mref should be set only for the first helper in the
+                           cluster, clear the others. */
+                        h->mref = NULL;
+                }
+                /* Point the mref helper to the current helper struct. */
+                mref->helper = h;
         }
-        return ret;
-}
-
-static void cluster_done(unsigned from,
-                         struct v4l2_ext_controls *cs,
-                         struct ctrl_helper *helpers)
-{
-        struct v4l2_ctrl **cluster = helpers[from].ctrl->cluster;
-        int i;
-
-        /* Find any controls from the same cluster and mark them as handled */
-        for (i = from; i < cs->count; i++)
-                if (helpers[i].ctrl->cluster == cluster)
-                        helpers[i].handled = true;
+        mutex_unlock(&hdl->lock);
+        return 0;
 }
 
 /* Handles the corner case where cs->count == 0. It checks whether the
@@ -1531,10 +1918,10 @@ static int class_check(struct v4l2_ctrl_handler *hdl, u32 ctrl_class)
 /* Get extended controls. Allocates the helpers array if needed. */
 int v4l2_g_ext_ctrls(struct v4l2_ctrl_handler *hdl, struct v4l2_ext_controls *cs)
 {
-        struct ctrl_helper helper[4];
-        struct ctrl_helper *helpers = helper;
+        struct v4l2_ctrl_helper helper[4];
+        struct v4l2_ctrl_helper *helpers = helper;
         int ret;
-        int i;
+        int i, j;
 
         cs->error_idx = cs->count;
         cs->ctrl_class = V4L2_CTRL_ID2CLASS(cs->ctrl_class);
@@ -1551,30 +1938,46 @@ int v4l2_g_ext_ctrls(struct v4l2_ctrl_handler *hdl, struct v4l2_ext_controls *cs
                         return -ENOMEM;
         }
 
-        ret = prepare_ext_ctrls(hdl, cs, helpers, false);
+        ret = prepare_ext_ctrls(hdl, cs, helpers);
+        cs->error_idx = cs->count;
 
         for (i = 0; !ret && i < cs->count; i++)
                 if (helpers[i].ctrl->flags & V4L2_CTRL_FLAG_WRITE_ONLY)
                         ret = -EACCES;
 
         for (i = 0; !ret && i < cs->count; i++) {
-                struct v4l2_ctrl *ctrl = helpers[i].ctrl;
-                struct v4l2_ctrl *master = ctrl->cluster[0];
+                int (*ctrl_to_user)(struct v4l2_ext_control *c,
+                                    struct v4l2_ctrl *ctrl) = cur_to_user;
+                struct v4l2_ctrl *master;
 
-                if (helpers[i].handled)
+                if (helpers[i].mref == NULL)
                         continue;
 
+                master = helpers[i].mref->ctrl;
                 cs->error_idx = i;
 
                 v4l2_ctrl_lock(master);
-                /* g_volatile_ctrl will update the current control values */
-                if (ctrl->is_volatile && master->ops->g_volatile_ctrl)
-                        ret = master->ops->g_volatile_ctrl(master);
-                /* If OK, then copy the current control values to the caller */
-                if (!ret)
-                        ret = cluster_walk(i, cs, helpers, cur_to_user);
+
+                /* g_volatile_ctrl will update the new control values */
+                if (has_op(master, g_volatile_ctrl) && !is_cur_manual(master)) {
+                        for (j = 0; j < master->ncontrols; j++)
+                                cur_to_new(master->cluster[j]);
+                        ret = call_op(master, g_volatile_ctrl);
+                        ctrl_to_user = new_to_user;
+                }
+                /* If OK, then copy the current (for non-volatile controls)
+                   or the new (for volatile controls) control values to the
+                   caller */
+                if (!ret) {
+                        u32 idx = i;
+
+                        do {
+                                ret = ctrl_to_user(cs->controls + idx,
+                                                   helpers[idx].ctrl);
+                                idx = helpers[idx].next;
+                        } while (!ret && idx);
+                }
                 v4l2_ctrl_unlock(master);
-                cluster_done(i, cs, helpers);
         }
 
         if (cs->count > ARRAY_SIZE(helper))
@@ -1594,15 +1997,21 @@ static int get_ctrl(struct v4l2_ctrl *ctrl, s32 *val)
 {
         struct v4l2_ctrl *master = ctrl->cluster[0];
         int ret = 0;
+        int i;
 
         if (ctrl->flags & V4L2_CTRL_FLAG_WRITE_ONLY)
                 return -EACCES;
 
         v4l2_ctrl_lock(master);
         /* g_volatile_ctrl will update the current control values */
-        if (ctrl->is_volatile && master->ops->g_volatile_ctrl)
-                ret = master->ops->g_volatile_ctrl(master);
-        *val = ctrl->cur.val;
+        if (ctrl->is_volatile && !is_cur_manual(master)) {
+                for (i = 0; i < master->ncontrols; i++)
+                        cur_to_new(master->cluster[i]);
+                ret = call_op(master, g_volatile_ctrl);
+                *val = ctrl->val;
+        } else {
+                *val = ctrl->cur.val;
+        }
         v4l2_ctrl_unlock(master);
         return ret;
 }
@@ -1638,72 +2047,61 @@ EXPORT_SYMBOL(v4l2_ctrl_g_ctrl);
 /* Core function that calls try/s_ctrl and ensures that the new value is
    copied to the current value on a set.
    Must be called with ctrl->handler->lock held. */
-static int try_or_set_control_cluster(struct v4l2_ctrl *master, bool set)
+static int try_or_set_cluster(struct v4l2_fh *fh,
+                              struct v4l2_ctrl *master, bool set)
 {
-        bool try = !set;
-        int ret = 0;
+        bool update_flag;
+        int ret;
         int i;
 
         /* Go through the cluster and either validate the new value or
            (if no new value was set), copy the current value to the new
            value, ensuring a consistent view for the control ops when
            called. */
-        for (i = 0; !ret && i < master->ncontrols; i++) {
+        for (i = 0; i < master->ncontrols; i++) {
                 struct v4l2_ctrl *ctrl = master->cluster[i];
 
                 if (ctrl == NULL)
                         continue;
 
-                if (ctrl->is_new) {
-                        /* Double check this: it may have changed since the
-                           last check in try_or_set_ext_ctrls(). */
-                        if (set && (ctrl->flags & V4L2_CTRL_FLAG_GRABBED))
-                                return -EBUSY;
-
-                        /* Validate if required */
-                        if (!set)
-                                ret = validate_new(ctrl);
+                if (!ctrl->is_new) {
+                        cur_to_new(ctrl);
                         continue;
                 }
-                /* No new value was set, so copy the current and force
-                   a call to try_ctrl later, since the values for the cluster
-                   may now have changed and the end result might be invalid. */
-                try = true;
-                cur_to_new(ctrl);
+                /* Check again: it may have changed since the
+                   previous check in try_or_set_ext_ctrls(). */
+                if (set && (ctrl->flags & V4L2_CTRL_FLAG_GRABBED))
+                        return -EBUSY;
         }
 
-        /* For larger clusters you have to call try_ctrl again to
-           verify that the controls are still valid after the
-           'cur_to_new' above. */
-        if (!ret && master->ops->try_ctrl && try)
-                ret = master->ops->try_ctrl(master);
+        ret = call_op(master, try_ctrl);
 
         /* Don't set if there is no change */
-        if (!ret && set && cluster_changed(master)) {
-                ret = master->ops->s_ctrl(master);
-                /* If OK, then make the new values permanent. */
-                if (!ret)
-                        for (i = 0; i < master->ncontrols; i++)
-                                new_to_cur(master->cluster[i]);
-        }
-        return ret;
+        if (ret || !set || !cluster_changed(master))
+                return ret;
+        ret = call_op(master, s_ctrl);
+        if (ret)
+                return ret;
+
+        /* If OK, then make the new values permanent. */
+        update_flag = is_cur_manual(master) != is_new_manual(master);
+        for (i = 0; i < master->ncontrols; i++)
+                new_to_cur(fh, master->cluster[i], update_flag && i > 0);
+        return 0;
 }
 
-/* Try or set controls. */
-static int try_or_set_ext_ctrls(struct v4l2_ctrl_handler *hdl,
-                                struct v4l2_ext_controls *cs,
-                                struct ctrl_helper *helpers,
-                                bool set)
+/* Validate controls. */
+static int validate_ctrls(struct v4l2_ext_controls *cs,
+                          struct v4l2_ctrl_helper *helpers, bool set)
 {
-        unsigned i, j;
+        unsigned i;
         int ret = 0;
 
         cs->error_idx = cs->count;
         for (i = 0; i < cs->count; i++) {
                 struct v4l2_ctrl *ctrl = helpers[i].ctrl;
 
-                if (!set)
-                        cs->error_idx = i;
+                cs->error_idx = i;
 
                 if (ctrl->flags & V4L2_CTRL_FLAG_READ_ONLY)
                         return -EACCES;
@@ -1715,50 +2113,22 @@ static int try_or_set_ext_ctrls(struct v4l2_ctrl_handler *hdl,
                    best-effort to avoid that. */
                 if (set && (ctrl->flags & V4L2_CTRL_FLAG_GRABBED))
                         return -EBUSY;
+                ret = validate_new(ctrl, &cs->controls[i]);
+                if (ret)
+                        return ret;
         }
-
-        for (i = 0; !ret && i < cs->count; i++) {
-                struct v4l2_ctrl *ctrl = helpers[i].ctrl;
-                struct v4l2_ctrl *master = ctrl->cluster[0];
-
-                cs->error_idx = i;
-
-                if (helpers[i].handled)
-                        continue;
-
-                v4l2_ctrl_lock(ctrl);
-
-                /* Reset the 'is_new' flags of the cluster */
-                for (j = 0; j < master->ncontrols; j++)
-                        if (master->cluster[j])
-                                master->cluster[j]->is_new = 0;
-
-                /* Copy the new caller-supplied control values.
-                   user_to_new() sets 'is_new' to 1. */
-                ret = cluster_walk(i, cs, helpers, user_to_new);
-
-                if (!ret)
-                        ret = try_or_set_control_cluster(master, set);
-
-                /* Copy the new values back to userspace. */
-                if (!ret)
-                        ret = cluster_walk(i, cs, helpers, new_to_user);
-
-                v4l2_ctrl_unlock(ctrl);
-                cluster_done(i, cs, helpers);
-        }
-        return ret;
+        return 0;
 }
 
 /* Try or try-and-set controls */
-static int try_set_ext_ctrls(struct v4l2_ctrl_handler *hdl,
+static int try_set_ext_ctrls(struct v4l2_fh *fh, struct v4l2_ctrl_handler *hdl,
                              struct v4l2_ext_controls *cs,
                              bool set)
 {
-        struct ctrl_helper helper[4];
-        struct ctrl_helper *helpers = helper;
+        struct v4l2_ctrl_helper helper[4];
+        struct v4l2_ctrl_helper *helpers = helper;
+        unsigned i, j;
         int ret;
-        int i;
 
         cs->error_idx = cs->count;
         cs->ctrl_class = V4L2_CTRL_ID2CLASS(cs->ctrl_class);
@@ -1774,25 +2144,49 @@ static int try_set_ext_ctrls(struct v4l2_ctrl_handler *hdl,
                 if (!helpers)
                         return -ENOMEM;
         }
-        ret = prepare_ext_ctrls(hdl, cs, helpers, !set);
-        if (ret)
-                goto free;
-
-        /* First 'try' all controls and abort on error */
-        ret = try_or_set_ext_ctrls(hdl, cs, helpers, false);
-        /* If this is a 'set' operation and the initial 'try' failed,
-           then set error_idx to count to tell the application that no
-           controls changed value yet. */
-        if (set)
+        ret = prepare_ext_ctrls(hdl, cs, helpers);
+        if (!ret)
+                ret = validate_ctrls(cs, helpers, set);
+        if (ret && set)
                 cs->error_idx = cs->count;
-        if (!ret && set) {
-                /* Reset 'handled' state */
-                for (i = 0; i < cs->count; i++)
-                        helpers[i].handled = false;
-                ret = try_or_set_ext_ctrls(hdl, cs, helpers, true);
+        for (i = 0; !ret && i < cs->count; i++) {
+                struct v4l2_ctrl *master;
+                u32 idx = i;
+
+                if (helpers[i].mref == NULL)
+                        continue;
+
+                cs->error_idx = i;
+                master = helpers[i].mref->ctrl;
+                v4l2_ctrl_lock(master);
+
+                /* Reset the 'is_new' flags of the cluster */
+                for (j = 0; j < master->ncontrols; j++)
+                        if (master->cluster[j])
+                                master->cluster[j]->is_new = 0;
+
+                /* Copy the new caller-supplied control values.
+                   user_to_new() sets 'is_new' to 1. */
+                do {
+                        ret = user_to_new(cs->controls + idx, helpers[idx].ctrl);
+                        idx = helpers[idx].next;
+                } while (!ret && idx);
+
+                if (!ret)
+                        ret = try_or_set_cluster(fh, master, set);
+
+                /* Copy the new values back to userspace. */
+                if (!ret) {
+                        idx = i;
+                        do {
+                                ret = new_to_user(cs->controls + idx,
+                                                helpers[idx].ctrl);
+                                idx = helpers[idx].next;
+                        } while (!ret && idx);
+                }
+                v4l2_ctrl_unlock(master);
         }
 
-free:
         if (cs->count > ARRAY_SIZE(helper))
                 kfree(helpers);
         return ret;
@@ -1800,37 +2194,39 @@ free:
 
 int v4l2_try_ext_ctrls(struct v4l2_ctrl_handler *hdl, struct v4l2_ext_controls *cs)
 {
-        return try_set_ext_ctrls(hdl, cs, false);
+        return try_set_ext_ctrls(NULL, hdl, cs, false);
 }
 EXPORT_SYMBOL(v4l2_try_ext_ctrls);
 
-int v4l2_s_ext_ctrls(struct v4l2_ctrl_handler *hdl, struct v4l2_ext_controls *cs)
+int v4l2_s_ext_ctrls(struct v4l2_fh *fh, struct v4l2_ctrl_handler *hdl,
+                                        struct v4l2_ext_controls *cs)
 {
-        return try_set_ext_ctrls(hdl, cs, true);
+        return try_set_ext_ctrls(fh, hdl, cs, true);
 }
 EXPORT_SYMBOL(v4l2_s_ext_ctrls);
 
 int v4l2_subdev_try_ext_ctrls(struct v4l2_subdev *sd, struct v4l2_ext_controls *cs)
 {
-        return try_set_ext_ctrls(sd->ctrl_handler, cs, false);
+        return try_set_ext_ctrls(NULL, sd->ctrl_handler, cs, false);
 }
 EXPORT_SYMBOL(v4l2_subdev_try_ext_ctrls);
 
 int v4l2_subdev_s_ext_ctrls(struct v4l2_subdev *sd, struct v4l2_ext_controls *cs)
 {
-        return try_set_ext_ctrls(sd->ctrl_handler, cs, true);
+        return try_set_ext_ctrls(NULL, sd->ctrl_handler, cs, true);
 }
 EXPORT_SYMBOL(v4l2_subdev_s_ext_ctrls);
 
 /* Helper function for VIDIOC_S_CTRL compatibility */
-static int set_ctrl(struct v4l2_ctrl *ctrl, s32 *val)
+static int set_ctrl(struct v4l2_fh *fh, struct v4l2_ctrl *ctrl, s32 *val)
 {
         struct v4l2_ctrl *master = ctrl->cluster[0];
         int ret;
         int i;
 
-        if (ctrl->flags & V4L2_CTRL_FLAG_READ_ONLY)
-                return -EACCES;
+        ret = validate_new_int(ctrl, val);
+        if (ret)
+                return ret;
 
         v4l2_ctrl_lock(ctrl);
 
@@ -1841,28 +2237,30 @@ static int set_ctrl(struct v4l2_ctrl *ctrl, s32 *val)
 
         ctrl->val = *val;
         ctrl->is_new = 1;
-        ret = try_or_set_control_cluster(master, false);
-        if (!ret)
-                ret = try_or_set_control_cluster(master, true);
+        ret = try_or_set_cluster(fh, master, true);
         *val = ctrl->cur.val;
         v4l2_ctrl_unlock(ctrl);
         return ret;
 }
 
-int v4l2_s_ctrl(struct v4l2_ctrl_handler *hdl, struct v4l2_control *control)
+int v4l2_s_ctrl(struct v4l2_fh *fh, struct v4l2_ctrl_handler *hdl,
+                                        struct v4l2_control *control)
 {
         struct v4l2_ctrl *ctrl = v4l2_ctrl_find(hdl, control->id);
 
         if (ctrl == NULL || !type_is_int(ctrl))
                 return -EINVAL;
 
-        return set_ctrl(ctrl, &control->value);
+        if (ctrl->flags & V4L2_CTRL_FLAG_READ_ONLY)
+                return -EACCES;
+
+        return set_ctrl(fh, ctrl, &control->value);
 }
 EXPORT_SYMBOL(v4l2_s_ctrl);
 
 int v4l2_subdev_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *control)
 {
-        return v4l2_s_ctrl(sd->ctrl_handler, control);
+        return v4l2_s_ctrl(NULL, sd->ctrl_handler, control);
 }
 EXPORT_SYMBOL(v4l2_subdev_s_ctrl);
 
@@ -1870,6 +2268,34 @@ int v4l2_ctrl_s_ctrl(struct v4l2_ctrl *ctrl, s32 val)
 {
         /* It's a driver bug if this happens. */
         WARN_ON(!type_is_int(ctrl));
-        return set_ctrl(ctrl, &val);
+        return set_ctrl(NULL, ctrl, &val);
 }
 EXPORT_SYMBOL(v4l2_ctrl_s_ctrl);
+
+void v4l2_ctrl_add_event(struct v4l2_ctrl *ctrl,
+                                struct v4l2_subscribed_event *sev)
+{
+        v4l2_ctrl_lock(ctrl);
+        list_add_tail(&sev->node, &ctrl->ev_subs);
+        if (ctrl->type != V4L2_CTRL_TYPE_CTRL_CLASS &&
+            (sev->flags & V4L2_EVENT_SUB_FL_SEND_INITIAL)) {
+                struct v4l2_event ev;
+                u32 changes = V4L2_EVENT_CTRL_CH_FLAGS;
+
+                if (!(ctrl->flags & V4L2_CTRL_FLAG_WRITE_ONLY))
+                        changes |= V4L2_EVENT_CTRL_CH_VALUE;
+                fill_event(&ev, ctrl, changes);
+                v4l2_event_queue_fh(sev->fh, &ev);
+        }
+        v4l2_ctrl_unlock(ctrl);
+}
+EXPORT_SYMBOL(v4l2_ctrl_add_event);
+
+void v4l2_ctrl_del_event(struct v4l2_ctrl *ctrl,
+                                struct v4l2_subscribed_event *sev)
+{
+        v4l2_ctrl_lock(ctrl);
+        list_del(&sev->node);
+        v4l2_ctrl_unlock(ctrl);
+}
+EXPORT_SYMBOL(v4l2_ctrl_del_event);
diff --git a/drivers/media/video/v4l2-device.c b/drivers/media/video/v4l2-device.c
index 4aae501f02d..c72856c4143 100644
--- a/drivers/media/video/v4l2-device.c
+++ b/drivers/media/video/v4l2-device.c
@@ -209,6 +209,7 @@ int v4l2_device_register_subdev_nodes(struct v4l2_device *v4l2_dev)
                 vdev->v4l2_dev = v4l2_dev;
                 vdev->fops = &v4l2_subdev_fops;
                 vdev->release = video_device_release_empty;
+                vdev->ctrl_handler = sd->ctrl_handler;
                 err = __video_register_device(vdev, VFL_TYPE_SUBDEV, -1, 1,
                                               sd->owner);
                 if (err < 0)
diff --git a/drivers/media/video/v4l2-event.c b/drivers/media/video/v4l2-event.c
index 69fd343d477..53b190cf225 100644
--- a/drivers/media/video/v4l2-event.c
+++ b/drivers/media/video/v4l2-event.c
@@ -25,100 +25,39 @@
 #include <media/v4l2-dev.h>
 #include <media/v4l2-fh.h>
 #include <media/v4l2-event.h>
+#include <media/v4l2-ctrls.h>
 
 #include <linux/sched.h>
 #include <linux/slab.h>
 
-int v4l2_event_init(struct v4l2_fh *fh)
+static unsigned sev_pos(const struct v4l2_subscribed_event *sev, unsigned idx)
 {
-        fh->events = kzalloc(sizeof(*fh->events), GFP_KERNEL);
-        if (fh->events == NULL)
-                return -ENOMEM;
-
-        init_waitqueue_head(&fh->events->wait);
-
-        INIT_LIST_HEAD(&fh->events->free);
-        INIT_LIST_HEAD(&fh->events->available);
-        INIT_LIST_HEAD(&fh->events->subscribed);
-
-        fh->events->sequence = -1;
-
-        return 0;
-}
-EXPORT_SYMBOL_GPL(v4l2_event_init);
-
-int v4l2_event_alloc(struct v4l2_fh *fh, unsigned int n)
-{
-        struct v4l2_events *events = fh->events;
-        unsigned long flags;
-
-        if (!events) {
-                WARN_ON(1);
-                return -ENOMEM;
-        }
-
-        while (events->nallocated < n) {
-                struct v4l2_kevent *kev;
-
-                kev = kzalloc(sizeof(*kev), GFP_KERNEL);
-                if (kev == NULL)
-                        return -ENOMEM;
-
-                spin_lock_irqsave(&fh->vdev->fh_lock, flags);
-                list_add_tail(&kev->list, &events->free);
-                events->nallocated++;
-                spin_unlock_irqrestore(&fh->vdev->fh_lock, flags);
-        }
-
-        return 0;
-}
-EXPORT_SYMBOL_GPL(v4l2_event_alloc);
-
-#define list_kfree(list, type, member)                          \
-        while (!list_empty(list)) {                             \
-                type *hi;                                       \
-                hi = list_first_entry(list, type, member);      \
-                list_del(&hi->member);                          \
-                kfree(hi);                                      \
-        }
-
-void v4l2_event_free(struct v4l2_fh *fh)
-{
-        struct v4l2_events *events = fh->events;
-
-        if (!events)
-                return;
-
-        list_kfree(&events->free, struct v4l2_kevent, list);
-        list_kfree(&events->available, struct v4l2_kevent, list);
-        list_kfree(&events->subscribed, struct v4l2_subscribed_event, list);
-
-        kfree(events);
-        fh->events = NULL;
+        idx += sev->first;
+        return idx >= sev->elems ? idx - sev->elems : idx;
 }
-EXPORT_SYMBOL_GPL(v4l2_event_free);
 
 static int __v4l2_event_dequeue(struct v4l2_fh *fh, struct v4l2_event *event)
 {
-        struct v4l2_events *events = fh->events;
         struct v4l2_kevent *kev;
         unsigned long flags;
 
         spin_lock_irqsave(&fh->vdev->fh_lock, flags);
 
-        if (list_empty(&events->available)) {
+        if (list_empty(&fh->available)) {
                 spin_unlock_irqrestore(&fh->vdev->fh_lock, flags);
                 return -ENOENT;
         }
 
-        WARN_ON(events->navailable == 0);
+        WARN_ON(fh->navailable == 0);
 
-        kev = list_first_entry(&events->available, struct v4l2_kevent, list);
-        list_move(&kev->list, &events->free);
-        events->navailable--;
+        kev = list_first_entry(&fh->available, struct v4l2_kevent, list);
+        list_del(&kev->list);
+        fh->navailable--;
 
-        kev->event.pending = events->navailable;
+        kev->event.pending = fh->navailable;
         *event = kev->event;
+        kev->sev->first = sev_pos(kev->sev, 1);
+        kev->sev->in_use--;
 
         spin_unlock_irqrestore(&fh->vdev->fh_lock, flags);
 
@@ -128,7 +67,6 @@ static int __v4l2_event_dequeue(struct v4l2_fh *fh, struct v4l2_event *event)
 int v4l2_event_dequeue(struct v4l2_fh *fh, struct v4l2_event *event,
                        int nonblocking)
 {
-        struct v4l2_events *events = fh->events;
         int ret;
 
         if (nonblocking)
@@ -139,8 +77,8 @@ int v4l2_event_dequeue(struct v4l2_fh *fh, struct v4l2_event *event,
                 mutex_unlock(fh->vdev->lock);
 
         do {
-                ret = wait_event_interruptible(events->wait,
-                                               events->navailable != 0);
+                ret = wait_event_interruptible(fh->wait,
+                                               fh->navailable != 0);
                 if (ret < 0)
                         break;
 
@@ -154,23 +92,72 @@ int v4l2_event_dequeue(struct v4l2_fh *fh, struct v4l2_event *event,
 }
 EXPORT_SYMBOL_GPL(v4l2_event_dequeue);
 
-/* Caller must hold fh->event->lock! */
+/* Caller must hold fh->vdev->fh_lock! */
 static struct v4l2_subscribed_event *v4l2_event_subscribed(
-        struct v4l2_fh *fh, u32 type)
+                struct v4l2_fh *fh, u32 type, u32 id)
 {
-        struct v4l2_events *events = fh->events;
         struct v4l2_subscribed_event *sev;
 
         assert_spin_locked(&fh->vdev->fh_lock);
 
-        list_for_each_entry(sev, &events->subscribed, list) {
-                if (sev->type == type)
+        list_for_each_entry(sev, &fh->subscribed, list)
+                if (sev->type == type && sev->id == id)
                         return sev;
-        }
 
         return NULL;
 }
 
+static void __v4l2_event_queue_fh(struct v4l2_fh *fh, const struct v4l2_event *ev,
+                const struct timespec *ts)
+{
+        struct v4l2_subscribed_event *sev;
+        struct v4l2_kevent *kev;
+        bool copy_payload = true;
+
+        /* Are we subscribed? */
+        sev = v4l2_event_subscribed(fh, ev->type, ev->id);
+        if (sev == NULL)
+                return;
+
+        /* Increase event sequence number on fh. */
+        fh->sequence++;
+
+        /* Do we have any free events? */
+        if (sev->in_use == sev->elems) {
+                /* no, remove the oldest one */
+                kev = sev->events + sev_pos(sev, 0);
+                list_del(&kev->list);
+                sev->in_use--;
+                sev->first = sev_pos(sev, 1);
+                fh->navailable--;
+                if (sev->elems == 1) {
+                        if (sev->replace) {
+                                sev->replace(&kev->event, ev);
+                                copy_payload = false;
+                        }
+                } else if (sev->merge) {
+                        struct v4l2_kevent *second_oldest =
+                                sev->events + sev_pos(sev, 0);
+                        sev->merge(&kev->event, &second_oldest->event);
+                }
+        }
+
+        /* Take one and fill it. */
+        kev = sev->events + sev_pos(sev, sev->in_use);
+        kev->event.type = ev->type;
+        if (copy_payload)
+                kev->event.u = ev->u;
+        kev->event.id = ev->id;
+        kev->event.timestamp = *ts;
+        kev->event.sequence = fh->sequence;
+        sev->in_use++;
+        list_add_tail(&kev->list, &fh->available);
+
+        fh->navailable++;
+
+        wake_up_all(&fh->wait);
+}
+
 void v4l2_event_queue(struct video_device *vdev, const struct v4l2_event *ev)
 {
         struct v4l2_fh *fh;
@@ -181,81 +168,95 @@ void v4l2_event_queue(struct video_device *vdev, const struct v4l2_event *ev)
 
         spin_lock_irqsave(&vdev->fh_lock, flags);
 
-        list_for_each_entry(fh, &vdev->fh_list, list) {
-                struct v4l2_events *events = fh->events;
-                struct v4l2_kevent *kev;
+        list_for_each_entry(fh, &vdev->fh_list, list)
+                __v4l2_event_queue_fh(fh, ev, &timestamp);
 
-                /* Are we subscribed? */
-                if (!v4l2_event_subscribed(fh, ev->type))
-                        continue;
+        spin_unlock_irqrestore(&vdev->fh_lock, flags);
+}
+EXPORT_SYMBOL_GPL(v4l2_event_queue);
 
-                /* Increase event sequence number on fh. */
-                events->sequence++;
+void v4l2_event_queue_fh(struct v4l2_fh *fh, const struct v4l2_event *ev)
+{
+        unsigned long flags;
+        struct timespec timestamp;
 
-                /* Do we have any free events? */
-                if (list_empty(&events->free))
-                        continue;
+        ktime_get_ts(&timestamp);
 
-                /* Take one and fill it. */
-                kev = list_first_entry(&events->free, struct v4l2_kevent, list);
-                kev->event.type = ev->type;
-                kev->event.u = ev->u;
-                kev->event.timestamp = timestamp;
-                kev->event.sequence = events->sequence;
-                list_move_tail(&kev->list, &events->available);
+        spin_lock_irqsave(&fh->vdev->fh_lock, flags);
+        __v4l2_event_queue_fh(fh, ev, &timestamp);
+        spin_unlock_irqrestore(&fh->vdev->fh_lock, flags);
+}
+EXPORT_SYMBOL_GPL(v4l2_event_queue_fh);
 
-                events->navailable++;
+int v4l2_event_pending(struct v4l2_fh *fh)
+{
+        return fh->navailable;
+}
+EXPORT_SYMBOL_GPL(v4l2_event_pending);
 
-                wake_up_all(&events->wait);
-        }
+static void ctrls_replace(struct v4l2_event *old, const struct v4l2_event *new)
+{
+        u32 old_changes = old->u.ctrl.changes;
 
-        spin_unlock_irqrestore(&vdev->fh_lock, flags);
+        old->u.ctrl = new->u.ctrl;
+        old->u.ctrl.changes |= old_changes;
 }
-EXPORT_SYMBOL_GPL(v4l2_event_queue);
 
-int v4l2_event_pending(struct v4l2_fh *fh)
+static void ctrls_merge(const struct v4l2_event *old, struct v4l2_event *new)
 {
-        return fh->events->navailable;
+        new->u.ctrl.changes |= old->u.ctrl.changes;
 }
-EXPORT_SYMBOL_GPL(v4l2_event_pending);
 
 int v4l2_event_subscribe(struct v4l2_fh *fh,
-                         struct v4l2_event_subscription *sub)
+                         struct v4l2_event_subscription *sub, unsigned elems)
 {
-        struct v4l2_events *events = fh->events;
-        struct v4l2_subscribed_event *sev;
+        struct v4l2_subscribed_event *sev, *found_ev;
+        struct v4l2_ctrl *ctrl = NULL;
         unsigned long flags;
-
-        if (fh->events == NULL) {
-                WARN_ON(1);
-                return -ENOMEM;
+        unsigned i;
+
+        if (elems < 1)
+                elems = 1;
+        if (sub->type == V4L2_EVENT_CTRL) {
+                ctrl = v4l2_ctrl_find(fh->ctrl_handler, sub->id);
+                if (ctrl == NULL)
+                        return -EINVAL;
         }
 
-        sev = kmalloc(sizeof(*sev), GFP_KERNEL);
+        sev = kzalloc(sizeof(*sev) + sizeof(struct v4l2_kevent) * elems, GFP_KERNEL);
         if (!sev)
                 return -ENOMEM;
-
-        spin_lock_irqsave(&fh->vdev->fh_lock, flags);
-
-        if (v4l2_event_subscribed(fh, sub->type) == NULL) {
-                INIT_LIST_HEAD(&sev->list);
-                sev->type = sub->type;
-
-                list_add(&sev->list, &events->subscribed);
-                sev = NULL;
+        for (i = 0; i < elems; i++)
+                sev->events[i].sev = sev;
+        sev->type = sub->type;
+        sev->id = sub->id;
+        sev->flags = sub->flags;
+        sev->fh = fh;
+        sev->elems = elems;
+        if (ctrl) {
+                sev->replace = ctrls_replace;
+                sev->merge = ctrls_merge;
         }
 
+        spin_lock_irqsave(&fh->vdev->fh_lock, flags);
+        found_ev = v4l2_event_subscribed(fh, sub->type, sub->id);
+        if (!found_ev)
+                list_add(&sev->list, &fh->subscribed);
         spin_unlock_irqrestore(&fh->vdev->fh_lock, flags);
 
-        kfree(sev);
+        /* v4l2_ctrl_add_event uses a mutex, so do this outside the spin lock */
+        if (found_ev)
+                kfree(sev);
+        else if (ctrl)
+                v4l2_ctrl_add_event(ctrl, sev);
 
         return 0;
 }
 EXPORT_SYMBOL_GPL(v4l2_event_subscribe);
 
-static void v4l2_event_unsubscribe_all(struct v4l2_fh *fh)
+void v4l2_event_unsubscribe_all(struct v4l2_fh *fh)
 {
-        struct v4l2_events *events = fh->events;
+        struct v4l2_event_subscription sub;
         struct v4l2_subscribed_event *sev;
         unsigned long flags;
 
@@ -263,15 +264,18 @@ static void v4l2_event_unsubscribe_all(struct v4l2_fh *fh)
                 sev = NULL;
 
                 spin_lock_irqsave(&fh->vdev->fh_lock, flags);
-                if (!list_empty(&events->subscribed)) {
-                        sev = list_first_entry(&events->subscribed,
-                                       struct v4l2_subscribed_event, list);
-                        list_del(&sev->list);
+                if (!list_empty(&fh->subscribed)) {
+                        sev = list_first_entry(&fh->subscribed,
+                                        struct v4l2_subscribed_event, list);
+                        sub.type = sev->type;
+                        sub.id = sev->id;
                 }
                 spin_unlock_irqrestore(&fh->vdev->fh_lock, flags);
-                kfree(sev);
+                if (sev)
+                        v4l2_event_unsubscribe(fh, &sub);
         } while (sev);
 }
+EXPORT_SYMBOL_GPL(v4l2_event_unsubscribe_all);
 
 int v4l2_event_unsubscribe(struct v4l2_fh *fh,
                            struct v4l2_event_subscription *sub)
@@ -286,11 +290,19 @@ int v4l2_event_unsubscribe(struct v4l2_fh *fh,
 
         spin_lock_irqsave(&fh->vdev->fh_lock, flags);
 
-        sev = v4l2_event_subscribed(fh, sub->type);
-        if (sev != NULL)
+        sev = v4l2_event_subscribed(fh, sub->type, sub->id);
+        if (sev != NULL) {
                 list_del(&sev->list);
+                sev->fh = NULL;
+        }
 
         spin_unlock_irqrestore(&fh->vdev->fh_lock, flags);
+        if (sev && sev->type == V4L2_EVENT_CTRL) {
+                struct v4l2_ctrl *ctrl = v4l2_ctrl_find(fh->ctrl_handler, sev->id);
+
+                if (ctrl)
+                        v4l2_ctrl_del_event(ctrl, sev);
+        }
 
         kfree(sev);
 
diff --git a/drivers/media/video/v4l2-fh.c b/drivers/media/video/v4l2-fh.c
index 717f71e6370..122822d2b8b 100644
--- a/drivers/media/video/v4l2-fh.c
+++ b/drivers/media/video/v4l2-fh.c
@@ -29,23 +29,18 @@
 #include <media/v4l2-event.h>
 #include <media/v4l2-ioctl.h>
 
-int v4l2_fh_init(struct v4l2_fh *fh, struct video_device *vdev)
+void v4l2_fh_init(struct v4l2_fh *fh, struct video_device *vdev)
 {
         fh->vdev = vdev;
+        /* Inherit from video_device. May be overridden by the driver. */
+        fh->ctrl_handler = vdev->ctrl_handler;
         INIT_LIST_HEAD(&fh->list);
         set_bit(V4L2_FL_USES_V4L2_FH, &fh->vdev->flags);
         fh->prio = V4L2_PRIORITY_UNSET;
-
-        /*
-         * fh->events only needs to be initialized if the driver
-         * supports the VIDIOC_SUBSCRIBE_EVENT ioctl.
-         */
-        if (vdev->ioctl_ops && vdev->ioctl_ops->vidioc_subscribe_event)
-                return v4l2_event_init(fh);
-
-        fh->events = NULL;
-
-        return 0;
+        init_waitqueue_head(&fh->wait);
+        INIT_LIST_HEAD(&fh->available);
+        INIT_LIST_HEAD(&fh->subscribed);
+        fh->sequence = -1;
 }
 EXPORT_SYMBOL_GPL(v4l2_fh_init);
 
@@ -91,10 +86,8 @@ void v4l2_fh_exit(struct v4l2_fh *fh)
 {
         if (fh->vdev == NULL)
                 return;
-
+        v4l2_event_unsubscribe_all(fh);
         fh->vdev = NULL;
-
-        v4l2_event_free(fh);
 }
 EXPORT_SYMBOL_GPL(v4l2_fh_exit);
 
diff --git a/drivers/media/video/v4l2-ioctl.c b/drivers/media/video/v4l2-ioctl.c
index 69e8c6ffcc4..002ce136344 100644
--- a/drivers/media/video/v4l2-ioctl.c
+++ b/drivers/media/video/v4l2-ioctl.c
@@ -16,6 +16,7 @@
 #include <linux/slab.h>
 #include <linux/types.h>
 #include <linux/kernel.h>
+#include <linux/version.h>
 
 #include <linux/videodev2.h>
 
@@ -542,12 +543,12 @@ static long __video_do_ioctl(struct file *file,
         struct v4l2_fh *vfh = NULL;
         struct v4l2_format f_copy;
         int use_fh_prio = 0;
-        long ret = -EINVAL;
+        long ret = -ENOTTY;
 
         if (ops == NULL) {
                 printk(KERN_WARNING "videodev: \"%s\" has no ioctl_ops.\n",
                                 vfd->name);
-                return -EINVAL;
+                return ret;
         }
 
         if ((vfd->debug & V4L2_DEBUG_IOCTL) &&
@@ -605,6 +606,7 @@ static long __video_do_ioctl(struct file *file,
                 if (!ops->vidioc_querycap)
                         break;
 
+                cap->version = LINUX_VERSION_CODE;
                 ret = ops->vidioc_querycap(file, fh, cap);
                 if (!ret)
                         dbgarg(cmd, "driver=%s, card=%s, bus=%s, "
@@ -1418,7 +1420,9 @@ static long __video_do_ioctl(struct file *file,
         {
                 struct v4l2_queryctrl *p = arg;
 
-                if (vfd->ctrl_handler)
+                if (vfh && vfh->ctrl_handler)
+                        ret = v4l2_queryctrl(vfh->ctrl_handler, p);
+                else if (vfd->ctrl_handler)
                         ret = v4l2_queryctrl(vfd->ctrl_handler, p);
                 else if (ops->vidioc_queryctrl)
                         ret = ops->vidioc_queryctrl(file, fh, p);
@@ -1438,7 +1442,9 @@ static long __video_do_ioctl(struct file *file,
         {
                 struct v4l2_control *p = arg;
 
-                if (vfd->ctrl_handler)
+                if (vfh && vfh->ctrl_handler)
+                        ret = v4l2_g_ctrl(vfh->ctrl_handler, p);
+                else if (vfd->ctrl_handler)
                         ret = v4l2_g_ctrl(vfd->ctrl_handler, p);
                 else if (ops->vidioc_g_ctrl)
                         ret = ops->vidioc_g_ctrl(file, fh, p);
@@ -1470,14 +1476,18 @@ static long __video_do_ioctl(struct file *file,
                 struct v4l2_ext_controls ctrls;
                 struct v4l2_ext_control ctrl;
 
-                if (!vfd->ctrl_handler &&
+                if (!(vfh && vfh->ctrl_handler) && !vfd->ctrl_handler &&
                         !ops->vidioc_s_ctrl && !ops->vidioc_s_ext_ctrls)
                         break;
 
                 dbgarg(cmd, "id=0x%x, value=%d\n", p->id, p->value);
 
+                if (vfh && vfh->ctrl_handler) {
+                        ret = v4l2_s_ctrl(vfh, vfh->ctrl_handler, p);
+                        break;
+                }
                 if (vfd->ctrl_handler) {
-                        ret = v4l2_s_ctrl(vfd->ctrl_handler, p);
+                        ret = v4l2_s_ctrl(NULL, vfd->ctrl_handler, p);
                         break;
                 }
                 if (ops->vidioc_s_ctrl) {
@@ -1501,7 +1511,9 @@ static long __video_do_ioctl(struct file *file,
                 struct v4l2_ext_controls *p = arg;
 
                 p->error_idx = p->count;
-                if (vfd->ctrl_handler)
+                if (vfh && vfh->ctrl_handler)
+                        ret = v4l2_g_ext_ctrls(vfh->ctrl_handler, p);
+                else if (vfd->ctrl_handler)
                         ret = v4l2_g_ext_ctrls(vfd->ctrl_handler, p);
                 else if (ops->vidioc_g_ext_ctrls && check_ext_ctrls(p, 0))
                         ret = ops->vidioc_g_ext_ctrls(file, fh, p);
@@ -1515,11 +1527,14 @@ static long __video_do_ioctl(struct file *file,
                 struct v4l2_ext_controls *p = arg;
 
                 p->error_idx = p->count;
-                if (!vfd->ctrl_handler && !ops->vidioc_s_ext_ctrls)
+                if (!(vfh && vfh->ctrl_handler) && !vfd->ctrl_handler &&
+                                !ops->vidioc_s_ext_ctrls)
                         break;
                 v4l_print_ext_ctrls(cmd, vfd, p, 1);
-                if (vfd->ctrl_handler)
-                        ret = v4l2_s_ext_ctrls(vfd->ctrl_handler, p);
+                if (vfh && vfh->ctrl_handler)
+                        ret = v4l2_s_ext_ctrls(vfh, vfh->ctrl_handler, p);
+                else if (vfd->ctrl_handler)
+                        ret = v4l2_s_ext_ctrls(NULL, vfd->ctrl_handler, p);
                 else if (check_ext_ctrls(p, 0))
                         ret = ops->vidioc_s_ext_ctrls(file, fh, p);
                 break;
@@ -1529,10 +1544,13 @@ static long __video_do_ioctl(struct file *file,
                 struct v4l2_ext_controls *p = arg;
 
                 p->error_idx = p->count;
-                if (!vfd->ctrl_handler && !ops->vidioc_try_ext_ctrls)
+                if (!(vfh && vfh->ctrl_handler) && !vfd->ctrl_handler &&
+                                !ops->vidioc_try_ext_ctrls)
                         break;
                 v4l_print_ext_ctrls(cmd, vfd, p, 1);
-                if (vfd->ctrl_handler)
+                if (vfh && vfh->ctrl_handler)
+                        ret = v4l2_try_ext_ctrls(vfh->ctrl_handler, p);
+                else if (vfd->ctrl_handler)
                         ret = v4l2_try_ext_ctrls(vfd->ctrl_handler, p);
                 else if (check_ext_ctrls(p, 0))
                         ret = ops->vidioc_try_ext_ctrls(file, fh, p);
@@ -1542,7 +1560,9 @@ static long __video_do_ioctl(struct file *file,
         {
                 struct v4l2_querymenu *p = arg;
 
-                if (vfd->ctrl_handler)
+                if (vfh && vfh->ctrl_handler)
+                        ret = v4l2_querymenu(vfh->ctrl_handler, p);
+                else if (vfd->ctrl_handler)
                         ret = v4l2_querymenu(vfd->ctrl_handler, p);
                 else if (ops->vidioc_querymenu)
                         ret = ops->vidioc_querymenu(file, fh, p);
@@ -2276,7 +2296,7 @@ static int check_array_args(unsigned int cmd, void *parg, size_t *array_size,
                                 break;
                         }
                         *user_ptr = (void __user *)buf->m.planes;
-                        *kernel_ptr = (void **)&buf->m.planes;
+                        *kernel_ptr = (void *)&buf->m.planes;
                         *array_size = sizeof(struct v4l2_plane) * buf->length;
                         ret = 1;
                 }
@@ -2290,7 +2310,7 @@ static int check_array_args(unsigned int cmd, void *parg, size_t *array_size,
 
                 if (ctrls->count != 0) {
                         *user_ptr = (void __user *)ctrls->controls;
-                        *kernel_ptr = (void **)&ctrls->controls;
+                        *kernel_ptr = (void *)&ctrls->controls;
                         *array_size = sizeof(struct v4l2_ext_control)
                                     * ctrls->count;
                         ret = 1;
diff --git a/drivers/media/video/v4l2-subdev.c b/drivers/media/video/v4l2-subdev.c
index 812729ebf09..b7967c9dc4a 100644
--- a/drivers/media/video/v4l2-subdev.c
+++ b/drivers/media/video/v4l2-subdev.c
@@ -75,20 +75,7 @@ static int subdev_open(struct file *file)
                 return ret;
         }
 
-        ret = v4l2_fh_init(&subdev_fh->vfh, vdev);
-        if (ret)
-                goto err;
-
-        if (sd->flags & V4L2_SUBDEV_FL_HAS_EVENTS) {
-                ret = v4l2_event_init(&subdev_fh->vfh);
-                if (ret)
-                        goto err;
-
-                ret = v4l2_event_alloc(&subdev_fh->vfh, sd->nevents);
-                if (ret)
-                        goto err;
-        }
-
+        v4l2_fh_init(&subdev_fh->vfh, vdev);
         v4l2_fh_add(&subdev_fh->vfh);
         file->private_data = &subdev_fh->vfh;
 #if defined(CONFIG_MEDIA_CONTROLLER)
@@ -155,25 +142,25 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 
         switch (cmd) {
         case VIDIOC_QUERYCTRL:
-                return v4l2_queryctrl(sd->ctrl_handler, arg);
+                return v4l2_queryctrl(vfh->ctrl_handler, arg);
 
         case VIDIOC_QUERYMENU:
-                return v4l2_querymenu(sd->ctrl_handler, arg);
+                return v4l2_querymenu(vfh->ctrl_handler, arg);
 
         case VIDIOC_G_CTRL:
-                return v4l2_g_ctrl(sd->ctrl_handler, arg);
+                return v4l2_g_ctrl(vfh->ctrl_handler, arg);
 
         case VIDIOC_S_CTRL:
-                return v4l2_s_ctrl(sd->ctrl_handler, arg);
+                return v4l2_s_ctrl(vfh, vfh->ctrl_handler, arg);
 
         case VIDIOC_G_EXT_CTRLS:
-                return v4l2_g_ext_ctrls(sd->ctrl_handler, arg);
+                return v4l2_g_ext_ctrls(vfh->ctrl_handler, arg);
 
         case VIDIOC_S_EXT_CTRLS:
-                return v4l2_s_ext_ctrls(sd->ctrl_handler, arg);
+                return v4l2_s_ext_ctrls(vfh, vfh->ctrl_handler, arg);
 
         case VIDIOC_TRY_EXT_CTRLS:
-                return v4l2_try_ext_ctrls(sd->ctrl_handler, arg);
+                return v4l2_try_ext_ctrls(vfh->ctrl_handler, arg);
 
         case VIDIOC_DQEVENT:
                 if (!(sd->flags & V4L2_SUBDEV_FL_HAS_EVENTS))
@@ -297,7 +284,7 @@ static unsigned int subdev_poll(struct file *file, poll_table *wait)
         if (!(sd->flags & V4L2_SUBDEV_FL_HAS_EVENTS))
                 return POLLERR;
 
-        poll_wait(file, &fh->events->wait, wait);
+        poll_wait(file, &fh->wait, wait);
 
         if (v4l2_event_pending(fh))
                 return POLLPRI;
diff --git a/drivers/media/video/videobuf-dma-sg.c b/drivers/media/video/videobuf-dma-sg.c
index ddb8f4b46c0..f300deafd26 100644
--- a/drivers/media/video/videobuf-dma-sg.c
+++ b/drivers/media/video/videobuf-dma-sg.c
@@ -108,8 +108,9 @@ static struct scatterlist *videobuf_pages_to_sg(struct page **pages,
         if (PageHighMem(pages[0]))
                 /* DMA to highmem pages might not work */
                 goto highmem;
-        sg_set_page(&sglist[0], pages[0], PAGE_SIZE - offset, offset);
-        size -= PAGE_SIZE - offset;
+        sg_set_page(&sglist[0], pages[0],
+                        min_t(size_t, PAGE_SIZE - offset, size), offset);
+        size -= min_t(size_t, PAGE_SIZE - offset, size);
         for (i = 1; i < nr_pages; i++) {
                 if (NULL == pages[i])
                         goto nopage;
diff --git a/drivers/media/video/videobuf2-dma-sg.c b/drivers/media/video/videobuf2-dma-sg.c
index 10a20d9509d..065f468faf8 100644
--- a/drivers/media/video/videobuf2-dma-sg.c
+++ b/drivers/media/video/videobuf2-dma-sg.c
@@ -48,12 +48,10 @@ static void *vb2_dma_sg_alloc(void *alloc_ctx, unsigned long size)
         buf->sg_desc.size = size;
         buf->sg_desc.num_pages = (size + PAGE_SIZE - 1) >> PAGE_SHIFT;
 
-        buf->sg_desc.sglist = vmalloc(buf->sg_desc.num_pages *
+        buf->sg_desc.sglist = vzalloc(buf->sg_desc.num_pages *
                                       sizeof(*buf->sg_desc.sglist));
         if (!buf->sg_desc.sglist)
                 goto fail_sglist_alloc;
-        memset(buf->sg_desc.sglist, 0, buf->sg_desc.num_pages *
-               sizeof(*buf->sg_desc.sglist));
         sg_init_table(buf->sg_desc.sglist, buf->sg_desc.num_pages);
 
         buf->pages = kzalloc(buf->sg_desc.num_pages * sizeof(struct page *),
@@ -136,13 +134,11 @@ static void *vb2_dma_sg_get_userptr(void *alloc_ctx, unsigned long vaddr,
         last  = ((vaddr + size - 1) & PAGE_MASK) >> PAGE_SHIFT;
         buf->sg_desc.num_pages = last - first + 1;
 
-        buf->sg_desc.sglist = vmalloc(
+        buf->sg_desc.sglist = vzalloc(
                 buf->sg_desc.num_pages * sizeof(*buf->sg_desc.sglist));
         if (!buf->sg_desc.sglist)
                 goto userptr_fail_sglist_alloc;
 
-        memset(buf->sg_desc.sglist, 0,
-                buf->sg_desc.num_pages * sizeof(*buf->sg_desc.sglist));
         sg_init_table(buf->sg_desc.sglist, buf->sg_desc.num_pages);
 
         buf->pages = kzalloc(buf->sg_desc.num_pages * sizeof(struct page *),
diff --git a/drivers/media/video/videobuf2-memops.c b/drivers/media/video/videobuf2-memops.c
index b03c3aea5be..569eeb3dfd5 100644
--- a/drivers/media/video/videobuf2-memops.c
+++ b/drivers/media/video/videobuf2-memops.c
@@ -176,7 +176,7 @@ int vb2_mmap_pfn_range(struct vm_area_struct *vma, unsigned long paddr,
 
         vma->vm_ops->open(vma);
 
-        printk(KERN_DEBUG "%s: mapped paddr 0x%08lx at 0x%08lx, size %ld\n",
+        pr_debug("%s: mapped paddr 0x%08lx at 0x%08lx, size %ld\n",
                         __func__, paddr, vma->vm_start, size);
 
         return 0;
@@ -194,7 +194,7 @@ static void vb2_common_vm_open(struct vm_area_struct *vma)
 {
         struct vb2_vmarea_handler *h = vma->vm_private_data;
 
-        printk(KERN_DEBUG "%s: %p, refcount: %d, vma: %08lx-%08lx\n",
+        pr_debug("%s: %p, refcount: %d, vma: %08lx-%08lx\n",
                __func__, h, atomic_read(h->refcount), vma->vm_start,
                vma->vm_end);
 
@@ -212,7 +212,7 @@ static void vb2_common_vm_close(struct vm_area_struct *vma)
 {
         struct vb2_vmarea_handler *h = vma->vm_private_data;
 
-        printk(KERN_DEBUG "%s: %p, refcount: %d, vma: %08lx-%08lx\n",
+        pr_debug("%s: %p, refcount: %d, vma: %08lx-%08lx\n",
                __func__, h, atomic_read(h->refcount), vma->vm_start,
                vma->vm_end);
 
diff --git a/drivers/media/video/vino.c b/drivers/media/video/vino.c
index d63e9d97849..52a0a3736c8 100644
--- a/drivers/media/video/vino.c
+++ b/drivers/media/video/vino.c
@@ -36,7 +36,6 @@
 #include <linux/slab.h>
 #include <linux/mm.h>
 #include <linux/time.h>
-#include <linux/version.h>
 #include <linux/kmod.h>
 
 #include <linux/i2c.h>
@@ -61,8 +60,7 @@
 // #define VINO_DEBUG
 // #define VINO_DEBUG_INT
 
-#define VINO_MODULE_VERSION "0.0.6"
-#define VINO_VERSION_CODE KERNEL_VERSION(0, 0, 6)
+#define VINO_MODULE_VERSION "0.0.7"
 
 MODULE_DESCRIPTION("SGI VINO Video4Linux2 driver");
 MODULE_VERSION(VINO_MODULE_VERSION);
@@ -2934,7 +2932,6 @@ static int vino_querycap(struct file *file, void *__fh,
         strcpy(cap->driver, vino_driver_name);
         strcpy(cap->card, vino_driver_description);
         strcpy(cap->bus_info, vino_bus_name);
-        cap->version = VINO_VERSION_CODE;
         cap->capabilities =
                 V4L2_CAP_VIDEO_CAPTURE |
                 V4L2_CAP_STREAMING;
diff --git a/drivers/media/video/vivi.c b/drivers/media/video/vivi.c
index 2238a613d66..a848bd2af97 100644
--- a/drivers/media/video/vivi.c
+++ b/drivers/media/video/vivi.c
@@ -22,7 +22,6 @@
 #include <linux/sched.h>
 #include <linux/slab.h>
 #include <linux/font.h>
-#include <linux/version.h>
 #include <linux/mutex.h>
 #include <linux/videodev2.h>
 #include <linux/kthread.h>
@@ -32,6 +31,7 @@
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
 #include <media/v4l2-common.h>
 
 #define VIVI_MODULE_NAME "vivi"
@@ -44,15 +44,12 @@
 #define MAX_WIDTH 1920
 #define MAX_HEIGHT 1200
 
-#define VIVI_MAJOR_VERSION 0
-#define VIVI_MINOR_VERSION 8
-#define VIVI_RELEASE 0
-#define VIVI_VERSION \
-        KERNEL_VERSION(VIVI_MAJOR_VERSION, VIVI_MINOR_VERSION, VIVI_RELEASE)
+#define VIVI_VERSION "0.8.1"
 
 MODULE_DESCRIPTION("Video Technology Magazine Virtual Video Capture Board");
 MODULE_AUTHOR("Mauro Carvalho Chehab, Ted Walther and John Sokol");
 MODULE_LICENSE("Dual BSD/GPL");
+MODULE_VERSION(VIVI_VERSION);
 
 static unsigned video_nr = -1;
 module_param(video_nr, uint, 0644);
@@ -167,6 +164,11 @@ struct vivi_dev {
         struct v4l2_ctrl           *contrast;
         struct v4l2_ctrl           *saturation;
         struct v4l2_ctrl           *hue;
+        struct {
+                /* autogain/gain cluster */
+                struct v4l2_ctrl           *autogain;
+                struct v4l2_ctrl           *gain;
+        };
         struct v4l2_ctrl           *volume;
         struct v4l2_ctrl           *button;
         struct v4l2_ctrl           *boolean;
@@ -174,6 +176,7 @@ struct vivi_dev {
         struct v4l2_ctrl           *int64;
         struct v4l2_ctrl           *menu;
         struct v4l2_ctrl           *string;
+        struct v4l2_ctrl           *bitmask;
 
         spinlock_t                 slock;
         struct mutex               mutex;
@@ -457,6 +460,7 @@ static void vivi_fillbuff(struct vivi_dev *dev, struct vivi_buffer *buf)
         unsigned ms;
         char str[100];
         int h, line = 1;
+        s32 gain;
 
         if (!vbuf)
                 return;
@@ -479,6 +483,7 @@ static void vivi_fillbuff(struct vivi_dev *dev, struct vivi_buffer *buf)
                         dev->width, dev->height, dev->input);
         gen_text(dev, vbuf, line++ * 16, 16, str);
 
+        gain = v4l2_ctrl_g_ctrl(dev->gain);
         mutex_lock(&dev->ctrl_handler.lock);
         snprintf(str, sizeof(str), " brightness %3d, contrast %3d, saturation %3d, hue %d ",
                         dev->brightness->cur.val,
@@ -486,11 +491,13 @@ static void vivi_fillbuff(struct vivi_dev *dev, struct vivi_buffer *buf)
                         dev->saturation->cur.val,
                         dev->hue->cur.val);
         gen_text(dev, vbuf, line++ * 16, 16, str);
-        snprintf(str, sizeof(str), " volume %3d ", dev->volume->cur.val);
+        snprintf(str, sizeof(str), " autogain %d, gain %3d, volume %3d ",
+                        dev->autogain->cur.val, gain, dev->volume->cur.val);
         gen_text(dev, vbuf, line++ * 16, 16, str);
-        snprintf(str, sizeof(str), " int32 %d, int64 %lld ",
+        snprintf(str, sizeof(str), " int32 %d, int64 %lld, bitmask %08x ",
                         dev->int32->cur.val,
-                        dev->int64->cur.val64);
+                        dev->int64->cur.val64,
+                        dev->bitmask->cur.val);
         gen_text(dev, vbuf, line++ * 16, 16, str);
         snprintf(str, sizeof(str), " boolean %d, menu %s, string \"%s\" ",
                         dev->boolean->cur.val,
@@ -524,11 +531,13 @@ static void vivi_thread_tick(struct vivi_dev *dev)
         spin_lock_irqsave(&dev->slock, flags);
         if (list_empty(&dma_q->active)) {
                 dprintk(dev, 1, "No active queue to serve\n");
-                goto unlock;
+                spin_unlock_irqrestore(&dev->slock, flags);
+                return;
         }
 
         buf = list_entry(dma_q->active.next, struct vivi_buffer, list);
         list_del(&buf->list);
+        spin_unlock_irqrestore(&dev->slock, flags);
 
         do_gettimeofday(&buf->vb.v4l2_buf.timestamp);
 
@@ -538,8 +547,6 @@ static void vivi_thread_tick(struct vivi_dev *dev)
 
         vb2_buffer_done(&buf->vb, VB2_BUF_STATE_DONE);
         dprintk(dev, 2, "[%p/%d] done\n", buf, buf->vb.v4l2_buf.index);
-unlock:
-        spin_unlock_irqrestore(&dev->slock, flags);
 }
 
 #define frames_to_ms(frames)                                    \
@@ -812,7 +819,6 @@ static int vidioc_querycap(struct file *file, void  *priv,
         strcpy(cap->driver, "vivi");
         strcpy(cap->card, "vivi");
         strlcpy(cap->bus_info, dev->v4l2_dev.name, sizeof(cap->bus_info));
-        cap->version = VIVI_VERSION;
         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING | \
                             V4L2_CAP_READWRITE;
         return 0;
@@ -975,14 +981,37 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
         if (i >= NUM_INPUTS)
                 return -EINVAL;
 
+        if (i == dev->input)
+                return 0;
+
         dev->input = i;
         precalculate_bars(dev);
         precalculate_line(dev);
         return 0;
 }
 
+static int vidioc_subscribe_event(struct v4l2_fh *fh,
+                                struct v4l2_event_subscription *sub)
+{
+        switch (sub->type) {
+        case V4L2_EVENT_CTRL:
+                return v4l2_event_subscribe(fh, sub, 0);
+        default:
+                return -EINVAL;
+        }
+}
+
 /* --- controls ---------------------------------------------- */
 
+static int vivi_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct vivi_dev *dev = container_of(ctrl->handler, struct vivi_dev, ctrl_handler);
+
+        if (ctrl == dev->autogain)
+                dev->gain->val = jiffies & 0xff;
+        return 0;
+}
+
 static int vivi_s_ctrl(struct v4l2_ctrl *ctrl)
 {
         struct vivi_dev *dev = container_of(ctrl->handler, struct vivi_dev, ctrl_handler);
@@ -1010,10 +1039,17 @@ static unsigned int
 vivi_poll(struct file *file, struct poll_table_struct *wait)
 {
         struct vivi_dev *dev = video_drvdata(file);
+        struct v4l2_fh *fh = file->private_data;
         struct vb2_queue *q = &dev->vb_vidq;
+        unsigned int res;
 
         dprintk(dev, 1, "%s\n", __func__);
-        return vb2_poll(q, file, wait);
+        res = vb2_poll(q, file, wait);
+        if (v4l2_event_pending(fh))
+                res |= POLLPRI;
+        else
+                poll_wait(file, &fh->wait, wait);
+        return res;
 }
 
 static int vivi_close(struct file *file)
@@ -1045,6 +1081,7 @@ static int vivi_mmap(struct file *file, struct vm_area_struct *vma)
 }
 
 static const struct v4l2_ctrl_ops vivi_ctrl_ops = {
+        .g_volatile_ctrl = vivi_g_volatile_ctrl,
         .s_ctrl = vivi_s_ctrl,
 };
 
@@ -1117,9 +1154,20 @@ static const struct v4l2_ctrl_config vivi_ctrl_string = {
         .step = 1,
 };
 
+static const struct v4l2_ctrl_config vivi_ctrl_bitmask = {
+        .ops = &vivi_ctrl_ops,
+        .id = VIVI_CID_CUSTOM_BASE + 6,
+        .name = "Bitmask",
+        .type = V4L2_CTRL_TYPE_BITMASK,
+        .def = 0x80002000,
+        .min = 0,
+        .max = 0x80402010,
+        .step = 0,
+};
+
 static const struct v4l2_file_operations vivi_fops = {
         .owner          = THIS_MODULE,
-        .open           = v4l2_fh_open,
+        .open           = v4l2_fh_open,
         .release        = vivi_close,
         .read           = vivi_read,
         .poll           = vivi_poll,
@@ -1143,6 +1191,8 @@ static const struct v4l2_ioctl_ops vivi_ioctl_ops = {
         .vidioc_s_input       = vidioc_s_input,
         .vidioc_streamon      = vidioc_streamon,
         .vidioc_streamoff     = vidioc_streamoff,
+        .vidioc_subscribe_event = vidioc_subscribe_event,
+        .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static struct video_device vivi_template = {
@@ -1213,16 +1263,22 @@ static int __init vivi_create_instance(int inst)
                         V4L2_CID_SATURATION, 0, 255, 1, 127);
         dev->hue = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
                         V4L2_CID_HUE, -128, 127, 1, 0);
+        dev->autogain = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
+                        V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+        dev->gain = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
+                        V4L2_CID_GAIN, 0, 255, 1, 100);
         dev->button = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_button, NULL);
         dev->int32 = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_int32, NULL);
         dev->int64 = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_int64, NULL);
         dev->boolean = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_boolean, NULL);
         dev->menu = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_menu, NULL);
         dev->string = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_string, NULL);
+        dev->bitmask = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_bitmask, NULL);
         if (hdl->error) {
                 ret = hdl->error;
                 goto unreg_dev;
         }
+        v4l2_ctrl_auto_cluster(2, &dev->autogain, 0, true);
         dev->v4l2_dev.ctrl_handler = hdl;
 
         /* initialize locks */
@@ -1325,9 +1381,8 @@ static int __init vivi_init(void)
         }
 
         printk(KERN_INFO "Video Technology Magazine Virtual Video "
-                        "Capture Board ver %u.%u.%u successfully loaded.\n",
-                        (VIVI_VERSION >> 16) & 0xFF, (VIVI_VERSION >> 8) & 0xFF,
-                        VIVI_VERSION & 0xFF);
+                        "Capture Board ver %s successfully loaded.\n",
+                        VIVI_VERSION);
 
         /* n_devs will reflect the actual number of allocated devices */
         n_devs = i;
diff --git a/drivers/media/video/w9966.c b/drivers/media/video/w9966.c
index fa35639d0c1..453dbbd1e6e 100644
--- a/drivers/media/video/w9966.c
+++ b/drivers/media/video/w9966.c
@@ -57,7 +57,6 @@
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/delay.h>
-#include <linux/version.h>
 #include <linux/videodev2.h>
 #include <linux/slab.h>
 #include <media/v4l2-common.h>
@@ -127,7 +126,7 @@ struct w9966 {
 MODULE_AUTHOR("Jakob Kemi <jakob.kemi@post.utfors.se>");
 MODULE_DESCRIPTION("Winbond w9966cf WebCam driver (0.32)");
 MODULE_LICENSE("GPL");
-
+MODULE_VERSION("0.33.1");
 
 #ifdef MODULE
 static const char *pardev[] = {[0 ... W9966_MAXCAMS] = ""};
@@ -568,7 +567,6 @@ static int cam_querycap(struct file *file, void  *priv,
         strlcpy(vcap->driver, cam->v4l2_dev.name, sizeof(vcap->driver));
         strlcpy(vcap->card, W9966_DRIVERNAME, sizeof(vcap->card));
         strlcpy(vcap->bus_info, "parport", sizeof(vcap->bus_info));
-        vcap->version = KERNEL_VERSION(0, 33, 0);
         vcap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
         return 0;
 }
diff --git a/drivers/media/video/zoran/zoran.h b/drivers/media/video/zoran/zoran.h
index f3f64001492..d7166afc255 100644
--- a/drivers/media/video/zoran/zoran.h
+++ b/drivers/media/video/zoran/zoran.h
@@ -41,10 +41,6 @@ struct zoran_sync {
 };
 
 
-#define MAJOR_VERSION 0         /* driver major version */
-#define MINOR_VERSION 10        /* driver minor version */
-#define RELEASE_VERSION 0       /* release version */
-
 #define ZORAN_NAME    "ZORAN"   /* name of the device */
 
 #define ZR_DEVNAME(zr) ((zr)->name)
diff --git a/drivers/media/video/zoran/zoran_card.c b/drivers/media/video/zoran/zoran_card.c
index 79b04ac0f1a..c3602d6cd48 100644
--- a/drivers/media/video/zoran/zoran_card.c
+++ b/drivers/media/video/zoran/zoran_card.c
@@ -123,9 +123,12 @@ int zr36067_debug = 1;
 module_param_named(debug, zr36067_debug, int, 0644);
 MODULE_PARM_DESC(debug, "Debug level (0-5)");
 
+#define ZORAN_VERSION "0.10.1"
+
 MODULE_DESCRIPTION("Zoran-36057/36067 JPEG codec driver");
 MODULE_AUTHOR("Serguei Miridonov");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(ZORAN_VERSION);
 
 #define ZR_DEVICE(subven, subdev, data) { \
         .vendor = PCI_VENDOR_ID_ZORAN, .device = PCI_DEVICE_ID_ZORAN_36057, \
@@ -1459,8 +1462,8 @@ static int __init zoran_init(void)
 {
         int res;
 
-        printk(KERN_INFO "Zoran MJPEG board driver version %d.%d.%d\n",
-               MAJOR_VERSION, MINOR_VERSION, RELEASE_VERSION);
+        printk(KERN_INFO "Zoran MJPEG board driver version %s\n",
+               ZORAN_VERSION);
 
         /* check the parameters we have been given, adjust if necessary */
         if (v4l_nbufs < 2)
diff --git a/drivers/media/video/zoran/zoran_driver.c b/drivers/media/video/zoran/zoran_driver.c
index 2771d818406..d4d05d2ace6 100644
--- a/drivers/media/video/zoran/zoran_driver.c
+++ b/drivers/media/video/zoran/zoran_driver.c
@@ -44,7 +44,6 @@
  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
-#include <linux/version.h>
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/delay.h>
@@ -1538,8 +1537,6 @@ static int zoran_querycap(struct file *file, void *__fh, struct v4l2_capability
         strncpy(cap->driver, "zoran", sizeof(cap->driver)-1);
         snprintf(cap->bus_info, sizeof(cap->bus_info), "PCI:%s",
                  pci_name(zr->pci_dev));
-        cap->version = KERNEL_VERSION(MAJOR_VERSION, MINOR_VERSION,
-                           RELEASE_VERSION);
         cap->capabilities = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE |
                             V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_VIDEO_OVERLAY;
         return 0;
diff --git a/drivers/media/video/zr364xx.c b/drivers/media/video/zr364xx.c
index 7dfb01e9930..c492846c1c5 100644
--- a/drivers/media/video/zr364xx.c
+++ b/drivers/media/video/zr364xx.c
@@ -29,7 +29,6 @@
 
 
 #include <linux/module.h>
-#include <linux/version.h>
 #include <linux/init.h>
 #include <linux/usb.h>
 #include <linux/vmalloc.h>
@@ -42,8 +41,7 @@
 
 
 /* Version Information */
-#define DRIVER_VERSION "v0.73"
-#define ZR364XX_VERSION_CODE KERNEL_VERSION(0, 7, 3)
+#define DRIVER_VERSION "0.7.4"
 #define DRIVER_AUTHOR "Antoine Jacquet, http://royale.zerezo.com/"
 #define DRIVER_DESC "Zoran 364xx"
 
@@ -744,7 +742,6 @@ static int zr364xx_vidioc_querycap(struct file *file, void *priv,
         strlcpy(cap->card, cam->udev->product, sizeof(cap->card));
         strlcpy(cap->bus_info, dev_name(&cam->udev->dev),
                 sizeof(cap->bus_info));
-        cap->version = ZR364XX_VERSION_CODE;
         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
                             V4L2_CAP_READWRITE |
                             V4L2_CAP_STREAMING;
@@ -1721,3 +1718,4 @@ module_exit(zr364xx_exit);
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
+MODULE_VERSION(DRIVER_VERSION);