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

* 'v4l_for_linus' of git://git.kernel.org/pub/scm/linux/kernel/git/mchehab/linux-media: (144 commits)
  [media] saa7134.h: Suppress compiler warnings when CONFIG_VIDEO_SAA7134_RC is not set
  [media] it913x [VER 1.07] Support for single ITE 9135 devices
  [media] Support for Terratec G1
  [media] cx25821: off by one in cx25821_vidioc_s_input()
  [media] media: tea5764: reconcile Kconfig symbol and macro
  [media] omap_vout: Add poll() support
  [media] omap3isp: preview: Add crop support on the sink pad
  [media] omap3isp: preview: Rename min/max input/output sizes defines
  [media] omap3isp: preview: Remove horizontal averager support
  [media] omap3isp: Report the ISP revision through the media controller API
  [media] omap3isp: ccdc: remove redundant operation
  [media] omap3isp: Fix memory leaks in initialization error paths
  [media] omap3isp: Add missing mutex_destroy() calls
  [media] omap3isp: Move *_init_entities() functions to the init/cleanup section
  [media] omap3isp: Move media_entity_cleanup() from unregister() to cleanup()
  [media] MFC: Change MFC firmware binary name
  [media] vb2: add vb2_get_unmapped_area in vb2 core
  [media] v4l: Add v4l2 subdev driver for S5K6AAFX sensor
  [media] v4l: Add AUTO option for the V4L2_CID_POWER_LINE_FREQUENCY control
  [media] media: ov6650: stylistic improvements
  ...

98 files changed, 18142 insertions, 3612 deletions

diff --git a/drivers/media/dvb/ddbridge/Makefile b/drivers/media/dvb/ddbridge/Makefile
index cf7214edf65f..38019bafb862 100644
--- a/drivers/media/dvb/ddbridge/Makefile
+++ b/drivers/media/dvb/ddbridge/Makefile
@@ -11,4 +11,4 @@ ccflags-y += -Idrivers/media/dvb/frontends/
 ccflags-y += -Idrivers/media/common/tuners/
 
 # For the staging CI driver cxd2099
-ccflags-y += -Idrivers/staging/cxd2099/
+ccflags-y += -Idrivers/staging/media/cxd2099/
diff --git a/drivers/media/dvb/dvb-usb/Makefile b/drivers/media/dvb/dvb-usb/Makefile
index 7d0710bb1978..26c8b9e57050 100644
--- a/drivers/media/dvb/dvb-usb/Makefile
+++ b/drivers/media/dvb/dvb-usb/Makefile
@@ -102,6 +102,7 @@ obj-$(CONFIG_DVB_USB_IT913X) += dvb-usb-it913x.o
 
 dvb-usb-mxl111sf-objs = mxl111sf.o mxl111sf-phy.o mxl111sf-i2c.o mxl111sf-gpio.o
 obj-$(CONFIG_DVB_USB_MXL111SF) += dvb-usb-mxl111sf.o
+obj-$(CONFIG_DVB_USB_MXL111SF) += mxl111sf-demod.o
 obj-$(CONFIG_DVB_USB_MXL111SF) += mxl111sf-tuner.o
 
 ccflags-y += -Idrivers/media/dvb/dvb-core/ -Idrivers/media/dvb/frontends/
diff --git a/drivers/media/dvb/dvb-usb/dvb-usb-ids.h b/drivers/media/dvb/dvb-usb/dvb-usb-ids.h
index 2ad33ba92ba2..2d08c9b5128a 100644
--- a/drivers/media/dvb/dvb-usb/dvb-usb-ids.h
+++ b/drivers/media/dvb/dvb-usb/dvb-usb-ids.h
@@ -37,6 +37,7 @@
 #define USB_VID_HAUPPAUGE                       0x2040
 #define USB_VID_HYPER_PALTEK                    0x1025
 #define USB_VID_INTEL                           0x8086
+#define USB_VID_ITETECH                         0x048d
 #define USB_VID_KWORLD                          0xeb2a
 #define USB_VID_KWORLD_2                        0x1b80
 #define USB_VID_KYE                             0x0458
@@ -126,6 +127,7 @@
 #define USB_PID_GRANDTEC_DVBT_USB_COLD                  0x0fa0
 #define USB_PID_GRANDTEC_DVBT_USB_WARM                  0x0fa1
 #define USB_PID_INTEL_CE9500                            0x9500
+#define USB_PID_ITETECH_IT9135                          0x9135
 #define USB_PID_KWORLD_399U                             0xe399
 #define USB_PID_KWORLD_399U_2                           0xe400
 #define USB_PID_KWORLD_395U                             0xe396
diff --git a/drivers/media/dvb/dvb-usb/it913x.c b/drivers/media/dvb/dvb-usb/it913x.c
index f027a2c1c3e8..c46226187143 100644
--- a/drivers/media/dvb/dvb-usb/it913x.c
+++ b/drivers/media/dvb/dvb-usb/it913x.c
@@ -60,6 +60,17 @@ struct it913x_state {
         u8 id;
 };
 
+struct ite_config {
+        u8 chip_ver;
+        u16 chip_type;
+        u32 firmware;
+        u8 tuner_id_0;
+        u8 tuner_id_1;
+        u8 dual_mode;
+};
+
+struct ite_config it913x_config;
+
 static int it913x_bulk_write(struct usb_device *dev,
                                 u8 *snd, int len, u8 pipe)
 {
@@ -191,18 +202,23 @@ static int it913x_read_reg(struct usb_device *udev, u32 reg)
 static u32 it913x_query(struct usb_device *udev, u8 pro)
 {
         int ret;
-        u32 res = 0;
         u8 data[4];
         ret = it913x_io(udev, READ_LONG, pro, CMD_DEMOD_READ,
-                0x1222, 0, &data[0], 1);
-        if (data[0] == 0x1) {
-                ret = it913x_io(udev, READ_SHORT, pro,
+                0x1222, 0, &data[0], 3);
+
+        it913x_config.chip_ver = data[0];
+        it913x_config.chip_type = (u16)(data[2] << 8) + data[1];
+
+        info("Chip Version=%02x Chip Type=%04x", it913x_config.chip_ver,
+                it913x_config.chip_type);
+
+        ret |= it913x_io(udev, READ_SHORT, pro,
                         CMD_QUERYINFO, 0, 0x1, &data[0], 4);
-                res = (data[0] << 24) + (data[1] << 16) +
+
+        it913x_config.firmware = (data[0] << 24) + (data[1] << 16) +
                         (data[2] << 8) + data[3];
-        }
 
-        return (ret < 0) ? 0 : res;
+        return (ret < 0) ? 0 : it913x_config.firmware;
 }
 
 static int it913x_pid_filter_ctrl(struct dvb_usb_adapter *adap, int onoff)
@@ -336,26 +352,35 @@ static int it913x_identify_state(struct usb_device *udev,
                 int *cold)
 {
         int ret = 0, firm_no;
-        u8 reg, adap, ep, tun0, tun1;
+        u8 reg, remote;
 
         firm_no = it913x_return_status(udev);
 
-        ep = it913x_read_reg(udev, 0x49ac);
-        adap = it913x_read_reg(udev, 0x49c5);
-        tun0 = it913x_read_reg(udev, 0x49d0);
-        info("No. Adapters=%x Endpoints=%x Tuner Type=%x", adap, ep, tun0);
+        /* checnk for dual mode */
+        it913x_config.dual_mode =  it913x_read_reg(udev, 0x49c5);
+
+        /* TODO different remotes */
+        remote = it913x_read_reg(udev, 0x49ac); /* Remote */
+        if (remote == 0)
+                props->rc.core.rc_codes = NULL;
+
+        /* TODO at the moment tuner_id is always assigned to 0x38 */
+        it913x_config.tuner_id_0 = it913x_read_reg(udev, 0x49d0);
+
+        info("Dual mode=%x Remote=%x Tuner Type=%x", it913x_config.dual_mode
+                , remote, it913x_config.tuner_id_0);
 
         if (firm_no > 0) {
                 *cold = 0;
                 return 0;
         }
 
-        if (adap > 2) {
-                tun1 = it913x_read_reg(udev, 0x49e0);
+        if (it913x_config.dual_mode) {
+                it913x_config.tuner_id_1 = it913x_read_reg(udev, 0x49e0);
                 ret = it913x_wr_reg(udev, DEV_0, GPIOH1_EN, 0x1);
                 ret |= it913x_wr_reg(udev, DEV_0, GPIOH1_ON, 0x1);
                 ret |= it913x_wr_reg(udev, DEV_0, GPIOH1_O, 0x1);
-                msleep(50); /* Delay noticed reset cycle ? */
+                msleep(50);
                 ret |= it913x_wr_reg(udev, DEV_0, GPIOH1_O, 0x0);
                 msleep(50);
                 reg = it913x_read_reg(udev, GPIOH1_O);
@@ -366,14 +391,19 @@ static int it913x_identify_state(struct usb_device *udev,
                                 ret = it913x_wr_reg(udev, DEV_0,
                                         GPIOH1_O, 0x0);
                 }
+                props->num_adapters = 2;
         } else
                 props->num_adapters = 1;
 
         reg = it913x_read_reg(udev, IO_MUX_POWER_CLK);
 
-        ret |= it913x_wr_reg(udev, DEV_0, 0x4bfb, CHIP2_I2C_ADDR);
-
-        ret |= it913x_wr_reg(udev, DEV_0,  CLK_O_EN, 0x1);
+        if (it913x_config.dual_mode) {
+                ret |= it913x_wr_reg(udev, DEV_0, 0x4bfb, CHIP2_I2C_ADDR);
+                ret |= it913x_wr_reg(udev, DEV_0,  CLK_O_EN, 0x1);
+        } else {
+                ret |= it913x_wr_reg(udev, DEV_0, 0x4bfb, 0x0);
+                ret |= it913x_wr_reg(udev, DEV_0,  CLK_O_EN, 0x0);
+        }
 
         *cold = 1;
 
@@ -403,13 +433,11 @@ static int it913x_download_firmware(struct usb_device *udev,
                                         const struct firmware *fw)
 {
         int ret = 0, i;
-        u8 packet_size, dlen, tun1;
+        u8 packet_size, dlen;
         u8 *fw_data;
 
         packet_size = 0x29;
 
-        tun1 = it913x_read_reg(udev, 0x49e0);
-
         ret = it913x_wr_reg(udev, DEV_0,  I2C_CLK, I2C_CLK_100);
 
         info("FRM Starting Firmware Download");
@@ -444,11 +472,12 @@ static int it913x_download_firmware(struct usb_device *udev,
         ret |= it913x_wr_reg(udev, DEV_0,  I2C_CLK, I2C_CLK_400);
 
         /* Tuner function */
-        ret |= it913x_wr_reg(udev, DEV_0_DMOD , 0xec4c, 0xa0);
+        if (it913x_config.dual_mode)
+                ret |= it913x_wr_reg(udev, DEV_0_DMOD , 0xec4c, 0xa0);
 
         ret |= it913x_wr_reg(udev, DEV_0,  PADODPU, 0x0);
         ret |= it913x_wr_reg(udev, DEV_0,  AGC_O_D, 0x0);
-        if (tun1 > 0) {
+        if (it913x_config.dual_mode) {
                 ret |= it913x_wr_reg(udev, DEV_1,  PADODPU, 0x0);
                 ret |= it913x_wr_reg(udev, DEV_1,  AGC_O_D, 0x0);
         }
@@ -475,9 +504,28 @@ static int it913x_frontend_attach(struct dvb_usb_adapter *adap)
         u8 adf = it913x_read_reg(udev, IO_MUX_POWER_CLK);
         u8 adap_addr = I2C_BASE_ADDR + (adap->id << 5);
         u16 ep_size = adap->props.fe[0].stream.u.bulk.buffersize;
+        u8 tuner_id, tuner_type;
+
+        if (adap->id == 0)
+                tuner_id = it913x_config.tuner_id_0;
+        else
+                tuner_id = it913x_config.tuner_id_1;
+
+        /* TODO we always use IT9137 possible references here*/
+        /* Documentation suggests don't care */
+        switch (tuner_id) {
+        case 0x51:
+        case 0x52:
+        case 0x60:
+        case 0x61:
+        case 0x62:
+        default:
+        case 0x38:
+                tuner_type = IT9137;
+        }
 
         adap->fe_adap[0].fe = dvb_attach(it913x_fe_attach,
-                &adap->dev->i2c_adap, adap_addr, adf, IT9137);
+                &adap->dev->i2c_adap, adap_addr, adf, tuner_type);
 
         if (adap->id == 0 && adap->fe_adap[0].fe) {
                 ret = it913x_wr_reg(udev, DEV_0_DMOD, MP2_SW_RST, 0x1);
@@ -533,6 +581,7 @@ static int it913x_probe(struct usb_interface *intf,
 
 static struct usb_device_id it913x_table[] = {
         { USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_UB499_2T_T09) },
+        { USB_DEVICE(USB_VID_ITETECH, USB_PID_ITETECH_IT9135) },
         {}              /* Terminating entry */
 };
 
@@ -608,12 +657,14 @@ static struct dvb_usb_device_properties it913x_properties = {
                 .rc_codes       = RC_MAP_KWORLD_315U,
         },
         .i2c_algo         = &it913x_i2c_algo,
-        .num_device_descs = 1,
+        .num_device_descs = 2,
         .devices = {
                 {   "Kworld UB499-2T T09(IT9137)",
                         { &it913x_table[0], NULL },
                         },
-
+                {   "ITE 9135 Generic",
+                        { &it913x_table[1], NULL },
+                        },
         }
 };
 
@@ -647,5 +698,5 @@ module_exit(it913x_module_exit);
 
 MODULE_AUTHOR("Malcolm Priestley <tvboxspy@gmail.com>");
 MODULE_DESCRIPTION("it913x USB 2 Driver");
-MODULE_VERSION("1.06");
+MODULE_VERSION("1.07");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb/dvb-usb/mxl111sf-demod.c b/drivers/media/dvb/dvb-usb/mxl111sf-demod.c
new file mode 100644
index 000000000000..d1f58371c711
--- /dev/null
+++ b/drivers/media/dvb/dvb-usb/mxl111sf-demod.c
@@ -0,0 +1,614 @@
+/*
+ *  mxl111sf-demod.c - driver for the MaxLinear MXL111SF DVB-T demodulator
+ *
+ *  Copyright (C) 2010 Michael Krufky <mkrufky@kernellabs.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include "mxl111sf-demod.h"
+#include "mxl111sf-reg.h"
+
+/* debug */
+static int mxl111sf_demod_debug;
+module_param_named(debug, mxl111sf_demod_debug, int, 0644);
+MODULE_PARM_DESC(debug, "set debugging level (1=info (or-able)).");
+
+#define mxl_dbg(fmt, arg...) \
+        if (mxl111sf_demod_debug) \
+                mxl_printk(KERN_DEBUG, fmt, ##arg)
+
+/* ------------------------------------------------------------------------ */
+
+struct mxl111sf_demod_state {
+        struct mxl111sf_state *mxl_state;
+
+        struct mxl111sf_demod_config *cfg;
+
+        struct dvb_frontend fe;
+};
+
+/* ------------------------------------------------------------------------ */
+
+static int mxl111sf_demod_read_reg(struct mxl111sf_demod_state *state,
+                                   u8 addr, u8 *data)
+{
+        return (state->cfg->read_reg) ?
+                state->cfg->read_reg(state->mxl_state, addr, data) :
+                -EINVAL;
+}
+
+static int mxl111sf_demod_write_reg(struct mxl111sf_demod_state *state,
+                                    u8 addr, u8 data)
+{
+        return (state->cfg->write_reg) ?
+                state->cfg->write_reg(state->mxl_state, addr, data) :
+                -EINVAL;
+}
+
+static
+int mxl111sf_demod_program_regs(struct mxl111sf_demod_state *state,
+                                struct mxl111sf_reg_ctrl_info *ctrl_reg_info)
+{
+        return (state->cfg->program_regs) ?
+                state->cfg->program_regs(state->mxl_state, ctrl_reg_info) :
+                -EINVAL;
+}
+
+/* ------------------------------------------------------------------------ */
+/* TPS */
+
+static
+int mxl1x1sf_demod_get_tps_code_rate(struct mxl111sf_demod_state *state,
+                                     fe_code_rate_t *code_rate)
+{
+        u8 val;
+        int ret = mxl111sf_demod_read_reg(state, V6_CODE_RATE_TPS_REG, &val);
+        /* bit<2:0> - 000:1/2, 001:2/3, 010:3/4, 011:5/6, 100:7/8 */
+        if (mxl_fail(ret))
+                goto fail;
+
+        switch (val & V6_CODE_RATE_TPS_MASK) {
+        case 0:
+                *code_rate = FEC_1_2;
+                break;
+        case 1:
+                *code_rate = FEC_2_3;
+                break;
+        case 2:
+                *code_rate = FEC_3_4;
+                break;
+        case 3:
+                *code_rate = FEC_5_6;
+                break;
+        case 4:
+                *code_rate = FEC_7_8;
+                break;
+        }
+fail:
+        return ret;
+}
+
+static
+int mxl1x1sf_demod_get_tps_constellation(struct mxl111sf_demod_state *state,
+                                         fe_modulation_t *constellation)
+{
+        u8 val;
+        int ret = mxl111sf_demod_read_reg(state, V6_MODORDER_TPS_REG, &val);
+        /* Constellation, 00 : QPSK, 01 : 16QAM, 10:64QAM */
+        if (mxl_fail(ret))
+                goto fail;
+
+        switch ((val & V6_PARAM_CONSTELLATION_MASK) >> 4) {
+        case 0:
+                *constellation = QPSK;
+                break;
+        case 1:
+                *constellation = QAM_16;
+                break;
+        case 2:
+                *constellation = QAM_64;
+                break;
+        }
+fail:
+        return ret;
+}
+
+static
+int mxl1x1sf_demod_get_tps_guard_fft_mode(struct mxl111sf_demod_state *state,
+                                          fe_transmit_mode_t *fft_mode)
+{
+        u8 val;
+        int ret = mxl111sf_demod_read_reg(state, V6_MODE_TPS_REG, &val);
+        /* FFT Mode, 00:2K, 01:8K, 10:4K */
+        if (mxl_fail(ret))
+                goto fail;
+
+        switch ((val & V6_PARAM_FFT_MODE_MASK) >> 2) {
+        case 0:
+                *fft_mode = TRANSMISSION_MODE_2K;
+                break;
+        case 1:
+                *fft_mode = TRANSMISSION_MODE_8K;
+                break;
+        case 2:
+                *fft_mode = TRANSMISSION_MODE_4K;
+                break;
+        }
+fail:
+        return ret;
+}
+
+static
+int mxl1x1sf_demod_get_tps_guard_interval(struct mxl111sf_demod_state *state,
+                                          fe_guard_interval_t *guard)
+{
+        u8 val;
+        int ret = mxl111sf_demod_read_reg(state, V6_CP_TPS_REG, &val);
+        /* 00:1/32, 01:1/16, 10:1/8, 11:1/4 */
+        if (mxl_fail(ret))
+                goto fail;
+
+        switch ((val & V6_PARAM_GI_MASK) >> 4) {
+        case 0:
+                *guard = GUARD_INTERVAL_1_32;
+                break;
+        case 1:
+                *guard = GUARD_INTERVAL_1_16;
+                break;
+        case 2:
+                *guard = GUARD_INTERVAL_1_8;
+                break;
+        case 3:
+                *guard = GUARD_INTERVAL_1_4;
+                break;
+        }
+fail:
+        return ret;
+}
+
+static
+int mxl1x1sf_demod_get_tps_hierarchy(struct mxl111sf_demod_state *state,
+                                     fe_hierarchy_t *hierarchy)
+{
+        u8 val;
+        int ret = mxl111sf_demod_read_reg(state, V6_TPS_HIERACHY_REG, &val);
+        /* bit<6:4> - 000:Non hierarchy, 001:1, 010:2, 011:4 */
+        if (mxl_fail(ret))
+                goto fail;
+
+        switch ((val & V6_TPS_HIERARCHY_INFO_MASK) >> 6) {
+        case 0:
+                *hierarchy = HIERARCHY_NONE;
+                break;
+        case 1:
+                *hierarchy = HIERARCHY_1;
+                break;
+        case 2:
+                *hierarchy = HIERARCHY_2;
+                break;
+        case 3:
+                *hierarchy = HIERARCHY_4;
+                break;
+        }
+fail:
+        return ret;
+}
+
+/* ------------------------------------------------------------------------ */
+/* LOCKS */
+
+static
+int mxl1x1sf_demod_get_sync_lock_status(struct mxl111sf_demod_state *state,
+                                        int *sync_lock)
+{
+        u8 val = 0;
+        int ret = mxl111sf_demod_read_reg(state, V6_SYNC_LOCK_REG, &val);
+        if (mxl_fail(ret))
+                goto fail;
+        *sync_lock = (val & SYNC_LOCK_MASK) >> 4;
+fail:
+        return ret;
+}
+
+static
+int mxl1x1sf_demod_get_rs_lock_status(struct mxl111sf_demod_state *state,
+                                      int *rs_lock)
+{
+        u8 val = 0;
+        int ret = mxl111sf_demod_read_reg(state, V6_RS_LOCK_DET_REG, &val);
+        if (mxl_fail(ret))
+                goto fail;
+        *rs_lock = (val & RS_LOCK_DET_MASK) >> 3;
+fail:
+        return ret;
+}
+
+static
+int mxl1x1sf_demod_get_tps_lock_status(struct mxl111sf_demod_state *state,
+                                       int *tps_lock)
+{
+        u8 val = 0;
+        int ret = mxl111sf_demod_read_reg(state, V6_TPS_LOCK_REG, &val);
+        if (mxl_fail(ret))
+                goto fail;
+        *tps_lock = (val & V6_PARAM_TPS_LOCK_MASK) >> 6;
+fail:
+        return ret;
+}
+
+static
+int mxl1x1sf_demod_get_fec_lock_status(struct mxl111sf_demod_state *state,
+                                       int *fec_lock)
+{
+        u8 val = 0;
+        int ret = mxl111sf_demod_read_reg(state, V6_IRQ_STATUS_REG, &val);
+        if (mxl_fail(ret))
+                goto fail;
+        *fec_lock = (val & IRQ_MASK_FEC_LOCK) >> 4;
+fail:
+        return ret;
+}
+
+#if 0
+static
+int mxl1x1sf_demod_get_cp_lock_status(struct mxl111sf_demod_state *state,
+                                      int *cp_lock)
+{
+        u8 val = 0;
+        int ret = mxl111sf_demod_read_reg(state, V6_CP_LOCK_DET_REG, &val);
+        if (mxl_fail(ret))
+                goto fail;
+        *cp_lock = (val & V6_CP_LOCK_DET_MASK) >> 2;
+fail:
+        return ret;
+}
+#endif
+
+static int mxl1x1sf_demod_reset_irq_status(struct mxl111sf_demod_state *state)
+{
+        return mxl111sf_demod_write_reg(state, 0x0e, 0xff);
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int mxl111sf_demod_set_frontend(struct dvb_frontend *fe,
+                                       struct dvb_frontend_parameters *param)
+{
+        struct mxl111sf_demod_state *state = fe->demodulator_priv;
+        int ret = 0;
+
+        struct mxl111sf_reg_ctrl_info phy_pll_patch[] = {
+                {0x00, 0xff, 0x01}, /* change page to 1 */
+                {0x40, 0xff, 0x05},
+                {0x40, 0xff, 0x01},
+                {0x41, 0xff, 0xca},
+                {0x41, 0xff, 0xc0},
+                {0x00, 0xff, 0x00}, /* change page to 0 */
+                {0,    0,    0}
+        };
+
+        mxl_dbg("()");
+
+        if (fe->ops.tuner_ops.set_params) {
+                ret = fe->ops.tuner_ops.set_params(fe, param);
+                if (mxl_fail(ret))
+                        goto fail;
+                msleep(50);
+        }
+        ret = mxl111sf_demod_program_regs(state, phy_pll_patch);
+        mxl_fail(ret);
+        msleep(50);
+        ret = mxl1x1sf_demod_reset_irq_status(state);
+        mxl_fail(ret);
+        msleep(100);
+fail:
+        return ret;
+}
+
+/* ------------------------------------------------------------------------ */
+
+#if 0
+/* resets TS Packet error count */
+/* After setting 7th bit of V5_PER_COUNT_RESET_REG, it should be reset to 0. */
+static
+int mxl1x1sf_demod_reset_packet_error_count(struct mxl111sf_demod_state *state)
+{
+        struct mxl111sf_reg_ctrl_info reset_per_count[] = {
+                {0x20, 0x01, 0x01},
+                {0x20, 0x01, 0x00},
+                {0,    0,    0}
+        };
+        return mxl111sf_demod_program_regs(state, reset_per_count);
+}
+#endif
+
+/* returns TS Packet error count */
+/* PER Count = FEC_PER_COUNT * (2 ** (FEC_PER_SCALE * 4)) */
+static int mxl111sf_demod_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
+{
+        struct mxl111sf_demod_state *state = fe->demodulator_priv;
+        u32 fec_per_count, fec_per_scale;
+        u8 val;
+        int ret;
+
+        *ucblocks = 0;
+
+        /* FEC_PER_COUNT Register */
+        ret = mxl111sf_demod_read_reg(state, V6_FEC_PER_COUNT_REG, &val);
+        if (mxl_fail(ret))
+                goto fail;
+
+        fec_per_count = val;
+
+        /* FEC_PER_SCALE Register */
+        ret = mxl111sf_demod_read_reg(state, V6_FEC_PER_SCALE_REG, &val);
+        if (mxl_fail(ret))
+                goto fail;
+
+        val &= V6_FEC_PER_SCALE_MASK;
+        val *= 4;
+
+        fec_per_scale = 1 << val;
+
+        fec_per_count *= fec_per_scale;
+
+        *ucblocks = fec_per_count;
+fail:
+        return ret;
+}
+
+#ifdef MXL111SF_DEMOD_ENABLE_CALCULATIONS
+/* FIXME: leaving this enabled breaks the build on some architectures,
+ * and we shouldn't have any floating point math in the kernel, anyway.
+ *
+ * These macros need to be re-written, but it's harmless to simply
+ * return zero for now. */
+#define CALCULATE_BER(avg_errors, count) \
+        ((u32)(avg_errors * 4)/(count*64*188*8))
+#define CALCULATE_SNR(data) \
+        ((u32)((10 * (u32)data / 64) - 2.5))
+#else
+#define CALCULATE_BER(avg_errors, count) 0
+#define CALCULATE_SNR(data) 0
+#endif
+
+static int mxl111sf_demod_read_ber(struct dvb_frontend *fe, u32 *ber)
+{
+        struct mxl111sf_demod_state *state = fe->demodulator_priv;
+        u8 val1, val2, val3;
+        int ret;
+
+        *ber = 0;
+
+        ret = mxl111sf_demod_read_reg(state, V6_RS_AVG_ERRORS_LSB_REG, &val1);
+        if (mxl_fail(ret))
+                goto fail;
+        ret = mxl111sf_demod_read_reg(state, V6_RS_AVG_ERRORS_MSB_REG, &val2);
+        if (mxl_fail(ret))
+                goto fail;
+        ret = mxl111sf_demod_read_reg(state, V6_N_ACCUMULATE_REG, &val3);
+        if (mxl_fail(ret))
+                goto fail;
+
+        *ber = CALCULATE_BER((val1 | (val2 << 8)), val3);
+fail:
+        return ret;
+}
+
+static int mxl111sf_demod_calc_snr(struct mxl111sf_demod_state *state,
+                                   u16 *snr)
+{
+        u8 val1, val2;
+        int ret;
+
+        *snr = 0;
+
+        ret = mxl111sf_demod_read_reg(state, V6_SNR_RB_LSB_REG, &val1);
+        if (mxl_fail(ret))
+                goto fail;
+        ret = mxl111sf_demod_read_reg(state, V6_SNR_RB_MSB_REG, &val2);
+        if (mxl_fail(ret))
+                goto fail;
+
+        *snr = CALCULATE_SNR(val1 | ((val2 & 0x03) << 8));
+fail:
+        return ret;
+}
+
+static int mxl111sf_demod_read_snr(struct dvb_frontend *fe, u16 *snr)
+{
+        struct mxl111sf_demod_state *state = fe->demodulator_priv;
+
+        int ret = mxl111sf_demod_calc_snr(state, snr);
+        if (mxl_fail(ret))
+                goto fail;
+
+        *snr /= 10; /* 0.1 dB */
+fail:
+        return ret;
+}
+
+static int mxl111sf_demod_read_status(struct dvb_frontend *fe,
+                                      fe_status_t *status)
+{
+        struct mxl111sf_demod_state *state = fe->demodulator_priv;
+        int ret, locked, cr_lock, sync_lock, fec_lock;
+
+        *status = 0;
+
+        ret = mxl1x1sf_demod_get_rs_lock_status(state, &locked);
+        if (mxl_fail(ret))
+                goto fail;
+        ret = mxl1x1sf_demod_get_tps_lock_status(state, &cr_lock);
+        if (mxl_fail(ret))
+                goto fail;
+        ret = mxl1x1sf_demod_get_sync_lock_status(state, &sync_lock);
+        if (mxl_fail(ret))
+                goto fail;
+        ret = mxl1x1sf_demod_get_fec_lock_status(state, &fec_lock);
+        if (mxl_fail(ret))
+                goto fail;
+
+        if (locked)
+                *status |= FE_HAS_SIGNAL;
+        if (cr_lock)
+                *status |= FE_HAS_CARRIER;
+        if (sync_lock)
+                *status |= FE_HAS_SYNC;
+        if (fec_lock) /* false positives? */
+                *status |= FE_HAS_VITERBI;
+
+        if ((locked) && (cr_lock) && (sync_lock))
+                *status |= FE_HAS_LOCK;
+fail:
+        return ret;
+}
+
+static int mxl111sf_demod_read_signal_strength(struct dvb_frontend *fe,
+                                               u16 *signal_strength)
+{
+        struct mxl111sf_demod_state *state = fe->demodulator_priv;
+        fe_modulation_t constellation;
+        u16 snr;
+
+        mxl111sf_demod_calc_snr(state, &snr);
+        mxl1x1sf_demod_get_tps_constellation(state, &constellation);
+
+        switch (constellation) {
+        case QPSK:
+                *signal_strength = (snr >= 1300) ?
+                        min(65535, snr * 44) : snr * 38;
+                break;
+        case QAM_16:
+                *signal_strength = (snr >= 1500) ?
+                        min(65535, snr * 38) : snr * 33;
+                break;
+        case QAM_64:
+                *signal_strength = (snr >= 2000) ?
+                        min(65535, snr * 29) : snr * 25;
+                break;
+        default:
+                *signal_strength = 0;
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+static int mxl111sf_demod_get_frontend(struct dvb_frontend *fe,
+                                       struct dvb_frontend_parameters *p)
+{
+        struct mxl111sf_demod_state *state = fe->demodulator_priv;
+
+        mxl_dbg("()");
+#if 0
+        p->inversion = /* FIXME */ ? INVERSION_ON : INVERSION_OFF;
+#endif
+        if (fe->ops.tuner_ops.get_bandwidth)
+                fe->ops.tuner_ops.get_bandwidth(fe, &p->u.ofdm.bandwidth);
+        if (fe->ops.tuner_ops.get_frequency)
+                fe->ops.tuner_ops.get_frequency(fe, &p->frequency);
+        mxl1x1sf_demod_get_tps_code_rate(state, &p->u.ofdm.code_rate_HP);
+        mxl1x1sf_demod_get_tps_code_rate(state, &p->u.ofdm.code_rate_LP);
+        mxl1x1sf_demod_get_tps_constellation(state, &p->u.ofdm.constellation);
+        mxl1x1sf_demod_get_tps_guard_fft_mode(state,
+                                              &p->u.ofdm.transmission_mode);
+        mxl1x1sf_demod_get_tps_guard_interval(state,
+                                              &p->u.ofdm.guard_interval);
+        mxl1x1sf_demod_get_tps_hierarchy(state,
+                                         &p->u.ofdm.hierarchy_information);
+
+        return 0;
+}
+
+static
+int mxl111sf_demod_get_tune_settings(struct dvb_frontend *fe,
+                                     struct dvb_frontend_tune_settings *tune)
+{
+        tune->min_delay_ms = 1000;
+        return 0;
+}
+
+static void mxl111sf_demod_release(struct dvb_frontend *fe)
+{
+        struct mxl111sf_demod_state *state = fe->demodulator_priv;
+        mxl_dbg("()");
+        kfree(state);
+        fe->demodulator_priv = NULL;
+}
+
+static struct dvb_frontend_ops mxl111sf_demod_ops = {
+
+        .info = {
+                .name               = "MaxLinear MxL111SF DVB-T demodulator",
+                .type               = FE_OFDM,
+                .frequency_min      = 177000000,
+                .frequency_max      = 858000000,
+                .frequency_stepsize = 166666,
+                .caps = FE_CAN_FEC_1_2 | FE_CAN_FEC_2_3 | FE_CAN_FEC_3_4 |
+                        FE_CAN_FEC_5_6 | FE_CAN_FEC_7_8 | FE_CAN_FEC_AUTO |
+                        FE_CAN_QPSK | FE_CAN_QAM_16 | FE_CAN_QAM_64 |
+                        FE_CAN_QAM_AUTO |
+                        FE_CAN_HIERARCHY_AUTO | FE_CAN_GUARD_INTERVAL_AUTO |
+                        FE_CAN_TRANSMISSION_MODE_AUTO | FE_CAN_RECOVER
+        },
+        .release              = mxl111sf_demod_release,
+#if 0
+        .init                 = mxl111sf_init,
+        .i2c_gate_ctrl        = mxl111sf_i2c_gate_ctrl,
+#endif
+        .set_frontend         = mxl111sf_demod_set_frontend,
+        .get_frontend         = mxl111sf_demod_get_frontend,
+        .get_tune_settings    = mxl111sf_demod_get_tune_settings,
+        .read_status          = mxl111sf_demod_read_status,
+        .read_signal_strength = mxl111sf_demod_read_signal_strength,
+        .read_ber             = mxl111sf_demod_read_ber,
+        .read_snr             = mxl111sf_demod_read_snr,
+        .read_ucblocks        = mxl111sf_demod_read_ucblocks,
+};
+
+struct dvb_frontend *mxl111sf_demod_attach(struct mxl111sf_state *mxl_state,
+                                           struct mxl111sf_demod_config *cfg)
+{
+        struct mxl111sf_demod_state *state = NULL;
+
+        mxl_dbg("()");
+
+        state = kzalloc(sizeof(struct mxl111sf_demod_state), GFP_KERNEL);
+        if (state == NULL)
+                return NULL;
+
+        state->mxl_state = mxl_state;
+        state->cfg = cfg;
+
+        memcpy(&state->fe.ops, &mxl111sf_demod_ops,
+               sizeof(struct dvb_frontend_ops));
+
+        state->fe.demodulator_priv = state;
+        return &state->fe;
+}
+EXPORT_SYMBOL_GPL(mxl111sf_demod_attach);
+
+MODULE_DESCRIPTION("MaxLinear MxL111SF DVB-T demodulator driver");
+MODULE_AUTHOR("Michael Krufky <mkrufky@kernellabs.com>");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("0.1");
+
+/*
+ * Local variables:
+ * c-basic-offset: 8
+ * End:
+ */
diff --git a/drivers/media/dvb/dvb-usb/mxl111sf-demod.h b/drivers/media/dvb/dvb-usb/mxl111sf-demod.h
new file mode 100644
index 000000000000..432706ae5274
--- /dev/null
+++ b/drivers/media/dvb/dvb-usb/mxl111sf-demod.h
@@ -0,0 +1,55 @@
+/*
+ *  mxl111sf-demod.h - driver for the MaxLinear MXL111SF DVB-T demodulator
+ *
+ *  Copyright (C) 2010 Michael Krufky <mkrufky@kernellabs.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __MXL111SF_DEMOD_H__
+#define __MXL111SF_DEMOD_H__
+
+#include "dvb_frontend.h"
+#include "mxl111sf.h"
+
+struct mxl111sf_demod_config {
+        int (*read_reg)(struct mxl111sf_state *state, u8 addr, u8 *data);
+        int (*write_reg)(struct mxl111sf_state *state, u8 addr, u8 data);
+        int (*program_regs)(struct mxl111sf_state *state,
+                            struct mxl111sf_reg_ctrl_info *ctrl_reg_info);
+};
+
+#if defined(CONFIG_DVB_USB_MXL111SF) || \
+        (defined(CONFIG_DVB_USB_MXL111SF_MODULE) && defined(MODULE))
+extern
+struct dvb_frontend *mxl111sf_demod_attach(struct mxl111sf_state *mxl_state,
+                                           struct mxl111sf_demod_config *cfg);
+#else
+static inline
+struct dvb_frontend *mxl111sf_demod_attach(struct mxl111sf_state *mxl_state,
+                                           struct mxl111sf_demod_config *cfg)
+{
+        printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+        return NULL;
+}
+#endif /* CONFIG_DVB_USB_MXL111SF */
+
+#endif /* __MXL111SF_DEMOD_H__ */
+
+/*
+ * Local variables:
+ * c-basic-offset: 8
+ * End:
+ */
diff --git a/drivers/media/dvb/dvb-usb/mxl111sf.c b/drivers/media/dvb/dvb-usb/mxl111sf.c
index 546ba5915a5b..b5c98da5d9e2 100644
--- a/drivers/media/dvb/dvb-usb/mxl111sf.c
+++ b/drivers/media/dvb/dvb-usb/mxl111sf.c
@@ -17,6 +17,7 @@
 #include "mxl111sf-i2c.h"
 #include "mxl111sf-gpio.h"
 
+#include "mxl111sf-demod.h"
 #include "mxl111sf-tuner.h"
 
 #include "lgdt3305.h"
@@ -362,6 +363,22 @@ static int mxl111sf_ep6_streaming_ctrl(struct dvb_usb_adapter *adap, int onoff)
         return ret;
 }
 
+static int mxl111sf_ep4_streaming_ctrl(struct dvb_usb_adapter *adap, int onoff)
+{
+        struct dvb_usb_device *d = adap->dev;
+        struct mxl111sf_state *state = d->priv;
+        int ret = 0;
+
+        deb_info("%s(%d)\n", __func__, onoff);
+
+        if (onoff) {
+                ret = mxl111sf_enable_usb_output(state);
+                mxl_fail(ret);
+        }
+
+        return ret;
+}
+
 /* ------------------------------------------------------------------------ */
 
 static struct lgdt3305_config hauppauge_lgdt3305_config = {
@@ -438,6 +455,70 @@ fail:
         return ret;
 }
 
+static struct mxl111sf_demod_config mxl_demod_config = {
+        .read_reg        = mxl111sf_read_reg,
+        .write_reg       = mxl111sf_write_reg,
+        .program_regs    = mxl111sf_ctrl_program_regs,
+};
+
+static int mxl111sf_attach_demod(struct dvb_usb_adapter *adap)
+{
+        struct dvb_usb_device *d = adap->dev;
+        struct mxl111sf_state *state = d->priv;
+        int fe_id = adap->num_frontends_initialized;
+        struct mxl111sf_adap_state *adap_state = adap->fe_adap[fe_id].priv;
+        int ret;
+
+        deb_adv("%s()\n", __func__);
+
+        /* save a pointer to the dvb_usb_device in device state */
+        state->d = d;
+        adap_state->alt_mode = (dvb_usb_mxl111sf_isoc) ? 1 : 2;
+        state->alt_mode = adap_state->alt_mode;
+
+        if (usb_set_interface(adap->dev->udev, 0, state->alt_mode) < 0)
+                err("set interface failed");
+
+        state->gpio_mode = MXL111SF_GPIO_MOD_DVBT;
+        adap_state->gpio_mode = state->gpio_mode;
+        adap_state->device_mode = MXL_SOC_MODE;
+        adap_state->ep6_clockphase = 1;
+
+        ret = mxl1x1sf_soft_reset(state);
+        if (mxl_fail(ret))
+                goto fail;
+        ret = mxl111sf_init_tuner_demod(state);
+        if (mxl_fail(ret))
+                goto fail;
+
+        ret = mxl1x1sf_set_device_mode(state, adap_state->device_mode);
+        if (mxl_fail(ret))
+                goto fail;
+
+        ret = mxl111sf_enable_usb_output(state);
+        if (mxl_fail(ret))
+                goto fail;
+        ret = mxl1x1sf_top_master_ctrl(state, 1);
+        if (mxl_fail(ret))
+                goto fail;
+
+        /* dont care if this fails */
+        mxl111sf_init_port_expander(state);
+
+        adap->fe_adap[fe_id].fe = dvb_attach(mxl111sf_demod_attach, state,
+                              &mxl_demod_config);
+        if (adap->fe_adap[fe_id].fe) {
+                adap_state->fe_init = adap->fe_adap[fe_id].fe->ops.init;
+                adap->fe_adap[fe_id].fe->ops.init = mxl111sf_adap_fe_init;
+                adap_state->fe_sleep = adap->fe_adap[fe_id].fe->ops.sleep;
+                adap->fe_adap[fe_id].fe->ops.sleep = mxl111sf_adap_fe_sleep;
+                return 0;
+        }
+        ret = -EIO;
+fail:
+        return ret;
+}
+
 static inline int mxl111sf_set_ant_path(struct mxl111sf_state *state,
                                         int antpath)
 {
@@ -567,7 +648,8 @@ struct i2c_algorithm mxl111sf_i2c_algo = {
 #endif
 };
 
-/* DVB USB Driver stuff */
+static struct dvb_usb_device_properties mxl111sf_dvbt_bulk_properties;
+static struct dvb_usb_device_properties mxl111sf_dvbt_isoc_properties;
 static struct dvb_usb_device_properties mxl111sf_atsc_bulk_properties;
 static struct dvb_usb_device_properties mxl111sf_atsc_isoc_properties;
 
@@ -580,9 +662,15 @@ static int mxl111sf_probe(struct usb_interface *intf,
 
         if (((dvb_usb_mxl111sf_isoc) &&
              (0 == dvb_usb_device_init(intf,
+                                       &mxl111sf_dvbt_isoc_properties,
+                                       THIS_MODULE, &d, adapter_nr) ||
+              0 == dvb_usb_device_init(intf,
                                        &mxl111sf_atsc_isoc_properties,
                                        THIS_MODULE, &d, adapter_nr))) ||
             0 == dvb_usb_device_init(intf,
+                                     &mxl111sf_dvbt_bulk_properties,
+                                     THIS_MODULE, &d, adapter_nr) ||
+            0 == dvb_usb_device_init(intf,
                                      &mxl111sf_atsc_bulk_properties,
                                      THIS_MODULE, &d, adapter_nr) || 0) {
 
@@ -669,6 +757,36 @@ static struct usb_device_id mxl111sf_table[] = {
 MODULE_DEVICE_TABLE(usb, mxl111sf_table);
 
 
+#define MXL111SF_EP4_BULK_STREAMING_CONFIG              \
+        .streaming_ctrl = mxl111sf_ep4_streaming_ctrl,  \
+        .stream = {                                     \
+                .type = USB_BULK,                       \
+                .count = 5,                             \
+                .endpoint = 0x04,                       \
+                .u = {                                  \
+                        .bulk = {                       \
+                                .buffersize = 8192,     \
+                        }                               \
+                }                                       \
+        }
+
+/* FIXME: works for v6 but not v8 silicon */
+#define MXL111SF_EP4_ISOC_STREAMING_CONFIG              \
+        .streaming_ctrl = mxl111sf_ep4_streaming_ctrl,  \
+        .stream = {                                     \
+                .type = USB_ISOC,                       \
+                .count = 5,                             \
+                .endpoint = 0x04,                       \
+                .u = {                                  \
+                        .isoc = {                       \
+                                .framesperurb = 96,     \
+                                /* FIXME: v6 SILICON: */        \
+                                .framesize = 564,       \
+                                .interval = 1,          \
+                        }                               \
+                }                                       \
+        }
+
 #define MXL111SF_EP6_BULK_STREAMING_CONFIG              \
         .streaming_ctrl = mxl111sf_ep6_streaming_ctrl,  \
         .stream = {                                     \
@@ -712,7 +830,7 @@ MODULE_DEVICE_TABLE(usb, mxl111sf_table);
         .generic_bulk_ctrl_endpoint_response = MXL_EP1_REG_READ, \
         .size_of_priv     = sizeof(struct mxl111sf_state)
 
-static struct dvb_usb_device_properties mxl111sf_atsc_bulk_properties = {
+static struct dvb_usb_device_properties mxl111sf_dvbt_bulk_properties = {
         MXL111SF_DEFAULT_DEVICE_PROPERTIES,
 
         .num_adapters = 1,
@@ -723,10 +841,106 @@ static struct dvb_usb_device_properties mxl111sf_atsc_bulk_properties = {
                 .fe = {{
                         .size_of_priv     = sizeof(struct mxl111sf_adap_state),
 
+                        .frontend_attach  = mxl111sf_attach_demod,
+                        .tuner_attach     = mxl111sf_attach_tuner,
+
+                        MXL111SF_EP4_BULK_STREAMING_CONFIG,
+                } },
+                },
+        },
+        .num_device_descs = 4,
+        .devices = {
+                {   "Hauppauge 126xxx DVBT (bulk)",
+                        { NULL },
+                        { &mxl111sf_table[4], &mxl111sf_table[8],
+                          NULL },
+                },
+                {   "Hauppauge 117xxx DVBT (bulk)",
+                        { NULL },
+                        { &mxl111sf_table[15], &mxl111sf_table[18],
+                          NULL },
+                },
+                {   "Hauppauge 138xxx DVBT (bulk)",
+                        { NULL },
+                        { &mxl111sf_table[20], &mxl111sf_table[22],
+                          &mxl111sf_table[24], &mxl111sf_table[26],
+                          NULL },
+                },
+                {   "Hauppauge 126xxx (tp-bulk)",
+                        { NULL },
+                        { &mxl111sf_table[28], &mxl111sf_table[30],
+                          NULL },
+                },
+        }
+};
+
+static struct dvb_usb_device_properties mxl111sf_dvbt_isoc_properties = {
+        MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+        .num_adapters = 1,
+        .adapter = {
+                {
+                .fe_ioctl_override = mxl111sf_fe_ioctl_override,
+                .num_frontends = 1,
+                .fe = {{
+                        .size_of_priv     = sizeof(struct mxl111sf_adap_state),
+
+                        .frontend_attach  = mxl111sf_attach_demod,
+                        .tuner_attach     = mxl111sf_attach_tuner,
+
+                        MXL111SF_EP4_ISOC_STREAMING_CONFIG,
+                } },
+                },
+        },
+        .num_device_descs = 4,
+        .devices = {
+                {   "Hauppauge 126xxx DVBT (isoc)",
+                        { NULL },
+                        { &mxl111sf_table[4], &mxl111sf_table[8],
+                          NULL },
+                },
+                {   "Hauppauge 117xxx DVBT (isoc)",
+                        { NULL },
+                        { &mxl111sf_table[15], &mxl111sf_table[18],
+                          NULL },
+                },
+                {   "Hauppauge 138xxx DVBT (isoc)",
+                        { NULL },
+                        { &mxl111sf_table[20], &mxl111sf_table[22],
+                          &mxl111sf_table[24], &mxl111sf_table[26],
+                          NULL },
+                },
+                {   "Hauppauge 126xxx (tp-isoc)",
+                        { NULL },
+                        { &mxl111sf_table[28], &mxl111sf_table[30],
+                          NULL },
+                },
+        }
+};
+
+static struct dvb_usb_device_properties mxl111sf_atsc_bulk_properties = {
+        MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+        .num_adapters = 1,
+        .adapter = {
+                {
+                .fe_ioctl_override = mxl111sf_fe_ioctl_override,
+                .num_frontends = 2,
+                .fe = {{
+                        .size_of_priv     = sizeof(struct mxl111sf_adap_state),
+
                         .frontend_attach  = mxl111sf_lgdt3305_frontend_attach,
                         .tuner_attach     = mxl111sf_attach_tuner,
 
                         MXL111SF_EP6_BULK_STREAMING_CONFIG,
+                },
+                {
+                        .size_of_priv     = sizeof(struct mxl111sf_adap_state),
+
+                        .frontend_attach  = mxl111sf_attach_demod,
+                        .tuner_attach     = mxl111sf_attach_tuner,
+
+                        MXL111SF_EP4_BULK_STREAMING_CONFIG,
                 }},
                 },
         },
@@ -776,7 +990,7 @@ static struct dvb_usb_device_properties mxl111sf_atsc_isoc_properties = {
         .adapter = {
                 {
                 .fe_ioctl_override = mxl111sf_fe_ioctl_override,
-                .num_frontends = 1,
+                .num_frontends = 2,
                 .fe = {{
                         .size_of_priv     = sizeof(struct mxl111sf_adap_state),
 
@@ -784,6 +998,14 @@ static struct dvb_usb_device_properties mxl111sf_atsc_isoc_properties = {
                         .tuner_attach     = mxl111sf_attach_tuner,
 
                         MXL111SF_EP6_ISOC_STREAMING_CONFIG,
+                },
+                {
+                        .size_of_priv     = sizeof(struct mxl111sf_adap_state),
+
+                        .frontend_attach  = mxl111sf_attach_demod,
+                        .tuner_attach     = mxl111sf_attach_tuner,
+
+                        MXL111SF_EP4_ISOC_STREAMING_CONFIG,
                 }},
                 },
         },
diff --git a/drivers/media/dvb/dvb-usb/mxl111sf.h b/drivers/media/dvb/dvb-usb/mxl111sf.h
index 5a2c7bb386cd..364d89f826bd 100644
--- a/drivers/media/dvb/dvb-usb/mxl111sf.h
+++ b/drivers/media/dvb/dvb-usb/mxl111sf.h
@@ -133,7 +133,7 @@ extern int dvb_usb_mxl111sf_debug;
 /* The following allows the mxl_fail() macro defined below to work
  * in externel modules, such as mxl111sf-tuner.ko, even though
  * dvb_usb_mxl111sf_debug is not defined within those modules */
-#ifdef __MXL111SF_TUNER_H__
+#if (defined(__MXL111SF_TUNER_H__)) || (defined(__MXL111SF_DEMOD_H__))
 #define MXL_ADV_DEBUG_ENABLED MXL_ADV_DBG
 #else
 #define MXL_ADV_DEBUG_ENABLED dvb_usb_mxl111sf_debug
diff --git a/drivers/media/dvb/ngene/Makefile b/drivers/media/dvb/ngene/Makefile
index 89873615e683..13ebeffb705f 100644
--- a/drivers/media/dvb/ngene/Makefile
+++ b/drivers/media/dvb/ngene/Makefile
@@ -11,4 +11,4 @@ ccflags-y += -Idrivers/media/dvb/frontends/
 ccflags-y += -Idrivers/media/common/tuners/
 
 # For the staging CI driver cxd2099
-ccflags-y += -Idrivers/staging/cxd2099/
+ccflags-y += -Idrivers/staging/media/cxd2099/
diff --git a/drivers/media/radio/radio-tea5764.c b/drivers/media/radio/radio-tea5764.c
index 95ddcc4845d3..db20904d01f0 100644
--- a/drivers/media/radio/radio-tea5764.c
+++ b/drivers/media/radio/radio-tea5764.c
@@ -128,8 +128,10 @@ struct tea5764_write_regs {
         u16 rdsbbl;                             /* PAUSEDET & RDSBBL */
 } __attribute__ ((packed));
 
-#ifndef RADIO_TEA5764_XTAL
+#ifdef CONFIG_RADIO_TEA5764_XTAL
 #define RADIO_TEA5764_XTAL 1
+#else
+#define RADIO_TEA5764_XTAL 0
 #endif
 
 static int radio_nr = -1;
diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
index d285c8c92819..b303a3f8a9f8 100644
--- a/drivers/media/video/Kconfig
+++ b/drivers/media/video/Kconfig
@@ -517,6 +517,13 @@ config VIDEO_NOON010PC30
 
 source "drivers/media/video/m5mols/Kconfig"
 
+config VIDEO_S5K6AA
+        tristate "Samsung S5K6AAFX sensor support"
+        depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+        ---help---
+          This is a V4L2 sensor-level driver for Samsung S5K6AA(FX) 1.3M
+          camera sensor with an embedded SoC image signal processor.
+
 comment "Flash devices"
 
 config VIDEO_ADP1653
@@ -736,6 +743,8 @@ source "drivers/media/video/cx88/Kconfig"
 
 source "drivers/media/video/cx23885/Kconfig"
 
+source "drivers/media/video/cx25821/Kconfig"
+
 source "drivers/media/video/au0828/Kconfig"
 
 source "drivers/media/video/ivtv/Kconfig"
diff --git a/drivers/media/video/Makefile b/drivers/media/video/Makefile
index 11fff97e7196..117f9c4b4cb9 100644
--- a/drivers/media/video/Makefile
+++ b/drivers/media/video/Makefile
@@ -72,6 +72,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_S5K6AA)      += s5k6aa.o
 obj-$(CONFIG_VIDEO_ADP1653)     += adp1653.o
 
 obj-$(CONFIG_SOC_CAMERA_IMX074)         += imx074.o
@@ -104,6 +105,7 @@ obj-$(CONFIG_VIDEO_CX88) += cx88/
 obj-$(CONFIG_VIDEO_EM28XX) += em28xx/
 obj-$(CONFIG_VIDEO_TLG2300) += tlg2300/
 obj-$(CONFIG_VIDEO_CX231XX) += cx231xx/
+obj-$(CONFIG_VIDEO_CX25821) += cx25821/
 obj-$(CONFIG_VIDEO_USBVISION) += usbvision/
 obj-$(CONFIG_VIDEO_PVRUSB2) += pvrusb2/
 obj-$(CONFIG_VIDEO_CPIA2) += cpia2/
diff --git a/drivers/media/video/atmel-isi.c b/drivers/media/video/atmel-isi.c
index 774715d2f84f..8c775c59e120 100644
--- a/drivers/media/video/atmel-isi.c
+++ b/drivers/media/video/atmel-isi.c
@@ -94,6 +94,7 @@ struct atmel_isi {
         unsigned int                    irq;
 
         struct isi_platform_data        *pdata;
+        u16                             width_flags;    /* max 12 bits */
 
         struct list_head                video_buffer_list;
         struct frame_buffer             *active;
@@ -248,9 +249,9 @@ static int atmel_isi_wait_status(struct atmel_isi *isi, int wait_reset)
 /* ------------------------------------------------------------------
         Videobuf operations
    ------------------------------------------------------------------*/
-static int queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
-                                unsigned int *nplanes, unsigned int sizes[],
-                                void *alloc_ctxs[])
+static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
+                                unsigned int *nbuffers, unsigned int *nplanes,
+                                unsigned int 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);
@@ -647,50 +648,42 @@ static bool isi_camera_packing_supported(const struct soc_mbus_pixelfmt *fmt)
                  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;
-}
+#define ISI_BUS_PARAM (V4L2_MBUS_MASTER |       \
+                V4L2_MBUS_HSYNC_ACTIVE_HIGH |   \
+                V4L2_MBUS_HSYNC_ACTIVE_LOW |    \
+                V4L2_MBUS_VSYNC_ACTIVE_HIGH |   \
+                V4L2_MBUS_VSYNC_ACTIVE_LOW |    \
+                V4L2_MBUS_PCLK_SAMPLE_RISING |  \
+                V4L2_MBUS_PCLK_SAMPLE_FALLING | \
+                V4L2_MBUS_DATA_ACTIVE_HIGH)
 
 static int isi_camera_try_bus_param(struct soc_camera_device *icd,
                                     unsigned char buswidth)
 {
+        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct atmel_isi *isi = ici->priv;
-        unsigned long camera_flags;
+        struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+        unsigned long common_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;
+        ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+        if (!ret) {
+                common_flags = soc_mbus_config_compatible(&cfg,
+                                                          ISI_BUS_PARAM);
+                if (!common_flags) {
+                        dev_warn(icd->parent,
+                                 "Flags incompatible: camera 0x%x, host 0x%x\n",
+                                 cfg.flags, ISI_BUS_PARAM);
+                        return -EINVAL;
+                }
+        } else if (ret != -ENOIOCTLCMD) {
+                return ret;
+        }
+
+        if ((1 << (buswidth - 1)) & isi->width_flags)
+                return 0;
+        return -EINVAL;
 }
 
 
@@ -812,59 +805,71 @@ static int isi_camera_querycap(struct soc_camera_host *ici,
 
 static int isi_camera_set_bus_param(struct soc_camera_device *icd, u32 pixfmt)
 {
+        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct atmel_isi *isi = ici->priv;
-        unsigned long bus_flags, camera_flags, common_flags;
+        struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+        unsigned long 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;
+        ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+        if (!ret) {
+                common_flags = soc_mbus_config_compatible(&cfg,
+                                                          ISI_BUS_PARAM);
+                if (!common_flags) {
+                        dev_warn(icd->parent,
+                                 "Flags incompatible: camera 0x%x, host 0x%x\n",
+                                 cfg.flags, ISI_BUS_PARAM);
+                        return -EINVAL;
+                }
+        } else if (ret != -ENOIOCTLCMD) {
+                return ret;
+        } else {
+                common_flags = ISI_BUS_PARAM;
+        }
+        dev_dbg(icd->parent, "Flags cam: 0x%x host: 0x%x common: 0x%lx\n",
+                cfg.flags, ISI_BUS_PARAM, common_flags);
 
         /* Make choises, based on platform preferences */
-        if ((common_flags & SOCAM_HSYNC_ACTIVE_HIGH) &&
-            (common_flags & SOCAM_HSYNC_ACTIVE_LOW)) {
+        if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
+            (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
                 if (isi->pdata->hsync_act_low)
-                        common_flags &= ~SOCAM_HSYNC_ACTIVE_HIGH;
+                        common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
                 else
-                        common_flags &= ~SOCAM_HSYNC_ACTIVE_LOW;
+                        common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
         }
 
-        if ((common_flags & SOCAM_VSYNC_ACTIVE_HIGH) &&
-            (common_flags & SOCAM_VSYNC_ACTIVE_LOW)) {
+        if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
+            (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
                 if (isi->pdata->vsync_act_low)
-                        common_flags &= ~SOCAM_VSYNC_ACTIVE_HIGH;
+                        common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
                 else
-                        common_flags &= ~SOCAM_VSYNC_ACTIVE_LOW;
+                        common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
         }
 
-        if ((common_flags & SOCAM_PCLK_SAMPLE_RISING) &&
-            (common_flags & SOCAM_PCLK_SAMPLE_FALLING)) {
+        if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
+            (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
                 if (isi->pdata->pclk_act_falling)
-                        common_flags &= ~SOCAM_PCLK_SAMPLE_RISING;
+                        common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
                 else
-                        common_flags &= ~SOCAM_PCLK_SAMPLE_FALLING;
+                        common_flags &= ~V4L2_MBUS_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",
+        cfg.flags = common_flags;
+        ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
+        if (ret < 0 && ret != -ENOIOCTLCMD) {
+                dev_dbg(icd->parent, "camera s_mbus_config(0x%lx) returned %d\n",
                         common_flags, ret);
                 return ret;
         }
 
         /* set bus param for ISI */
-        if (common_flags & SOCAM_HSYNC_ACTIVE_LOW)
+        if (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
                 cfg1 |= ISI_CFG1_HSYNC_POL_ACTIVE_LOW;
-        if (common_flags & SOCAM_VSYNC_ACTIVE_LOW)
+        if (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
                 cfg1 |= ISI_CFG1_VSYNC_POL_ACTIVE_LOW;
-        if (common_flags & SOCAM_PCLK_SAMPLE_FALLING)
+        if (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
                 cfg1 |= ISI_CFG1_PIXCLK_POL_ACTIVE_FALLING;
 
         if (isi->pdata->has_emb_sync)
@@ -983,6 +988,11 @@ static int __devinit atmel_isi_probe(struct platform_device *pdev)
                 goto err_ioremap;
         }
 
+        if (pdata->data_width_flags & ISI_DATAWIDTH_8)
+                isi->width_flags = 1 << 7;
+        if (pdata->data_width_flags & ISI_DATAWIDTH_10)
+                isi->width_flags |= 1 << 9;
+
         isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
 
         irq = platform_get_irq(pdev, 0);
diff --git a/drivers/media/video/cx18/cx18-driver.c b/drivers/media/video/cx18/cx18-driver.c
index 9e2f870f4258..c6ff32a6137c 100644
--- a/drivers/media/video/cx18/cx18-driver.c
+++ b/drivers/media/video/cx18/cx18-driver.c
@@ -1085,6 +1085,8 @@ static int __devinit cx18_probe(struct pci_dev *pci_dev,
                 setup.addr = ADDR_UNSET;
                 setup.type = cx->options.tuner;
                 setup.mode_mask = T_ANALOG_TV;  /* matches TV tuners */
+                if (cx->options.radio > 0)
+                        setup.mode_mask |= T_RADIO;
                 setup.tuner_callback = (setup.type == TUNER_XC2028) ?
                         cx18_reset_tuner_gpio : NULL;
                 cx18_call_all(cx, tuner, s_type_addr, &setup);
diff --git a/drivers/media/video/cx25821/Kconfig b/drivers/media/video/cx25821/Kconfig
new file mode 100644
index 000000000000..5f6b54213713
--- /dev/null
+++ b/drivers/media/video/cx25821/Kconfig
@@ -0,0 +1,34 @@
+config VIDEO_CX25821
+        tristate "Conexant cx25821 support"
+        depends on DVB_CORE && VIDEO_DEV && PCI && I2C
+        select I2C_ALGOBIT
+        select VIDEO_BTCX
+        select VIDEO_TVEEPROM
+        depends on RC_CORE
+        select VIDEOBUF_DVB
+        select VIDEOBUF_DMA_SG
+        select VIDEO_CX25840
+        select VIDEO_CX2341X
+        ---help---
+          This is a video4linux driver for Conexant 25821 based
+          TV cards.
+
+          To compile this driver as a module, choose M here: the
+          module will be called cx25821
+
+config VIDEO_CX25821_ALSA
+        tristate "Conexant 25821 DMA audio support"
+        depends on VIDEO_CX25821 && SND && EXPERIMENTAL
+        select SND_PCM
+        ---help---
+          This is a video4linux driver for direct (DMA) audio on
+          Conexant 25821 based capture cards using ALSA.
+
+          It only works with boards with function 01 enabled.
+          To check if your board supports, use lspci -n.
+          If supported, you should see 14f1:8801 or 14f1:8811
+          PCI device.
+
+          To compile this driver as a module, choose M here: the
+          module will be called cx25821-alsa.
+
diff --git a/drivers/media/video/cx25821/Makefile b/drivers/media/video/cx25821/Makefile
new file mode 100644
index 000000000000..aedde18c68f9
--- /dev/null
+++ b/drivers/media/video/cx25821/Makefile
@@ -0,0 +1,13 @@
+cx25821-y   := cx25821-core.o cx25821-cards.o cx25821-i2c.o \
+                       cx25821-gpio.o cx25821-medusa-video.o \
+                       cx25821-video.o cx25821-video-upstream.o \
+                       cx25821-video-upstream-ch2.o \
+                       cx25821-audio-upstream.o
+
+obj-$(CONFIG_VIDEO_CX25821) += cx25821.o
+obj-$(CONFIG_VIDEO_CX25821_ALSA) += cx25821-alsa.o
+
+ccflags-y := -Idrivers/media/video
+ccflags-y += -Idrivers/media/common/tuners
+ccflags-y += -Idrivers/media/dvb/dvb-core
+ccflags-y += -Idrivers/media/dvb/frontends
diff --git a/drivers/media/video/cx25821/cx25821-alsa.c b/drivers/media/video/cx25821/cx25821-alsa.c
new file mode 100644
index 000000000000..09e99de5fd21
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-alsa.c
@@ -0,0 +1,795 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <shu.lin@conexant.com>, <hiep.huynh@conexant.com>
+ *      Based on SAA713x ALSA driver and CX88 driver
+ *
+ *   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
+ *
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/vmalloc.h>
+#include <linux/dma-mapping.h>
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include <linux/delay.h>
+#include <sound/core.h>
+#include <sound/pcm.h>
+#include <sound/pcm_params.h>
+#include <sound/control.h>
+#include <sound/initval.h>
+#include <sound/tlv.h>
+
+#include "cx25821.h"
+#include "cx25821-reg.h"
+
+#define AUDIO_SRAM_CHANNEL      SRAM_CH08
+
+#define dprintk(level, fmt, arg...)                             \
+do {                                                            \
+        if (debug >= level)                                     \
+                pr_info("%s/1: " fmt, chip->dev->name, ##arg);  \
+} while (0)
+#define dprintk_core(level, fmt, arg...)                                \
+do {                                                                    \
+        if (debug >= level)                                             \
+                printk(KERN_DEBUG "%s/1: " fmt, chip->dev->name, ##arg); \
+} while (0)
+
+/****************************************************************************
+        Data type declarations - Can be moded to a header file later
+ ****************************************************************************/
+
+static struct snd_card *snd_cx25821_cards[SNDRV_CARDS];
+static int devno;
+
+struct cx25821_audio_buffer {
+        unsigned int bpl;
+        struct btcx_riscmem risc;
+        struct videobuf_dmabuf dma;
+};
+
+struct cx25821_audio_dev {
+        struct cx25821_dev *dev;
+        struct cx25821_dmaqueue q;
+
+        /* pci i/o */
+        struct pci_dev *pci;
+
+        /* audio controls */
+        int irq;
+
+        struct snd_card *card;
+
+        unsigned long iobase;
+        spinlock_t reg_lock;
+        atomic_t count;
+
+        unsigned int dma_size;
+        unsigned int period_size;
+        unsigned int num_periods;
+
+        struct videobuf_dmabuf *dma_risc;
+
+        struct cx25821_audio_buffer *buf;
+
+        struct snd_pcm_substream *substream;
+};
+
+
+/****************************************************************************
+                        Module global static vars
+ ****************************************************************************/
+
+static int index[SNDRV_CARDS] = SNDRV_DEFAULT_IDX;      /* Index 0-MAX */
+static char *id[SNDRV_CARDS] = SNDRV_DEFAULT_STR;       /* ID for this card */
+static int enable[SNDRV_CARDS] = { 1, [1 ... (SNDRV_CARDS - 1)] = 1 };
+
+module_param_array(enable, bool, NULL, 0444);
+MODULE_PARM_DESC(enable, "Enable cx25821 soundcard. default enabled.");
+
+module_param_array(index, int, NULL, 0444);
+MODULE_PARM_DESC(index, "Index value for cx25821 capture interface(s).");
+
+/****************************************************************************
+                                Module macros
+ ****************************************************************************/
+
+MODULE_DESCRIPTION("ALSA driver module for cx25821 based capture cards");
+MODULE_AUTHOR("Hiep Huynh");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("{{Conexant,25821}");   /* "{{Conexant,23881}," */
+
+static unsigned int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "enable debug messages");
+
+/****************************************************************************
+                        Module specific funtions
+ ****************************************************************************/
+/* Constants taken from cx88-reg.h */
+#define AUD_INT_DN_RISCI1       (1 <<  0)
+#define AUD_INT_UP_RISCI1       (1 <<  1)
+#define AUD_INT_RDS_DN_RISCI1   (1 <<  2)
+#define AUD_INT_DN_RISCI2       (1 <<  4)       /* yes, 3 is skipped */
+#define AUD_INT_UP_RISCI2       (1 <<  5)
+#define AUD_INT_RDS_DN_RISCI2   (1 <<  6)
+#define AUD_INT_DN_SYNC         (1 << 12)
+#define AUD_INT_UP_SYNC         (1 << 13)
+#define AUD_INT_RDS_DN_SYNC     (1 << 14)
+#define AUD_INT_OPC_ERR         (1 << 16)
+#define AUD_INT_BER_IRQ         (1 << 20)
+#define AUD_INT_MCHG_IRQ        (1 << 21)
+#define GP_COUNT_CONTROL_RESET  0x3
+
+#define PCI_MSK_AUD_EXT   (1 <<  4)
+#define PCI_MSK_AUD_INT   (1 <<  3)
+/*
+ * BOARD Specific: Sets audio DMA
+ */
+
+static int _cx25821_start_audio_dma(struct cx25821_audio_dev *chip)
+{
+        struct cx25821_audio_buffer *buf = chip->buf;
+        struct cx25821_dev *dev = chip->dev;
+        struct sram_channel *audio_ch =
+            &cx25821_sram_channels[AUDIO_SRAM_CHANNEL];
+        u32 tmp = 0;
+
+        /* enable output on the GPIO 0 for the MCLK ADC (Audio) */
+        cx25821_set_gpiopin_direction(chip->dev, 0, 0);
+
+        /* Make sure RISC/FIFO are off before changing FIFO/RISC settings */
+        cx_clear(AUD_INT_DMA_CTL,
+                 FLD_AUD_DST_A_RISC_EN | FLD_AUD_DST_A_FIFO_EN);
+
+        /* setup fifo + format - out channel */
+        cx25821_sram_channel_setup_audio(chip->dev, audio_ch, buf->bpl,
+                                         buf->risc.dma);
+
+        /* sets bpl size */
+        cx_write(AUD_A_LNGTH, buf->bpl);
+
+        /* reset counter */
+        /* GP_COUNT_CONTROL_RESET = 0x3 */
+        cx_write(AUD_A_GPCNT_CTL, GP_COUNT_CONTROL_RESET);
+        atomic_set(&chip->count, 0);
+
+        /* Set the input mode to 16-bit */
+        tmp = cx_read(AUD_A_CFG);
+        cx_write(AUD_A_CFG,
+                 tmp | FLD_AUD_DST_PK_MODE | FLD_AUD_DST_ENABLE |
+                 FLD_AUD_CLK_ENABLE);
+
+        /*
+        pr_info("DEBUG: Start audio DMA, %d B/line, cmds_start(0x%x)= %d lines/FIFO, %d periods, %d byte buffer\n",
+                buf->bpl, audio_ch->cmds_start,
+                cx_read(audio_ch->cmds_start + 12)>>1,
+                chip->num_periods, buf->bpl * chip->num_periods);
+        */
+
+        /* Enables corresponding bits at AUD_INT_STAT */
+        cx_write(AUD_A_INT_MSK,
+                 FLD_AUD_DST_RISCI1 | FLD_AUD_DST_OF | FLD_AUD_DST_SYNC |
+                 FLD_AUD_DST_OPC_ERR);
+
+        /* Clean any pending interrupt bits already set */
+        cx_write(AUD_A_INT_STAT, ~0);
+
+        /* enable audio irqs */
+        cx_set(PCI_INT_MSK, chip->dev->pci_irqmask | PCI_MSK_AUD_INT);
+
+        /* Turn on audio downstream fifo and risc enable 0x101 */
+        tmp = cx_read(AUD_INT_DMA_CTL);
+        cx_set(AUD_INT_DMA_CTL,
+               tmp | (FLD_AUD_DST_A_RISC_EN | FLD_AUD_DST_A_FIFO_EN));
+
+        mdelay(100);
+        return 0;
+}
+
+/*
+ * BOARD Specific: Resets audio DMA
+ */
+static int _cx25821_stop_audio_dma(struct cx25821_audio_dev *chip)
+{
+        struct cx25821_dev *dev = chip->dev;
+
+        /* stop dma */
+        cx_clear(AUD_INT_DMA_CTL,
+                 FLD_AUD_DST_A_RISC_EN | FLD_AUD_DST_A_FIFO_EN);
+
+        /* disable irqs */
+        cx_clear(PCI_INT_MSK, PCI_MSK_AUD_INT);
+        cx_clear(AUD_A_INT_MSK,
+                 AUD_INT_OPC_ERR | AUD_INT_DN_SYNC | AUD_INT_DN_RISCI2 |
+                 AUD_INT_DN_RISCI1);
+
+        return 0;
+}
+
+#define MAX_IRQ_LOOP 50
+
+/*
+ * BOARD Specific: IRQ dma bits
+ */
+static char *cx25821_aud_irqs[32] = {
+        "dn_risci1", "up_risci1", "rds_dn_risc1",       /* 0-2 */
+        NULL,                   /* reserved */
+        "dn_risci2", "up_risci2", "rds_dn_risc2",       /* 4-6 */
+        NULL,                   /* reserved */
+        "dnf_of", "upf_uf", "rds_dnf_uf",       /* 8-10 */
+        NULL,                   /* reserved */
+        "dn_sync", "up_sync", "rds_dn_sync",    /* 12-14 */
+        NULL,                   /* reserved */
+        "opc_err", "par_err", "rip_err",        /* 16-18 */
+        "pci_abort", "ber_irq", "mchg_irq"      /* 19-21 */
+};
+
+/*
+ * BOARD Specific: Threats IRQ audio specific calls
+ */
+static void cx25821_aud_irq(struct cx25821_audio_dev *chip, u32 status,
+                            u32 mask)
+{
+        struct cx25821_dev *dev = chip->dev;
+
+        if (0 == (status & mask))
+                return;
+
+        cx_write(AUD_A_INT_STAT, status);
+        if (debug > 1 || (status & mask & ~0xff))
+                cx25821_print_irqbits(dev->name, "irq aud",
+                                      cx25821_aud_irqs,
+                                      ARRAY_SIZE(cx25821_aud_irqs), status,
+                                      mask);
+
+        /* risc op code error */
+        if (status & AUD_INT_OPC_ERR) {
+                pr_warn("WARNING %s/1: Audio risc op code error\n", dev->name);
+
+                cx_clear(AUD_INT_DMA_CTL,
+                         FLD_AUD_DST_A_RISC_EN | FLD_AUD_DST_A_FIFO_EN);
+                cx25821_sram_channel_dump_audio(dev,
+                                                &cx25821_sram_channels
+                                                [AUDIO_SRAM_CHANNEL]);
+        }
+        if (status & AUD_INT_DN_SYNC) {
+                pr_warn("WARNING %s: Downstream sync error!\n", dev->name);
+                cx_write(AUD_A_GPCNT_CTL, GP_COUNT_CONTROL_RESET);
+                return;
+        }
+
+        /* risc1 downstream */
+        if (status & AUD_INT_DN_RISCI1) {
+                atomic_set(&chip->count, cx_read(AUD_A_GPCNT));
+                snd_pcm_period_elapsed(chip->substream);
+        }
+}
+
+/*
+ * BOARD Specific: Handles IRQ calls
+ */
+static irqreturn_t cx25821_irq(int irq, void *dev_id)
+{
+        struct cx25821_audio_dev *chip = dev_id;
+        struct cx25821_dev *dev = chip->dev;
+        u32 status, pci_status;
+        u32 audint_status, audint_mask;
+        int loop, handled = 0;
+        int audint_count = 0;
+
+        audint_status = cx_read(AUD_A_INT_STAT);
+        audint_mask = cx_read(AUD_A_INT_MSK);
+        audint_count = cx_read(AUD_A_GPCNT);
+        status = cx_read(PCI_INT_STAT);
+
+        for (loop = 0; loop < 1; loop++) {
+                status = cx_read(PCI_INT_STAT);
+                if (0 == status) {
+                        status = cx_read(PCI_INT_STAT);
+                        audint_status = cx_read(AUD_A_INT_STAT);
+                        audint_mask = cx_read(AUD_A_INT_MSK);
+
+                        if (status) {
+                                handled = 1;
+                                cx_write(PCI_INT_STAT, status);
+
+                                cx25821_aud_irq(chip, audint_status,
+                                                audint_mask);
+                                break;
+                        } else
+                                goto out;
+                }
+
+                handled = 1;
+                cx_write(PCI_INT_STAT, status);
+
+                cx25821_aud_irq(chip, audint_status, audint_mask);
+        }
+
+        pci_status = cx_read(PCI_INT_STAT);
+
+        if (handled)
+                cx_write(PCI_INT_STAT, pci_status);
+
+out:
+        return IRQ_RETVAL(handled);
+}
+
+static int dsp_buffer_free(struct cx25821_audio_dev *chip)
+{
+        BUG_ON(!chip->dma_size);
+
+        dprintk(2, "Freeing buffer\n");
+        videobuf_dma_unmap(&chip->pci->dev, chip->dma_risc);
+        videobuf_dma_free(chip->dma_risc);
+        btcx_riscmem_free(chip->pci, &chip->buf->risc);
+        kfree(chip->buf);
+
+        chip->dma_risc = NULL;
+        chip->dma_size = 0;
+
+        return 0;
+}
+
+/****************************************************************************
+                                ALSA PCM Interface
+ ****************************************************************************/
+
+/*
+ * Digital hardware definition
+ */
+#define DEFAULT_FIFO_SIZE       384
+static struct snd_pcm_hardware snd_cx25821_digital_hw = {
+        .info = SNDRV_PCM_INFO_MMAP |
+            SNDRV_PCM_INFO_INTERLEAVED |
+            SNDRV_PCM_INFO_BLOCK_TRANSFER | SNDRV_PCM_INFO_MMAP_VALID,
+        .formats = SNDRV_PCM_FMTBIT_S16_LE,
+
+        .rates = SNDRV_PCM_RATE_48000,
+        .rate_min = 48000,
+        .rate_max = 48000,
+        .channels_min = 2,
+        .channels_max = 2,
+        /* Analog audio output will be full of clicks and pops if there
+           are not exactly four lines in the SRAM FIFO buffer.  */
+        .period_bytes_min = DEFAULT_FIFO_SIZE / 3,
+        .period_bytes_max = DEFAULT_FIFO_SIZE / 3,
+        .periods_min = 1,
+        .periods_max = AUDIO_LINE_SIZE,
+        /* 128 * 128 = 16384 = 1024 * 16 */
+        .buffer_bytes_max = (AUDIO_LINE_SIZE * AUDIO_LINE_SIZE),
+};
+
+/*
+ * audio pcm capture open callback
+ */
+static int snd_cx25821_pcm_open(struct snd_pcm_substream *substream)
+{
+        struct cx25821_audio_dev *chip = snd_pcm_substream_chip(substream);
+        struct snd_pcm_runtime *runtime = substream->runtime;
+        int err;
+        unsigned int bpl = 0;
+
+        if (!chip) {
+                pr_err("DEBUG: cx25821 can't find device struct. Can't proceed with open\n");
+                return -ENODEV;
+        }
+
+        err =
+            snd_pcm_hw_constraint_pow2(runtime, 0, SNDRV_PCM_HW_PARAM_PERIODS);
+        if (err < 0)
+                goto _error;
+
+        chip->substream = substream;
+
+        runtime->hw = snd_cx25821_digital_hw;
+
+        if (cx25821_sram_channels[AUDIO_SRAM_CHANNEL].fifo_size !=
+            DEFAULT_FIFO_SIZE) {
+                /* since there are 3 audio Clusters */
+                bpl = cx25821_sram_channels[AUDIO_SRAM_CHANNEL].fifo_size / 3;
+                bpl &= ~7;      /* must be multiple of 8 */
+
+                if (bpl > AUDIO_LINE_SIZE)
+                        bpl = AUDIO_LINE_SIZE;
+
+                runtime->hw.period_bytes_min = bpl;
+                runtime->hw.period_bytes_max = bpl;
+        }
+
+        return 0;
+_error:
+        dprintk(1, "Error opening PCM!\n");
+        return err;
+}
+
+/*
+ * audio close callback
+ */
+static int snd_cx25821_close(struct snd_pcm_substream *substream)
+{
+        return 0;
+}
+
+/*
+ * hw_params callback
+ */
+static int snd_cx25821_hw_params(struct snd_pcm_substream *substream,
+                                 struct snd_pcm_hw_params *hw_params)
+{
+        struct cx25821_audio_dev *chip = snd_pcm_substream_chip(substream);
+        struct videobuf_dmabuf *dma;
+
+        struct cx25821_audio_buffer *buf;
+        int ret;
+
+        if (substream->runtime->dma_area) {
+                dsp_buffer_free(chip);
+                substream->runtime->dma_area = NULL;
+        }
+
+        chip->period_size = params_period_bytes(hw_params);
+        chip->num_periods = params_periods(hw_params);
+        chip->dma_size = chip->period_size * params_periods(hw_params);
+
+        BUG_ON(!chip->dma_size);
+        BUG_ON(chip->num_periods & (chip->num_periods - 1));
+
+        buf = kzalloc(sizeof(*buf), GFP_KERNEL);
+        if (NULL == buf)
+                return -ENOMEM;
+
+        if (chip->period_size > AUDIO_LINE_SIZE)
+                chip->period_size = AUDIO_LINE_SIZE;
+
+        buf->bpl = chip->period_size;
+
+        dma = &buf->dma;
+        videobuf_dma_init(dma);
+        ret = videobuf_dma_init_kernel(dma, PCI_DMA_FROMDEVICE,
+                                       (PAGE_ALIGN(chip->dma_size) >>
+                                        PAGE_SHIFT));
+        if (ret < 0)
+                goto error;
+
+        ret = videobuf_dma_map(&chip->pci->dev, dma);
+        if (ret < 0)
+                goto error;
+
+        ret =
+            cx25821_risc_databuffer_audio(chip->pci, &buf->risc, dma->sglist,
+                                          chip->period_size, chip->num_periods,
+                                          1);
+        if (ret < 0) {
+                pr_info("DEBUG: ERROR after cx25821_risc_databuffer_audio()\n");
+                goto error;
+        }
+
+        /* Loop back to start of program */
+        buf->risc.jmp[0] = cpu_to_le32(RISC_JUMP | RISC_IRQ1 | RISC_CNT_INC);
+        buf->risc.jmp[1] = cpu_to_le32(buf->risc.dma);
+        buf->risc.jmp[2] = cpu_to_le32(0);      /* bits 63-32 */
+
+        chip->buf = buf;
+        chip->dma_risc = dma;
+
+        substream->runtime->dma_area = chip->dma_risc->vaddr;
+        substream->runtime->dma_bytes = chip->dma_size;
+        substream->runtime->dma_addr = 0;
+
+        return 0;
+
+error:
+        kfree(buf);
+        return ret;
+}
+
+/*
+ * hw free callback
+ */
+static int snd_cx25821_hw_free(struct snd_pcm_substream *substream)
+{
+        struct cx25821_audio_dev *chip = snd_pcm_substream_chip(substream);
+
+        if (substream->runtime->dma_area) {
+                dsp_buffer_free(chip);
+                substream->runtime->dma_area = NULL;
+        }
+
+        return 0;
+}
+
+/*
+ * prepare callback
+ */
+static int snd_cx25821_prepare(struct snd_pcm_substream *substream)
+{
+        return 0;
+}
+
+/*
+ * trigger callback
+ */
+static int snd_cx25821_card_trigger(struct snd_pcm_substream *substream,
+                                    int cmd)
+{
+        struct cx25821_audio_dev *chip = snd_pcm_substream_chip(substream);
+        int err = 0;
+
+        /* Local interrupts are already disabled by ALSA */
+        spin_lock(&chip->reg_lock);
+
+        switch (cmd) {
+        case SNDRV_PCM_TRIGGER_START:
+                err = _cx25821_start_audio_dma(chip);
+                break;
+        case SNDRV_PCM_TRIGGER_STOP:
+                err = _cx25821_stop_audio_dma(chip);
+                break;
+        default:
+                err = -EINVAL;
+                break;
+        }
+
+        spin_unlock(&chip->reg_lock);
+
+        return err;
+}
+
+/*
+ * pointer callback
+ */
+static snd_pcm_uframes_t snd_cx25821_pointer(struct snd_pcm_substream
+                                             *substream)
+{
+        struct cx25821_audio_dev *chip = snd_pcm_substream_chip(substream);
+        struct snd_pcm_runtime *runtime = substream->runtime;
+        u16 count;
+
+        count = atomic_read(&chip->count);
+
+        return runtime->period_size * (count & (runtime->periods - 1));
+}
+
+/*
+ * page callback (needed for mmap)
+ */
+static struct page *snd_cx25821_page(struct snd_pcm_substream *substream,
+                                     unsigned long offset)
+{
+        void *pageptr = substream->runtime->dma_area + offset;
+
+        return vmalloc_to_page(pageptr);
+}
+
+/*
+ * operators
+ */
+static struct snd_pcm_ops snd_cx25821_pcm_ops = {
+        .open = snd_cx25821_pcm_open,
+        .close = snd_cx25821_close,
+        .ioctl = snd_pcm_lib_ioctl,
+        .hw_params = snd_cx25821_hw_params,
+        .hw_free = snd_cx25821_hw_free,
+        .prepare = snd_cx25821_prepare,
+        .trigger = snd_cx25821_card_trigger,
+        .pointer = snd_cx25821_pointer,
+        .page = snd_cx25821_page,
+};
+
+/*
+ * ALSA create a PCM device:  Called when initializing the board.
+ * Sets up the name and hooks up the callbacks
+ */
+static int snd_cx25821_pcm(struct cx25821_audio_dev *chip, int device,
+                           char *name)
+{
+        struct snd_pcm *pcm;
+        int err;
+
+        err = snd_pcm_new(chip->card, name, device, 0, 1, &pcm);
+        if (err < 0) {
+                pr_info("ERROR: FAILED snd_pcm_new() in %s\n", __func__);
+                return err;
+        }
+        pcm->private_data = chip;
+        pcm->info_flags = 0;
+        strcpy(pcm->name, name);
+        snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_CAPTURE, &snd_cx25821_pcm_ops);
+
+        return 0;
+}
+
+/****************************************************************************
+                        Basic Flow for Sound Devices
+ ****************************************************************************/
+
+/*
+ * PCI ID Table - 14f1:8801 and 14f1:8811 means function 1: Audio
+ * Only boards with eeprom and byte 1 at eeprom=1 have it
+ */
+
+static DEFINE_PCI_DEVICE_TABLE(cx25821_audio_pci_tbl) = {
+        {0x14f1, 0x0920, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+        {0,}
+};
+
+MODULE_DEVICE_TABLE(pci, cx25821_audio_pci_tbl);
+
+/*
+ * Not used in the function snd_cx25821_dev_free so removing
+ * from the file.
+ */
+/*
+static int snd_cx25821_free(struct cx25821_audio_dev *chip)
+{
+        if (chip->irq >= 0)
+                free_irq(chip->irq, chip);
+
+        cx25821_dev_unregister(chip->dev);
+        pci_disable_device(chip->pci);
+
+        return 0;
+}
+*/
+
+/*
+ * Component Destructor
+ */
+static void snd_cx25821_dev_free(struct snd_card *card)
+{
+        struct cx25821_audio_dev *chip = card->private_data;
+
+        /* snd_cx25821_free(chip); */
+        snd_card_free(chip->card);
+}
+
+/*
+ * Alsa Constructor - Component probe
+ */
+static int cx25821_audio_initdev(struct cx25821_dev *dev)
+{
+        struct snd_card *card;
+        struct cx25821_audio_dev *chip;
+        int err;
+
+        if (devno >= SNDRV_CARDS) {
+                pr_info("DEBUG ERROR: devno >= SNDRV_CARDS %s\n", __func__);
+                return -ENODEV;
+        }
+
+        if (!enable[devno]) {
+                ++devno;
+                pr_info("DEBUG ERROR: !enable[devno] %s\n", __func__);
+                return -ENOENT;
+        }
+
+        err = snd_card_create(index[devno], id[devno], THIS_MODULE,
+                         sizeof(struct cx25821_audio_dev), &card);
+        if (err < 0) {
+                pr_info("DEBUG ERROR: cannot create snd_card_new in %s\n",
+                        __func__);
+                return err;
+        }
+
+        strcpy(card->driver, "cx25821");
+
+        /* Card "creation" */
+        card->private_free = snd_cx25821_dev_free;
+        chip = card->private_data;
+        spin_lock_init(&chip->reg_lock);
+
+        chip->dev = dev;
+        chip->card = card;
+        chip->pci = dev->pci;
+        chip->iobase = pci_resource_start(dev->pci, 0);
+
+        chip->irq = dev->pci->irq;
+
+        err = request_irq(dev->pci->irq, cx25821_irq,
+                          IRQF_SHARED, chip->dev->name, chip);
+
+        if (err < 0) {
+                pr_err("ERROR %s: can't get IRQ %d for ALSA\n",
+                       chip->dev->name, dev->pci->irq);
+                goto error;
+        }
+
+        err = snd_cx25821_pcm(chip, 0, "cx25821 Digital");
+        if (err < 0) {
+                pr_info("DEBUG ERROR: cannot create snd_cx25821_pcm %s\n",
+                        __func__);
+                goto error;
+        }
+
+        snd_card_set_dev(card, &chip->pci->dev);
+
+        strcpy(card->shortname, "cx25821");
+        sprintf(card->longname, "%s at 0x%lx irq %d", chip->dev->name,
+                chip->iobase, chip->irq);
+        strcpy(card->mixername, "CX25821");
+
+        pr_info("%s/%i: ALSA support for cx25821 boards\n",
+                card->driver, devno);
+
+        err = snd_card_register(card);
+        if (err < 0) {
+                pr_info("DEBUG ERROR: cannot register sound card %s\n",
+                        __func__);
+                goto error;
+        }
+
+        snd_cx25821_cards[devno] = card;
+
+        devno++;
+        return 0;
+
+error:
+        snd_card_free(card);
+        return err;
+}
+
+/****************************************************************************
+                                LINUX MODULE INIT
+ ****************************************************************************/
+static void cx25821_audio_fini(void)
+{
+        snd_card_free(snd_cx25821_cards[0]);
+}
+
+/*
+ * Module initializer
+ *
+ * Loops through present saa7134 cards, and assigns an ALSA device
+ * to each one
+ *
+ */
+static int cx25821_alsa_init(void)
+{
+        struct cx25821_dev *dev = NULL;
+        struct list_head *list;
+
+        mutex_lock(&cx25821_devlist_mutex);
+        list_for_each(list, &cx25821_devlist) {
+                dev = list_entry(list, struct cx25821_dev, devlist);
+                cx25821_audio_initdev(dev);
+        }
+        mutex_unlock(&cx25821_devlist_mutex);
+
+        if (dev == NULL)
+                pr_info("ERROR ALSA: no cx25821 cards found\n");
+
+        return 0;
+
+}
+
+late_initcall(cx25821_alsa_init);
+module_exit(cx25821_audio_fini);
+
+/* ----------------------------------------------------------- */
+/*
+ * Local variables:
+ * c-basic-offset: 8
+ * End:
+ */
diff --git a/drivers/media/video/cx25821/cx25821-audio-upstream.c b/drivers/media/video/cx25821/cx25821-audio-upstream.c
new file mode 100644
index 000000000000..c20d6dece154
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-audio-upstream.c
@@ -0,0 +1,788 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <hiep.huynh@conexant.com>, <shu.lin@conexant.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include "cx25821-video.h"
+#include "cx25821-audio-upstream.h"
+
+#include <linux/fs.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/syscalls.h>
+#include <linux/file.h>
+#include <linux/fcntl.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+
+MODULE_DESCRIPTION("v4l2 driver module for cx25821 based TV cards");
+MODULE_AUTHOR("Hiep Huynh <hiep.huynh@conexant.com>");
+MODULE_LICENSE("GPL");
+
+static int _intr_msk = FLD_AUD_SRC_RISCI1 | FLD_AUD_SRC_OF |
+                        FLD_AUD_SRC_SYNC | FLD_AUD_SRC_OPC_ERR;
+
+int cx25821_sram_channel_setup_upstream_audio(struct cx25821_dev *dev,
+                                              struct sram_channel *ch,
+                                              unsigned int bpl, u32 risc)
+{
+        unsigned int i, lines;
+        u32 cdt;
+
+        if (ch->cmds_start == 0) {
+                cx_write(ch->ptr1_reg, 0);
+                cx_write(ch->ptr2_reg, 0);
+                cx_write(ch->cnt2_reg, 0);
+                cx_write(ch->cnt1_reg, 0);
+                return 0;
+        }
+
+        bpl = (bpl + 7) & ~7;   /* alignment */
+        cdt = ch->cdt;
+        lines = ch->fifo_size / bpl;
+
+        if (lines > 3)
+                lines = 3;
+
+        BUG_ON(lines < 2);
+
+        /* write CDT */
+        for (i = 0; i < lines; i++) {
+                cx_write(cdt + 16 * i, ch->fifo_start + bpl * i);
+                cx_write(cdt + 16 * i + 4, 0);
+                cx_write(cdt + 16 * i + 8, 0);
+                cx_write(cdt + 16 * i + 12, 0);
+        }
+
+        /* write CMDS */
+        cx_write(ch->cmds_start + 0, risc);
+
+        cx_write(ch->cmds_start + 4, 0);
+        cx_write(ch->cmds_start + 8, cdt);
+        cx_write(ch->cmds_start + 12, AUDIO_CDT_SIZE_QW);
+        cx_write(ch->cmds_start + 16, ch->ctrl_start);
+
+        /* IQ size */
+        cx_write(ch->cmds_start + 20, AUDIO_IQ_SIZE_DW);
+
+        for (i = 24; i < 80; i += 4)
+                cx_write(ch->cmds_start + i, 0);
+
+        /* fill registers */
+        cx_write(ch->ptr1_reg, ch->fifo_start);
+        cx_write(ch->ptr2_reg, cdt);
+        cx_write(ch->cnt2_reg, AUDIO_CDT_SIZE_QW);
+        cx_write(ch->cnt1_reg, AUDIO_CLUSTER_SIZE_QW - 1);
+
+        return 0;
+}
+
+static __le32 *cx25821_risc_field_upstream_audio(struct cx25821_dev *dev,
+                                                 __le32 *rp,
+                                                 dma_addr_t databuf_phys_addr,
+                                                 unsigned int bpl,
+                                                 int fifo_enable)
+{
+        unsigned int line;
+        struct sram_channel *sram_ch =
+           dev->channels[dev->_audio_upstream_channel].sram_channels;
+        int offset = 0;
+
+        /* scan lines */
+        for (line = 0; line < LINES_PER_AUDIO_BUFFER; line++) {
+                *(rp++) = cpu_to_le32(RISC_READ | RISC_SOL | RISC_EOL | bpl);
+                *(rp++) = cpu_to_le32(databuf_phys_addr + offset);
+                *(rp++) = cpu_to_le32(0);       /* bits 63-32 */
+
+                /* Check if we need to enable the FIFO
+                 * after the first 3 lines.
+                 * For the upstream audio channel,
+                 * the risc engine will enable the FIFO */
+                if (fifo_enable && line == 2) {
+                        *(rp++) = RISC_WRITECR;
+                        *(rp++) = sram_ch->dma_ctl;
+                        *(rp++) = sram_ch->fld_aud_fifo_en;
+                        *(rp++) = 0x00000020;
+                }
+
+                offset += AUDIO_LINE_SIZE;
+        }
+
+        return rp;
+}
+
+int cx25821_risc_buffer_upstream_audio(struct cx25821_dev *dev,
+                                       struct pci_dev *pci,
+                                       unsigned int bpl, unsigned int lines)
+{
+        __le32 *rp;
+        int fifo_enable = 0;
+        int frame = 0, i = 0;
+        int frame_size = AUDIO_DATA_BUF_SZ;
+        int databuf_offset = 0;
+        int risc_flag = RISC_CNT_INC;
+        dma_addr_t risc_phys_jump_addr;
+
+        /* Virtual address of Risc buffer program */
+        rp = dev->_risc_virt_addr;
+
+        /* sync instruction */
+        *(rp++) = cpu_to_le32(RISC_RESYNC | AUDIO_SYNC_LINE);
+
+        for (frame = 0; frame < NUM_AUDIO_FRAMES; frame++) {
+                databuf_offset = frame_size * frame;
+
+                if (frame == 0) {
+                        fifo_enable = 1;
+                        risc_flag = RISC_CNT_RESET;
+                } else {
+                        fifo_enable = 0;
+                        risc_flag = RISC_CNT_INC;
+                }
+
+                /* Calculate physical jump address */
+                if ((frame + 1) == NUM_AUDIO_FRAMES) {
+                        risc_phys_jump_addr =
+                            dev->_risc_phys_start_addr +
+                            RISC_SYNC_INSTRUCTION_SIZE;
+                } else {
+                        risc_phys_jump_addr =
+                            dev->_risc_phys_start_addr +
+                            RISC_SYNC_INSTRUCTION_SIZE +
+                            AUDIO_RISC_DMA_BUF_SIZE * (frame + 1);
+                }
+
+                rp = cx25821_risc_field_upstream_audio(dev, rp,
+                                                       dev->
+                                                       _audiodata_buf_phys_addr
+                                                       + databuf_offset, bpl,
+                                                       fifo_enable);
+
+                if (USE_RISC_NOOP_AUDIO) {
+                        for (i = 0; i < NUM_NO_OPS; i++)
+                                *(rp++) = cpu_to_le32(RISC_NOOP);
+                }
+
+                /* Loop to (Nth)FrameRISC or to Start of Risc program &
+                 * generate IRQ */
+                *(rp++) = cpu_to_le32(RISC_JUMP | RISC_IRQ1 | risc_flag);
+                *(rp++) = cpu_to_le32(risc_phys_jump_addr);
+                *(rp++) = cpu_to_le32(0);
+
+                /* Recalculate virtual address based on frame index */
+                rp = dev->_risc_virt_addr + RISC_SYNC_INSTRUCTION_SIZE / 4 +
+                    (AUDIO_RISC_DMA_BUF_SIZE * (frame + 1) / 4);
+        }
+
+        return 0;
+}
+
+void cx25821_free_memory_audio(struct cx25821_dev *dev)
+{
+        if (dev->_risc_virt_addr) {
+                pci_free_consistent(dev->pci, dev->_audiorisc_size,
+                                    dev->_risc_virt_addr, dev->_risc_phys_addr);
+                dev->_risc_virt_addr = NULL;
+        }
+
+        if (dev->_audiodata_buf_virt_addr) {
+                pci_free_consistent(dev->pci, dev->_audiodata_buf_size,
+                                    dev->_audiodata_buf_virt_addr,
+                                    dev->_audiodata_buf_phys_addr);
+                dev->_audiodata_buf_virt_addr = NULL;
+        }
+}
+
+void cx25821_stop_upstream_audio(struct cx25821_dev *dev)
+{
+        struct sram_channel *sram_ch =
+           dev->channels[AUDIO_UPSTREAM_SRAM_CHANNEL_B].sram_channels;
+        u32 tmp = 0;
+
+        if (!dev->_audio_is_running) {
+                printk(KERN_DEBUG
+                       pr_fmt("No audio file is currently running so return!\n"));
+                return;
+        }
+        /* Disable RISC interrupts */
+        cx_write(sram_ch->int_msk, 0);
+
+        /* Turn OFF risc and fifo enable in AUD_DMA_CNTRL */
+        tmp = cx_read(sram_ch->dma_ctl);
+        cx_write(sram_ch->dma_ctl,
+                 tmp & ~(sram_ch->fld_aud_fifo_en | sram_ch->fld_aud_risc_en));
+
+        /* Clear data buffer memory */
+        if (dev->_audiodata_buf_virt_addr)
+                memset(dev->_audiodata_buf_virt_addr, 0,
+                       dev->_audiodata_buf_size);
+
+        dev->_audio_is_running = 0;
+        dev->_is_first_audio_frame = 0;
+        dev->_audioframe_count = 0;
+        dev->_audiofile_status = END_OF_FILE;
+
+        kfree(dev->_irq_audio_queues);
+        dev->_irq_audio_queues = NULL;
+
+        kfree(dev->_audiofilename);
+}
+
+void cx25821_free_mem_upstream_audio(struct cx25821_dev *dev)
+{
+        if (dev->_audio_is_running)
+                cx25821_stop_upstream_audio(dev);
+
+        cx25821_free_memory_audio(dev);
+}
+
+int cx25821_get_audio_data(struct cx25821_dev *dev,
+                           struct sram_channel *sram_ch)
+{
+        struct file *myfile;
+        int frame_index_temp = dev->_audioframe_index;
+        int i = 0;
+        int line_size = AUDIO_LINE_SIZE;
+        int frame_size = AUDIO_DATA_BUF_SZ;
+        int frame_offset = frame_size * frame_index_temp;
+        ssize_t vfs_read_retval = 0;
+        char mybuf[line_size];
+        loff_t file_offset = dev->_audioframe_count * frame_size;
+        loff_t pos;
+        mm_segment_t old_fs;
+
+        if (dev->_audiofile_status == END_OF_FILE)
+                return 0;
+
+        myfile = filp_open(dev->_audiofilename, O_RDONLY | O_LARGEFILE, 0);
+
+        if (IS_ERR(myfile)) {
+                const int open_errno = -PTR_ERR(myfile);
+                pr_err("%s(): ERROR opening file(%s) with errno = %d!\n",
+                       __func__, dev->_audiofilename, open_errno);
+                return PTR_ERR(myfile);
+        } else {
+                if (!(myfile->f_op)) {
+                        pr_err("%s(): File has no file operations registered!\n",
+                               __func__);
+                        filp_close(myfile, NULL);
+                        return -EIO;
+                }
+
+                if (!myfile->f_op->read) {
+                        pr_err("%s(): File has no READ operations registered!\n",
+                               __func__);
+                        filp_close(myfile, NULL);
+                        return -EIO;
+                }
+
+                pos = myfile->f_pos;
+                old_fs = get_fs();
+                set_fs(KERNEL_DS);
+
+                for (i = 0; i < dev->_audio_lines_count; i++) {
+                        pos = file_offset;
+
+                        vfs_read_retval =
+                            vfs_read(myfile, mybuf, line_size, &pos);
+
+                        if (vfs_read_retval > 0 && vfs_read_retval == line_size
+                            && dev->_audiodata_buf_virt_addr != NULL) {
+                                memcpy((void *)(dev->_audiodata_buf_virt_addr +
+                                                frame_offset / 4), mybuf,
+                                       vfs_read_retval);
+                        }
+
+                        file_offset += vfs_read_retval;
+                        frame_offset += vfs_read_retval;
+
+                        if (vfs_read_retval < line_size) {
+                                pr_info("Done: exit %s() since no more bytes to read from Audio file\n",
+                                        __func__);
+                                break;
+                        }
+                }
+
+                if (i > 0)
+                        dev->_audioframe_count++;
+
+                dev->_audiofile_status =
+                    (vfs_read_retval == line_size) ? IN_PROGRESS : END_OF_FILE;
+
+                set_fs(old_fs);
+                filp_close(myfile, NULL);
+        }
+
+        return 0;
+}
+
+static void cx25821_audioups_handler(struct work_struct *work)
+{
+        struct cx25821_dev *dev =
+            container_of(work, struct cx25821_dev, _audio_work_entry);
+
+        if (!dev) {
+                pr_err("ERROR %s(): since container_of(work_struct) FAILED!\n",
+                       __func__);
+                return;
+        }
+
+        cx25821_get_audio_data(dev, dev->channels[dev->_audio_upstream_channel].
+                        sram_channels);
+}
+
+int cx25821_openfile_audio(struct cx25821_dev *dev,
+                           struct sram_channel *sram_ch)
+{
+        struct file *myfile;
+        int i = 0, j = 0;
+        int line_size = AUDIO_LINE_SIZE;
+        ssize_t vfs_read_retval = 0;
+        char mybuf[line_size];
+        loff_t pos;
+        loff_t offset = (unsigned long)0;
+        mm_segment_t old_fs;
+
+        myfile = filp_open(dev->_audiofilename, O_RDONLY | O_LARGEFILE, 0);
+
+        if (IS_ERR(myfile)) {
+                const int open_errno = -PTR_ERR(myfile);
+                pr_err("%s(): ERROR opening file(%s) with errno = %d!\n",
+                       __func__, dev->_audiofilename, open_errno);
+                return PTR_ERR(myfile);
+        } else {
+                if (!(myfile->f_op)) {
+                        pr_err("%s(): File has no file operations registered!\n",
+                               __func__);
+                        filp_close(myfile, NULL);
+                        return -EIO;
+                }
+
+                if (!myfile->f_op->read) {
+                        pr_err("%s(): File has no READ operations registered!\n",
+                               __func__);
+                        filp_close(myfile, NULL);
+                        return -EIO;
+                }
+
+                pos = myfile->f_pos;
+                old_fs = get_fs();
+                set_fs(KERNEL_DS);
+
+                for (j = 0; j < NUM_AUDIO_FRAMES; j++) {
+                        for (i = 0; i < dev->_audio_lines_count; i++) {
+                                pos = offset;
+
+                                vfs_read_retval =
+                                    vfs_read(myfile, mybuf, line_size, &pos);
+
+                                if (vfs_read_retval > 0
+                                    && vfs_read_retval == line_size
+                                    && dev->_audiodata_buf_virt_addr != NULL) {
+                                        memcpy((void *)(dev->
+                                                        _audiodata_buf_virt_addr
+                                                        + offset / 4), mybuf,
+                                               vfs_read_retval);
+                                }
+
+                                offset += vfs_read_retval;
+
+                                if (vfs_read_retval < line_size) {
+                                        pr_info("Done: exit %s() since no more bytes to read from Audio file\n",
+                                                __func__);
+                                        break;
+                                }
+                        }
+
+                        if (i > 0)
+                                dev->_audioframe_count++;
+
+                        if (vfs_read_retval < line_size)
+                                break;
+                }
+
+                dev->_audiofile_status =
+                    (vfs_read_retval == line_size) ? IN_PROGRESS : END_OF_FILE;
+
+                set_fs(old_fs);
+                myfile->f_pos = 0;
+                filp_close(myfile, NULL);
+        }
+
+        return 0;
+}
+
+static int cx25821_audio_upstream_buffer_prepare(struct cx25821_dev *dev,
+                                                 struct sram_channel *sram_ch,
+                                                 int bpl)
+{
+        int ret = 0;
+        dma_addr_t dma_addr;
+        dma_addr_t data_dma_addr;
+
+        cx25821_free_memory_audio(dev);
+
+        dev->_risc_virt_addr =
+            pci_alloc_consistent(dev->pci, dev->audio_upstream_riscbuf_size,
+                                 &dma_addr);
+        dev->_risc_virt_start_addr = dev->_risc_virt_addr;
+        dev->_risc_phys_start_addr = dma_addr;
+        dev->_risc_phys_addr = dma_addr;
+        dev->_audiorisc_size = dev->audio_upstream_riscbuf_size;
+
+        if (!dev->_risc_virt_addr) {
+                printk(KERN_DEBUG
+                       pr_fmt("ERROR: pci_alloc_consistent() FAILED to allocate memory for RISC program! Returning\n"));
+                return -ENOMEM;
+        }
+        /* Clear out memory at address */
+        memset(dev->_risc_virt_addr, 0, dev->_audiorisc_size);
+
+        /* For Audio Data buffer allocation */
+        dev->_audiodata_buf_virt_addr =
+            pci_alloc_consistent(dev->pci, dev->audio_upstream_databuf_size,
+                                 &data_dma_addr);
+        dev->_audiodata_buf_phys_addr = data_dma_addr;
+        dev->_audiodata_buf_size = dev->audio_upstream_databuf_size;
+
+        if (!dev->_audiodata_buf_virt_addr) {
+                printk(KERN_DEBUG
+                       pr_fmt("ERROR: pci_alloc_consistent() FAILED to allocate memory for data buffer! Returning\n"));
+                return -ENOMEM;
+        }
+        /* Clear out memory at address */
+        memset(dev->_audiodata_buf_virt_addr, 0, dev->_audiodata_buf_size);
+
+        ret = cx25821_openfile_audio(dev, sram_ch);
+        if (ret < 0)
+                return ret;
+
+        /* Creating RISC programs */
+        ret =
+            cx25821_risc_buffer_upstream_audio(dev, dev->pci, bpl,
+                                               dev->_audio_lines_count);
+        if (ret < 0) {
+                printk(KERN_DEBUG
+                       pr_fmt("ERROR creating audio upstream RISC programs!\n"));
+                goto error;
+        }
+
+        return 0;
+
+error:
+        return ret;
+}
+
+int cx25821_audio_upstream_irq(struct cx25821_dev *dev, int chan_num,
+                               u32 status)
+{
+        int i = 0;
+        u32 int_msk_tmp;
+        struct sram_channel *channel = dev->channels[chan_num].sram_channels;
+        dma_addr_t risc_phys_jump_addr;
+        __le32 *rp;
+
+        if (status & FLD_AUD_SRC_RISCI1) {
+                /* Get interrupt_index of the program that interrupted */
+                u32 prog_cnt = cx_read(channel->gpcnt);
+
+                /* Since we've identified our IRQ, clear our bits from the
+                 * interrupt mask and interrupt status registers */
+                cx_write(channel->int_msk, 0);
+                cx_write(channel->int_stat, cx_read(channel->int_stat));
+
+                spin_lock(&dev->slock);
+
+                while (prog_cnt != dev->_last_index_irq) {
+                        /* Update _last_index_irq */
+                        if (dev->_last_index_irq < (NUMBER_OF_PROGRAMS - 1))
+                                dev->_last_index_irq++;
+                        else
+                                dev->_last_index_irq = 0;
+
+                        dev->_audioframe_index = dev->_last_index_irq;
+
+                        queue_work(dev->_irq_audio_queues,
+                                   &dev->_audio_work_entry);
+                }
+
+                if (dev->_is_first_audio_frame) {
+                        dev->_is_first_audio_frame = 0;
+
+                        if (dev->_risc_virt_start_addr != NULL) {
+                                risc_phys_jump_addr =
+                                    dev->_risc_phys_start_addr +
+                                    RISC_SYNC_INSTRUCTION_SIZE +
+                                    AUDIO_RISC_DMA_BUF_SIZE;
+
+                                rp = cx25821_risc_field_upstream_audio(dev,
+                                                dev->_risc_virt_start_addr + 1,
+                                                dev->_audiodata_buf_phys_addr,
+                                                AUDIO_LINE_SIZE, FIFO_DISABLE);
+
+                                if (USE_RISC_NOOP_AUDIO) {
+                                        for (i = 0; i < NUM_NO_OPS; i++) {
+                                                *(rp++) =
+                                                    cpu_to_le32(RISC_NOOP);
+                                        }
+                                }
+                                /* Jump to 2nd Audio Frame */
+                                *(rp++) = cpu_to_le32(RISC_JUMP | RISC_IRQ1 |
+                                                RISC_CNT_RESET);
+                                *(rp++) = cpu_to_le32(risc_phys_jump_addr);
+                                *(rp++) = cpu_to_le32(0);
+                        }
+                }
+
+                spin_unlock(&dev->slock);
+        } else {
+                if (status & FLD_AUD_SRC_OF)
+                        pr_warn("%s(): Audio Received Overflow Error Interrupt!\n",
+                                __func__);
+
+                if (status & FLD_AUD_SRC_SYNC)
+                        pr_warn("%s(): Audio Received Sync Error Interrupt!\n",
+                                __func__);
+
+                if (status & FLD_AUD_SRC_OPC_ERR)
+                        pr_warn("%s(): Audio Received OpCode Error Interrupt!\n",
+                                __func__);
+
+                /* Read and write back the interrupt status register to clear
+                 * our bits */
+                cx_write(channel->int_stat, cx_read(channel->int_stat));
+        }
+
+        if (dev->_audiofile_status == END_OF_FILE) {
+                pr_warn("EOF Channel Audio Framecount = %d\n",
+                        dev->_audioframe_count);
+                return -1;
+        }
+        /* ElSE, set the interrupt mask register, re-enable irq. */
+        int_msk_tmp = cx_read(channel->int_msk);
+        cx_write(channel->int_msk, int_msk_tmp |= _intr_msk);
+
+        return 0;
+}
+
+static irqreturn_t cx25821_upstream_irq_audio(int irq, void *dev_id)
+{
+        struct cx25821_dev *dev = dev_id;
+        u32 msk_stat, audio_status;
+        int handled = 0;
+        struct sram_channel *sram_ch;
+
+        if (!dev)
+                return -1;
+
+        sram_ch = dev->channels[dev->_audio_upstream_channel].sram_channels;
+
+        msk_stat = cx_read(sram_ch->int_mstat);
+        audio_status = cx_read(sram_ch->int_stat);
+
+        /* Only deal with our interrupt */
+        if (audio_status) {
+                handled = cx25821_audio_upstream_irq(dev,
+                                dev->_audio_upstream_channel, audio_status);
+        }
+
+        if (handled < 0)
+                cx25821_stop_upstream_audio(dev);
+        else
+                handled += handled;
+
+        return IRQ_RETVAL(handled);
+}
+
+static void cx25821_wait_fifo_enable(struct cx25821_dev *dev,
+                                     struct sram_channel *sram_ch)
+{
+        int count = 0;
+        u32 tmp;
+
+        do {
+                /* Wait 10 microsecond before checking to see if the FIFO is
+                 * turned ON. */
+                udelay(10);
+
+                tmp = cx_read(sram_ch->dma_ctl);
+
+                /* 10 millisecond timeout */
+                if (count++ > 1000) {
+                        pr_err("ERROR: %s() fifo is NOT turned on. Timeout!\n",
+                               __func__);
+                        return;
+                }
+
+        } while (!(tmp & sram_ch->fld_aud_fifo_en));
+
+}
+
+int cx25821_start_audio_dma_upstream(struct cx25821_dev *dev,
+                                     struct sram_channel *sram_ch)
+{
+        u32 tmp = 0;
+        int err = 0;
+
+        /* Set the physical start address of the RISC program in the initial
+         * program counter(IPC) member of the CMDS. */
+        cx_write(sram_ch->cmds_start + 0, dev->_risc_phys_addr);
+        /* Risc IPC High 64 bits 63-32 */
+        cx_write(sram_ch->cmds_start + 4, 0);
+
+        /* reset counter */
+        cx_write(sram_ch->gpcnt_ctl, 3);
+
+        /* Set the line length       (It looks like we do not need to set the
+         * line length) */
+        cx_write(sram_ch->aud_length, AUDIO_LINE_SIZE & FLD_AUD_DST_LN_LNGTH);
+
+        /* Set the input mode to 16-bit */
+        tmp = cx_read(sram_ch->aud_cfg);
+        tmp |=
+            FLD_AUD_SRC_ENABLE | FLD_AUD_DST_PK_MODE | FLD_AUD_CLK_ENABLE |
+            FLD_AUD_MASTER_MODE | FLD_AUD_CLK_SELECT_PLL_D | FLD_AUD_SONY_MODE;
+        cx_write(sram_ch->aud_cfg, tmp);
+
+        /* Read and write back the interrupt status register to clear it */
+        tmp = cx_read(sram_ch->int_stat);
+        cx_write(sram_ch->int_stat, tmp);
+
+        /* Clear our bits from the interrupt status register. */
+        cx_write(sram_ch->int_stat, _intr_msk);
+
+        /* Set the interrupt mask register, enable irq. */
+        cx_set(PCI_INT_MSK, cx_read(PCI_INT_MSK) | (1 << sram_ch->irq_bit));
+        tmp = cx_read(sram_ch->int_msk);
+        cx_write(sram_ch->int_msk, tmp |= _intr_msk);
+
+        err =
+            request_irq(dev->pci->irq, cx25821_upstream_irq_audio,
+                        IRQF_SHARED, dev->name, dev);
+        if (err < 0) {
+                pr_err("%s: can't get upstream IRQ %d\n",
+                       dev->name, dev->pci->irq);
+                goto fail_irq;
+        }
+
+        /* Start the DMA  engine */
+        tmp = cx_read(sram_ch->dma_ctl);
+        cx_set(sram_ch->dma_ctl, tmp | sram_ch->fld_aud_risc_en);
+
+        dev->_audio_is_running = 1;
+        dev->_is_first_audio_frame = 1;
+
+        /* The fifo_en bit turns on by the first Risc program */
+        cx25821_wait_fifo_enable(dev, sram_ch);
+
+        return 0;
+
+fail_irq:
+        cx25821_dev_unregister(dev);
+        return err;
+}
+
+int cx25821_audio_upstream_init(struct cx25821_dev *dev, int channel_select)
+{
+        struct sram_channel *sram_ch;
+        int retval = 0;
+        int err = 0;
+        int str_length = 0;
+
+        if (dev->_audio_is_running) {
+                pr_warn("Audio Channel is still running so return!\n");
+                return 0;
+        }
+
+        dev->_audio_upstream_channel = channel_select;
+        sram_ch = dev->channels[channel_select].sram_channels;
+
+        /* Work queue */
+        INIT_WORK(&dev->_audio_work_entry, cx25821_audioups_handler);
+        dev->_irq_audio_queues =
+            create_singlethread_workqueue("cx25821_audioworkqueue");
+
+        if (!dev->_irq_audio_queues) {
+                printk(KERN_DEBUG
+                       pr_fmt("ERROR: create_singlethread_workqueue() for Audio FAILED!\n"));
+                return -ENOMEM;
+        }
+
+        dev->_last_index_irq = 0;
+        dev->_audio_is_running = 0;
+        dev->_audioframe_count = 0;
+        dev->_audiofile_status = RESET_STATUS;
+        dev->_audio_lines_count = LINES_PER_AUDIO_BUFFER;
+        _line_size = AUDIO_LINE_SIZE;
+
+        if (dev->input_audiofilename) {
+                str_length = strlen(dev->input_audiofilename);
+                dev->_audiofilename = kmalloc(str_length + 1, GFP_KERNEL);
+
+                if (!dev->_audiofilename)
+                        goto error;
+
+                memcpy(dev->_audiofilename, dev->input_audiofilename,
+                       str_length + 1);
+
+                /* Default if filename is empty string */
+                if (strcmp(dev->input_audiofilename, "") == 0)
+                        dev->_audiofilename = "/root/audioGOOD.wav";
+        } else {
+                str_length = strlen(_defaultAudioName);
+                dev->_audiofilename = kmalloc(str_length + 1, GFP_KERNEL);
+
+                if (!dev->_audiofilename)
+                        goto error;
+
+                memcpy(dev->_audiofilename, _defaultAudioName, str_length + 1);
+        }
+
+        retval = cx25821_sram_channel_setup_upstream_audio(dev, sram_ch,
+                                                        _line_size, 0);
+
+        dev->audio_upstream_riscbuf_size =
+            AUDIO_RISC_DMA_BUF_SIZE * NUM_AUDIO_PROGS +
+            RISC_SYNC_INSTRUCTION_SIZE;
+        dev->audio_upstream_databuf_size = AUDIO_DATA_BUF_SZ * NUM_AUDIO_PROGS;
+
+        /* Allocating buffers and prepare RISC program */
+        retval = cx25821_audio_upstream_buffer_prepare(dev, sram_ch,
+                                                        _line_size);
+        if (retval < 0) {
+                pr_err("%s: Failed to set up Audio upstream buffers!\n",
+                       dev->name);
+                goto error;
+        }
+        /* Start RISC engine */
+        cx25821_start_audio_dma_upstream(dev, sram_ch);
+
+        return 0;
+
+error:
+        cx25821_dev_unregister(dev);
+
+        return err;
+}
diff --git a/drivers/media/video/cx25821/cx25821-audio-upstream.h b/drivers/media/video/cx25821/cx25821-audio-upstream.h
new file mode 100644
index 000000000000..af2ae7c5815a
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-audio-upstream.h
@@ -0,0 +1,62 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <hiep.huynh@conexant.com>, <shu.lin@conexant.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/mutex.h>
+#include <linux/workqueue.h>
+
+#define NUM_AUDIO_PROGS       8
+#define NUM_AUDIO_FRAMES      8
+#define END_OF_FILE           0
+#define IN_PROGRESS           1
+#define RESET_STATUS          -1
+#define FIFO_DISABLE          0
+#define FIFO_ENABLE           1
+#define NUM_NO_OPS            4
+
+#define RISC_READ_INSTRUCTION_SIZE      12
+#define RISC_JUMP_INSTRUCTION_SIZE      12
+#define RISC_WRITECR_INSTRUCTION_SIZE   16
+#define RISC_SYNC_INSTRUCTION_SIZE      4
+#define DWORD_SIZE                      4
+#define AUDIO_SYNC_LINE                 4
+
+#define LINES_PER_AUDIO_BUFFER      15
+#define AUDIO_LINE_SIZE             128
+#define AUDIO_DATA_BUF_SZ           (AUDIO_LINE_SIZE * LINES_PER_AUDIO_BUFFER)
+
+#define USE_RISC_NOOP_AUDIO   1
+
+#ifdef USE_RISC_NOOP_AUDIO
+#define AUDIO_RISC_DMA_BUF_SIZE                                         \
+        (LINES_PER_AUDIO_BUFFER * RISC_READ_INSTRUCTION_SIZE +          \
+         RISC_WRITECR_INSTRUCTION_SIZE + NUM_NO_OPS * DWORD_SIZE +      \
+         RISC_JUMP_INSTRUCTION_SIZE)
+#endif
+
+#ifndef USE_RISC_NOOP_AUDIO
+#define AUDIO_RISC_DMA_BUF_SIZE                                         \
+        (LINES_PER_AUDIO_BUFFER * RISC_READ_INSTRUCTION_SIZE +          \
+         RISC_WRITECR_INSTRUCTION_SIZE + RISC_JUMP_INSTRUCTION_SIZE)
+#endif
+
+static int _line_size;
+char *_defaultAudioName = "/root/audioGOOD.wav";
diff --git a/drivers/media/video/cx25821/cx25821-audio.h b/drivers/media/video/cx25821/cx25821-audio.h
new file mode 100644
index 000000000000..8eb55b7b88cb
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-audio.h
@@ -0,0 +1,61 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <shu.lin@conexant.com>, <hiep.huynh@conexant.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __CX25821_AUDIO_H__
+#define __CX25821_AUDIO_H__
+
+#define USE_RISC_NOOP               1
+#define LINES_PER_BUFFER            15
+#define AUDIO_LINE_SIZE             128
+
+/* Number of buffer programs to use at once. */
+#define NUMBER_OF_PROGRAMS  8
+
+/*
+ * Max size of the RISC program for a buffer. - worst case is 2 writes per line
+ * Space is also added for the 4 no-op instructions added on the end.
+ */
+#ifndef USE_RISC_NOOP
+#define MAX_BUFFER_PROGRAM_SIZE     \
+        (2 * LINES_PER_BUFFER * RISC_WRITE_INSTRUCTION_SIZE + \
+        RISC_WRITECR_INSTRUCTION_SIZE * 4)
+#endif
+
+/* MAE 12 July 2005 Try to use NOOP RISC instruction instead */
+#ifdef USE_RISC_NOOP
+#define MAX_BUFFER_PROGRAM_SIZE     \
+        (2 * LINES_PER_BUFFER * RISC_WRITE_INSTRUCTION_SIZE + \
+        RISC_NOOP_INSTRUCTION_SIZE * 4)
+#endif
+
+/* Sizes of various instructions in bytes.  Used when adding instructions. */
+#define RISC_WRITE_INSTRUCTION_SIZE 12
+#define RISC_JUMP_INSTRUCTION_SIZE  12
+#define RISC_SKIP_INSTRUCTION_SIZE  4
+#define RISC_SYNC_INSTRUCTION_SIZE  4
+#define RISC_WRITECR_INSTRUCTION_SIZE  16
+#define RISC_NOOP_INSTRUCTION_SIZE 4
+
+#define MAX_AUDIO_DMA_BUFFER_SIZE       \
+(MAX_BUFFER_PROGRAM_SIZE * NUMBER_OF_PROGRAMS + RISC_SYNC_INSTRUCTION_SIZE)
+
+#endif
diff --git a/drivers/media/video/cx25821/cx25821-biffuncs.h b/drivers/media/video/cx25821/cx25821-biffuncs.h
new file mode 100644
index 000000000000..9326a7c729ec
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-biffuncs.h
@@ -0,0 +1,45 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <shu.lin@conexant.com>, <hiep.huynh@conexant.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _BITFUNCS_H
+#define _BITFUNCS_H
+
+#define SetBit(Bit)  (1 << Bit)
+
+inline u8 getBit(u32 sample, u8 index)
+{
+        return (u8) ((sample >> index) & 1);
+}
+
+inline u32 clearBitAtPos(u32 value, u8 bit)
+{
+        return value & ~(1 << bit);
+}
+
+inline u32 setBitAtPos(u32 sample, u8 bit)
+{
+        sample |= (1 << bit);
+        return sample;
+
+}
+
+#endif
diff --git a/drivers/media/video/cx25821/cx25821-cards.c b/drivers/media/video/cx25821/cx25821-cards.c
new file mode 100644
index 000000000000..6ace60313b49
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-cards.c
@@ -0,0 +1,72 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <shu.lin@conexant.com>, <hiep.huynh@conexant.com>
+ *      Based on Steven Toth <stoth@linuxtv.org> cx23885 driver
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/delay.h>
+#include <media/cx25840.h>
+
+#include "cx25821.h"
+#include "tuner-xc2028.h"
+
+/* board config info */
+
+struct cx25821_board cx25821_boards[] = {
+        [UNKNOWN_BOARD] = {
+                .name = "UNKNOWN/GENERIC",
+                /* Ensure safe default for unknown boards */
+                .clk_freq = 0,
+        },
+
+        [CX25821_BOARD] = {
+                .name = "CX25821",
+                .portb = CX25821_RAW,
+                .portc = CX25821_264,
+                .input[0].type = CX25821_VMUX_COMPOSITE,
+        },
+
+};
+
+const unsigned int cx25821_bcount = ARRAY_SIZE(cx25821_boards);
+
+struct cx25821_subid cx25821_subids[] = {
+        {
+                .subvendor = 0x14f1,
+                .subdevice = 0x0920,
+                .card = CX25821_BOARD,
+        },
+};
+
+void cx25821_card_setup(struct cx25821_dev *dev)
+{
+        static u8 eeprom[256];
+
+        if (dev->i2c_bus[0].i2c_rc == 0) {
+                dev->i2c_bus[0].i2c_client.addr = 0xa0 >> 1;
+                tveeprom_read(&dev->i2c_bus[0].i2c_client, eeprom,
+                              sizeof(eeprom));
+        }
+}
diff --git a/drivers/media/video/cx25821/cx25821-core.c b/drivers/media/video/cx25821/cx25821-core.c
new file mode 100644
index 000000000000..a7fa38f9594e
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-core.c
@@ -0,0 +1,1517 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <shu.lin@conexant.com>, <hiep.huynh@conexant.com>
+ *  Based on Steven Toth <stoth@linuxtv.org> cx23885 driver
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include "cx25821.h"
+#include "cx25821-sram.h"
+#include "cx25821-video.h"
+
+MODULE_DESCRIPTION("Driver for Athena cards");
+MODULE_AUTHOR("Shu Lin - Hiep Huynh");
+MODULE_LICENSE("GPL");
+
+static unsigned int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "enable debug messages");
+
+static unsigned int card[] = {[0 ... (CX25821_MAXBOARDS - 1)] = UNSET };
+module_param_array(card, int, NULL, 0444);
+MODULE_PARM_DESC(card, "card type");
+
+static unsigned int cx25821_devcount;
+
+DEFINE_MUTEX(cx25821_devlist_mutex);
+EXPORT_SYMBOL(cx25821_devlist_mutex);
+LIST_HEAD(cx25821_devlist);
+EXPORT_SYMBOL(cx25821_devlist);
+
+struct sram_channel cx25821_sram_channels[] = {
+        [SRAM_CH00] = {
+                .i = SRAM_CH00,
+                .name = "VID A",
+                .cmds_start = VID_A_DOWN_CMDS,
+                .ctrl_start = VID_A_IQ,
+                .cdt = VID_A_CDT,
+                .fifo_start = VID_A_DOWN_CLUSTER_1,
+                .fifo_size = (VID_CLUSTER_SIZE << 2),
+                .ptr1_reg = DMA1_PTR1,
+                .ptr2_reg = DMA1_PTR2,
+                .cnt1_reg = DMA1_CNT1,
+                .cnt2_reg = DMA1_CNT2,
+                .int_msk = VID_A_INT_MSK,
+                .int_stat = VID_A_INT_STAT,
+                .int_mstat = VID_A_INT_MSTAT,
+                .dma_ctl = VID_DST_A_DMA_CTL,
+                .gpcnt_ctl = VID_DST_A_GPCNT_CTL,
+                .gpcnt = VID_DST_A_GPCNT,
+                .vip_ctl = VID_DST_A_VIP_CTL,
+                .pix_frmt = VID_DST_A_PIX_FRMT,
+        },
+
+        [SRAM_CH01] = {
+                .i = SRAM_CH01,
+                .name = "VID B",
+                .cmds_start = VID_B_DOWN_CMDS,
+                .ctrl_start = VID_B_IQ,
+                .cdt = VID_B_CDT,
+                .fifo_start = VID_B_DOWN_CLUSTER_1,
+                .fifo_size = (VID_CLUSTER_SIZE << 2),
+                .ptr1_reg = DMA2_PTR1,
+                .ptr2_reg = DMA2_PTR2,
+                .cnt1_reg = DMA2_CNT1,
+                .cnt2_reg = DMA2_CNT2,
+                .int_msk = VID_B_INT_MSK,
+                .int_stat = VID_B_INT_STAT,
+                .int_mstat = VID_B_INT_MSTAT,
+                .dma_ctl = VID_DST_B_DMA_CTL,
+                .gpcnt_ctl = VID_DST_B_GPCNT_CTL,
+                .gpcnt = VID_DST_B_GPCNT,
+                .vip_ctl = VID_DST_B_VIP_CTL,
+                .pix_frmt = VID_DST_B_PIX_FRMT,
+        },
+
+        [SRAM_CH02] = {
+                .i = SRAM_CH02,
+                .name = "VID C",
+                .cmds_start = VID_C_DOWN_CMDS,
+                .ctrl_start = VID_C_IQ,
+                .cdt = VID_C_CDT,
+                .fifo_start = VID_C_DOWN_CLUSTER_1,
+                .fifo_size = (VID_CLUSTER_SIZE << 2),
+                .ptr1_reg = DMA3_PTR1,
+                .ptr2_reg = DMA3_PTR2,
+                .cnt1_reg = DMA3_CNT1,
+                .cnt2_reg = DMA3_CNT2,
+                .int_msk = VID_C_INT_MSK,
+                .int_stat = VID_C_INT_STAT,
+                .int_mstat = VID_C_INT_MSTAT,
+                .dma_ctl = VID_DST_C_DMA_CTL,
+                .gpcnt_ctl = VID_DST_C_GPCNT_CTL,
+                .gpcnt = VID_DST_C_GPCNT,
+                .vip_ctl = VID_DST_C_VIP_CTL,
+                .pix_frmt = VID_DST_C_PIX_FRMT,
+        },
+
+        [SRAM_CH03] = {
+                .i = SRAM_CH03,
+                .name = "VID D",
+                .cmds_start = VID_D_DOWN_CMDS,
+                .ctrl_start = VID_D_IQ,
+                .cdt = VID_D_CDT,
+                .fifo_start = VID_D_DOWN_CLUSTER_1,
+                .fifo_size = (VID_CLUSTER_SIZE << 2),
+                .ptr1_reg = DMA4_PTR1,
+                .ptr2_reg = DMA4_PTR2,
+                .cnt1_reg = DMA4_CNT1,
+                .cnt2_reg = DMA4_CNT2,
+                .int_msk = VID_D_INT_MSK,
+                .int_stat = VID_D_INT_STAT,
+                .int_mstat = VID_D_INT_MSTAT,
+                .dma_ctl = VID_DST_D_DMA_CTL,
+                .gpcnt_ctl = VID_DST_D_GPCNT_CTL,
+                .gpcnt = VID_DST_D_GPCNT,
+                .vip_ctl = VID_DST_D_VIP_CTL,
+                .pix_frmt = VID_DST_D_PIX_FRMT,
+        },
+
+        [SRAM_CH04] = {
+                .i = SRAM_CH04,
+                .name = "VID E",
+                .cmds_start = VID_E_DOWN_CMDS,
+                .ctrl_start = VID_E_IQ,
+                .cdt = VID_E_CDT,
+                .fifo_start = VID_E_DOWN_CLUSTER_1,
+                .fifo_size = (VID_CLUSTER_SIZE << 2),
+                .ptr1_reg = DMA5_PTR1,
+                .ptr2_reg = DMA5_PTR2,
+                .cnt1_reg = DMA5_CNT1,
+                .cnt2_reg = DMA5_CNT2,
+                .int_msk = VID_E_INT_MSK,
+                .int_stat = VID_E_INT_STAT,
+                .int_mstat = VID_E_INT_MSTAT,
+                .dma_ctl = VID_DST_E_DMA_CTL,
+                .gpcnt_ctl = VID_DST_E_GPCNT_CTL,
+                .gpcnt = VID_DST_E_GPCNT,
+                .vip_ctl = VID_DST_E_VIP_CTL,
+                .pix_frmt = VID_DST_E_PIX_FRMT,
+        },
+
+        [SRAM_CH05] = {
+                .i = SRAM_CH05,
+                .name = "VID F",
+                .cmds_start = VID_F_DOWN_CMDS,
+                .ctrl_start = VID_F_IQ,
+                .cdt = VID_F_CDT,
+                .fifo_start = VID_F_DOWN_CLUSTER_1,
+                .fifo_size = (VID_CLUSTER_SIZE << 2),
+                .ptr1_reg = DMA6_PTR1,
+                .ptr2_reg = DMA6_PTR2,
+                .cnt1_reg = DMA6_CNT1,
+                .cnt2_reg = DMA6_CNT2,
+                .int_msk = VID_F_INT_MSK,
+                .int_stat = VID_F_INT_STAT,
+                .int_mstat = VID_F_INT_MSTAT,
+                .dma_ctl = VID_DST_F_DMA_CTL,
+                .gpcnt_ctl = VID_DST_F_GPCNT_CTL,
+                .gpcnt = VID_DST_F_GPCNT,
+                .vip_ctl = VID_DST_F_VIP_CTL,
+                .pix_frmt = VID_DST_F_PIX_FRMT,
+        },
+
+        [SRAM_CH06] = {
+                .i = SRAM_CH06,
+                .name = "VID G",
+                .cmds_start = VID_G_DOWN_CMDS,
+                .ctrl_start = VID_G_IQ,
+                .cdt = VID_G_CDT,
+                .fifo_start = VID_G_DOWN_CLUSTER_1,
+                .fifo_size = (VID_CLUSTER_SIZE << 2),
+                .ptr1_reg = DMA7_PTR1,
+                .ptr2_reg = DMA7_PTR2,
+                .cnt1_reg = DMA7_CNT1,
+                .cnt2_reg = DMA7_CNT2,
+                .int_msk = VID_G_INT_MSK,
+                .int_stat = VID_G_INT_STAT,
+                .int_mstat = VID_G_INT_MSTAT,
+                .dma_ctl = VID_DST_G_DMA_CTL,
+                .gpcnt_ctl = VID_DST_G_GPCNT_CTL,
+                .gpcnt = VID_DST_G_GPCNT,
+                .vip_ctl = VID_DST_G_VIP_CTL,
+                .pix_frmt = VID_DST_G_PIX_FRMT,
+        },
+
+        [SRAM_CH07] = {
+                .i = SRAM_CH07,
+                .name = "VID H",
+                .cmds_start = VID_H_DOWN_CMDS,
+                .ctrl_start = VID_H_IQ,
+                .cdt = VID_H_CDT,
+                .fifo_start = VID_H_DOWN_CLUSTER_1,
+                .fifo_size = (VID_CLUSTER_SIZE << 2),
+                .ptr1_reg = DMA8_PTR1,
+                .ptr2_reg = DMA8_PTR2,
+                .cnt1_reg = DMA8_CNT1,
+                .cnt2_reg = DMA8_CNT2,
+                .int_msk = VID_H_INT_MSK,
+                .int_stat = VID_H_INT_STAT,
+                .int_mstat = VID_H_INT_MSTAT,
+                .dma_ctl = VID_DST_H_DMA_CTL,
+                .gpcnt_ctl = VID_DST_H_GPCNT_CTL,
+                .gpcnt = VID_DST_H_GPCNT,
+                .vip_ctl = VID_DST_H_VIP_CTL,
+                .pix_frmt = VID_DST_H_PIX_FRMT,
+        },
+
+        [SRAM_CH08] = {
+                .name = "audio from",
+                .cmds_start = AUD_A_DOWN_CMDS,
+                .ctrl_start = AUD_A_IQ,
+                .cdt = AUD_A_CDT,
+                .fifo_start = AUD_A_DOWN_CLUSTER_1,
+                .fifo_size = AUDIO_CLUSTER_SIZE * 3,
+                .ptr1_reg = DMA17_PTR1,
+                .ptr2_reg = DMA17_PTR2,
+                .cnt1_reg = DMA17_CNT1,
+                .cnt2_reg = DMA17_CNT2,
+        },
+
+        [SRAM_CH09] = {
+                .i = SRAM_CH09,
+                .name = "VID Upstream I",
+                .cmds_start = VID_I_UP_CMDS,
+                .ctrl_start = VID_I_IQ,
+                .cdt = VID_I_CDT,
+                .fifo_start = VID_I_UP_CLUSTER_1,
+                .fifo_size = (VID_CLUSTER_SIZE << 2),
+                .ptr1_reg = DMA15_PTR1,
+                .ptr2_reg = DMA15_PTR2,
+                .cnt1_reg = DMA15_CNT1,
+                .cnt2_reg = DMA15_CNT2,
+                .int_msk = VID_I_INT_MSK,
+                .int_stat = VID_I_INT_STAT,
+                .int_mstat = VID_I_INT_MSTAT,
+                .dma_ctl = VID_SRC_I_DMA_CTL,
+                .gpcnt_ctl = VID_SRC_I_GPCNT_CTL,
+                .gpcnt = VID_SRC_I_GPCNT,
+
+                .vid_fmt_ctl = VID_SRC_I_FMT_CTL,
+                .vid_active_ctl1 = VID_SRC_I_ACTIVE_CTL1,
+                .vid_active_ctl2 = VID_SRC_I_ACTIVE_CTL2,
+                .vid_cdt_size = VID_SRC_I_CDT_SZ,
+                .irq_bit = 8,
+        },
+
+        [SRAM_CH10] = {
+                .i = SRAM_CH10,
+                .name = "VID Upstream J",
+                .cmds_start = VID_J_UP_CMDS,
+                .ctrl_start = VID_J_IQ,
+                .cdt = VID_J_CDT,
+                .fifo_start = VID_J_UP_CLUSTER_1,
+                .fifo_size = (VID_CLUSTER_SIZE << 2),
+                .ptr1_reg = DMA16_PTR1,
+                .ptr2_reg = DMA16_PTR2,
+                .cnt1_reg = DMA16_CNT1,
+                .cnt2_reg = DMA16_CNT2,
+                .int_msk = VID_J_INT_MSK,
+                .int_stat = VID_J_INT_STAT,
+                .int_mstat = VID_J_INT_MSTAT,
+                .dma_ctl = VID_SRC_J_DMA_CTL,
+                .gpcnt_ctl = VID_SRC_J_GPCNT_CTL,
+                .gpcnt = VID_SRC_J_GPCNT,
+
+                .vid_fmt_ctl = VID_SRC_J_FMT_CTL,
+                .vid_active_ctl1 = VID_SRC_J_ACTIVE_CTL1,
+                .vid_active_ctl2 = VID_SRC_J_ACTIVE_CTL2,
+                .vid_cdt_size = VID_SRC_J_CDT_SZ,
+                .irq_bit = 9,
+        },
+
+        [SRAM_CH11] = {
+                .i = SRAM_CH11,
+                .name = "Audio Upstream Channel B",
+                .cmds_start = AUD_B_UP_CMDS,
+                .ctrl_start = AUD_B_IQ,
+                .cdt = AUD_B_CDT,
+                .fifo_start = AUD_B_UP_CLUSTER_1,
+                .fifo_size = (AUDIO_CLUSTER_SIZE * 3),
+                .ptr1_reg = DMA22_PTR1,
+                .ptr2_reg = DMA22_PTR2,
+                .cnt1_reg = DMA22_CNT1,
+                .cnt2_reg = DMA22_CNT2,
+                .int_msk = AUD_B_INT_MSK,
+                .int_stat = AUD_B_INT_STAT,
+                .int_mstat = AUD_B_INT_MSTAT,
+                .dma_ctl = AUD_INT_DMA_CTL,
+                .gpcnt_ctl = AUD_B_GPCNT_CTL,
+                .gpcnt = AUD_B_GPCNT,
+                .aud_length = AUD_B_LNGTH,
+                .aud_cfg = AUD_B_CFG,
+                .fld_aud_fifo_en = FLD_AUD_SRC_B_FIFO_EN,
+                .fld_aud_risc_en = FLD_AUD_SRC_B_RISC_EN,
+                .irq_bit = 11,
+        },
+};
+EXPORT_SYMBOL(cx25821_sram_channels);
+
+struct sram_channel *channel0 = &cx25821_sram_channels[SRAM_CH00];
+struct sram_channel *channel1 = &cx25821_sram_channels[SRAM_CH01];
+struct sram_channel *channel2 = &cx25821_sram_channels[SRAM_CH02];
+struct sram_channel *channel3 = &cx25821_sram_channels[SRAM_CH03];
+struct sram_channel *channel4 = &cx25821_sram_channels[SRAM_CH04];
+struct sram_channel *channel5 = &cx25821_sram_channels[SRAM_CH05];
+struct sram_channel *channel6 = &cx25821_sram_channels[SRAM_CH06];
+struct sram_channel *channel7 = &cx25821_sram_channels[SRAM_CH07];
+struct sram_channel *channel9 = &cx25821_sram_channels[SRAM_CH09];
+struct sram_channel *channel10 = &cx25821_sram_channels[SRAM_CH10];
+struct sram_channel *channel11 = &cx25821_sram_channels[SRAM_CH11];
+
+struct cx25821_dmaqueue mpegq;
+
+static int cx25821_risc_decode(u32 risc)
+{
+        static const char * const instr[16] = {
+                [RISC_SYNC >> 28] = "sync",
+                [RISC_WRITE >> 28] = "write",
+                [RISC_WRITEC >> 28] = "writec",
+                [RISC_READ >> 28] = "read",
+                [RISC_READC >> 28] = "readc",
+                [RISC_JUMP >> 28] = "jump",
+                [RISC_SKIP >> 28] = "skip",
+                [RISC_WRITERM >> 28] = "writerm",
+                [RISC_WRITECM >> 28] = "writecm",
+                [RISC_WRITECR >> 28] = "writecr",
+        };
+        static const int incr[16] = {
+                [RISC_WRITE >> 28] = 3,
+                [RISC_JUMP >> 28] = 3,
+                [RISC_SKIP >> 28] = 1,
+                [RISC_SYNC >> 28] = 1,
+                [RISC_WRITERM >> 28] = 3,
+                [RISC_WRITECM >> 28] = 3,
+                [RISC_WRITECR >> 28] = 4,
+        };
+        static const char * const bits[] = {
+                "12", "13", "14", "resync",
+                "cnt0", "cnt1", "18", "19",
+                "20", "21", "22", "23",
+                "irq1", "irq2", "eol", "sol",
+        };
+        int i;
+
+        pr_cont("0x%08x [ %s",
+                risc, instr[risc >> 28] ? instr[risc >> 28] : "INVALID");
+        for (i = ARRAY_SIZE(bits) - 1; i >= 0; i--) {
+                if (risc & (1 << (i + 12)))
+                        pr_cont(" %s", bits[i]);
+        }
+        pr_cont(" count=%d ]\n", risc & 0xfff);
+        return incr[risc >> 28] ? incr[risc >> 28] : 1;
+}
+
+static inline int i2c_slave_did_ack(struct i2c_adapter *i2c_adap)
+{
+        struct cx25821_i2c *bus = i2c_adap->algo_data;
+        struct cx25821_dev *dev = bus->dev;
+        return cx_read(bus->reg_stat) & 0x01;
+}
+
+void cx_i2c_read_print(struct cx25821_dev *dev, u32 reg, const char *reg_string)
+{
+        int tmp = 0;
+        u32 value = 0;
+
+        value = cx25821_i2c_read(&dev->i2c_bus[0], reg, &tmp);
+}
+
+static void cx25821_registers_init(struct cx25821_dev *dev)
+{
+        u32 tmp;
+
+        /* enable RUN_RISC in Pecos */
+        cx_write(DEV_CNTRL2, 0x20);
+
+        /* Set the master PCI interrupt masks to enable video, audio, MBIF,
+         * and GPIO interrupts
+         * I2C interrupt masking is handled by the I2C objects themselves. */
+        cx_write(PCI_INT_MSK, 0x2001FFFF);
+
+        tmp = cx_read(RDR_TLCTL0);
+        tmp &= ~FLD_CFG_RCB_CK_EN;      /* Clear the RCB_CK_EN bit */
+        cx_write(RDR_TLCTL0, tmp);
+
+        /* PLL-A setting for the Audio Master Clock */
+        cx_write(PLL_A_INT_FRAC, 0x9807A58B);
+
+        /* PLL_A_POST = 0x1C, PLL_A_OUT_TO_PIN = 0x1 */
+        cx_write(PLL_A_POST_STAT_BIST, 0x8000019C);
+
+        /* clear reset bit [31] */
+        tmp = cx_read(PLL_A_INT_FRAC);
+        cx_write(PLL_A_INT_FRAC, tmp & 0x7FFFFFFF);
+
+        /* PLL-B setting for Mobilygen Host Bus Interface */
+        cx_write(PLL_B_INT_FRAC, 0x9883A86F);
+
+        /* PLL_B_POST = 0xD, PLL_B_OUT_TO_PIN = 0x0 */
+        cx_write(PLL_B_POST_STAT_BIST, 0x8000018D);
+
+        /* clear reset bit [31] */
+        tmp = cx_read(PLL_B_INT_FRAC);
+        cx_write(PLL_B_INT_FRAC, tmp & 0x7FFFFFFF);
+
+        /* PLL-C setting for video upstream channel */
+        cx_write(PLL_C_INT_FRAC, 0x96A0EA3F);
+
+        /* PLL_C_POST = 0x3, PLL_C_OUT_TO_PIN = 0x0 */
+        cx_write(PLL_C_POST_STAT_BIST, 0x80000103);
+
+        /* clear reset bit [31] */
+        tmp = cx_read(PLL_C_INT_FRAC);
+        cx_write(PLL_C_INT_FRAC, tmp & 0x7FFFFFFF);
+
+        /* PLL-D setting for audio upstream channel */
+        cx_write(PLL_D_INT_FRAC, 0x98757F5B);
+
+        /* PLL_D_POST = 0x13, PLL_D_OUT_TO_PIN = 0x0 */
+        cx_write(PLL_D_POST_STAT_BIST, 0x80000113);
+
+        /* clear reset bit [31] */
+        tmp = cx_read(PLL_D_INT_FRAC);
+        cx_write(PLL_D_INT_FRAC, tmp & 0x7FFFFFFF);
+
+        /* This selects the PLL C clock source for the video upstream channel
+         * I and J */
+        tmp = cx_read(VID_CH_CLK_SEL);
+        cx_write(VID_CH_CLK_SEL, (tmp & 0x00FFFFFF) | 0x24000000);
+
+        /* 656/VIP SRC Upstream Channel I & J and 7 - Host Bus Interface for
+         * channel A-C
+         * select 656/VIP DST for downstream Channel A - C */
+        tmp = cx_read(VID_CH_MODE_SEL);
+        /* cx_write( VID_CH_MODE_SEL, tmp | 0x1B0001FF); */
+        cx_write(VID_CH_MODE_SEL, tmp & 0xFFFFFE00);
+
+        /* enables 656 port I and J as output */
+        tmp = cx_read(CLK_RST);
+        /* use external ALT_PLL_REF pin as its reference clock instead */
+        tmp |= FLD_USE_ALT_PLL_REF;
+        cx_write(CLK_RST, tmp & ~(FLD_VID_I_CLK_NOE | FLD_VID_J_CLK_NOE));
+
+        mdelay(100);
+}
+
+int cx25821_sram_channel_setup(struct cx25821_dev *dev,
+                               struct sram_channel *ch,
+                               unsigned int bpl, u32 risc)
+{
+        unsigned int i, lines;
+        u32 cdt;
+
+        if (ch->cmds_start == 0) {
+                cx_write(ch->ptr1_reg, 0);
+                cx_write(ch->ptr2_reg, 0);
+                cx_write(ch->cnt2_reg, 0);
+                cx_write(ch->cnt1_reg, 0);
+                return 0;
+        }
+
+        bpl = (bpl + 7) & ~7;   /* alignment */
+        cdt = ch->cdt;
+        lines = ch->fifo_size / bpl;
+
+        if (lines > 4)
+                lines = 4;
+
+        BUG_ON(lines < 2);
+
+        cx_write(8 + 0, RISC_JUMP | RISC_IRQ1 | RISC_CNT_INC);
+        cx_write(8 + 4, 8);
+        cx_write(8 + 8, 0);
+
+        /* write CDT */
+        for (i = 0; i < lines; i++) {
+                cx_write(cdt + 16 * i, ch->fifo_start + bpl * i);
+                cx_write(cdt + 16 * i + 4, 0);
+                cx_write(cdt + 16 * i + 8, 0);
+                cx_write(cdt + 16 * i + 12, 0);
+        }
+
+        /* init the first cdt buffer */
+        for (i = 0; i < 128; i++)
+                cx_write(ch->fifo_start + 4 * i, i);
+
+        /* write CMDS */
+        if (ch->jumponly)
+                cx_write(ch->cmds_start + 0, 8);
+        else
+                cx_write(ch->cmds_start + 0, risc);
+
+        cx_write(ch->cmds_start + 4, 0);        /* 64 bits 63-32 */
+        cx_write(ch->cmds_start + 8, cdt);
+        cx_write(ch->cmds_start + 12, (lines * 16) >> 3);
+        cx_write(ch->cmds_start + 16, ch->ctrl_start);
+
+        if (ch->jumponly)
+                cx_write(ch->cmds_start + 20, 0x80000000 | (64 >> 2));
+        else
+                cx_write(ch->cmds_start + 20, 64 >> 2);
+
+        for (i = 24; i < 80; i += 4)
+                cx_write(ch->cmds_start + i, 0);
+
+        /* fill registers */
+        cx_write(ch->ptr1_reg, ch->fifo_start);
+        cx_write(ch->ptr2_reg, cdt);
+        cx_write(ch->cnt2_reg, (lines * 16) >> 3);
+        cx_write(ch->cnt1_reg, (bpl >> 3) - 1);
+
+        return 0;
+}
+EXPORT_SYMBOL(cx25821_sram_channel_setup);
+
+int cx25821_sram_channel_setup_audio(struct cx25821_dev *dev,
+                                     struct sram_channel *ch,
+                                     unsigned int bpl, u32 risc)
+{
+        unsigned int i, lines;
+        u32 cdt;
+
+        if (ch->cmds_start == 0) {
+                cx_write(ch->ptr1_reg, 0);
+                cx_write(ch->ptr2_reg, 0);
+                cx_write(ch->cnt2_reg, 0);
+                cx_write(ch->cnt1_reg, 0);
+                return 0;
+        }
+
+        bpl = (bpl + 7) & ~7;   /* alignment */
+        cdt = ch->cdt;
+        lines = ch->fifo_size / bpl;
+
+        if (lines > 3)
+                lines = 3;      /* for AUDIO */
+
+        BUG_ON(lines < 2);
+
+        cx_write(8 + 0, RISC_JUMP | RISC_IRQ1 | RISC_CNT_INC);
+        cx_write(8 + 4, 8);
+        cx_write(8 + 8, 0);
+
+        /* write CDT */
+        for (i = 0; i < lines; i++) {
+                cx_write(cdt + 16 * i, ch->fifo_start + bpl * i);
+                cx_write(cdt + 16 * i + 4, 0);
+                cx_write(cdt + 16 * i + 8, 0);
+                cx_write(cdt + 16 * i + 12, 0);
+        }
+
+        /* write CMDS */
+        if (ch->jumponly)
+                cx_write(ch->cmds_start + 0, 8);
+        else
+                cx_write(ch->cmds_start + 0, risc);
+
+        cx_write(ch->cmds_start + 4, 0);        /* 64 bits 63-32 */
+        cx_write(ch->cmds_start + 8, cdt);
+        cx_write(ch->cmds_start + 12, (lines * 16) >> 3);
+        cx_write(ch->cmds_start + 16, ch->ctrl_start);
+
+        /* IQ size */
+        if (ch->jumponly)
+                cx_write(ch->cmds_start + 20, 0x80000000 | (64 >> 2));
+        else
+                cx_write(ch->cmds_start + 20, 64 >> 2);
+
+        /* zero out */
+        for (i = 24; i < 80; i += 4)
+                cx_write(ch->cmds_start + i, 0);
+
+        /* fill registers */
+        cx_write(ch->ptr1_reg, ch->fifo_start);
+        cx_write(ch->ptr2_reg, cdt);
+        cx_write(ch->cnt2_reg, (lines * 16) >> 3);
+        cx_write(ch->cnt1_reg, (bpl >> 3) - 1);
+
+        return 0;
+}
+EXPORT_SYMBOL(cx25821_sram_channel_setup_audio);
+
+void cx25821_sram_channel_dump(struct cx25821_dev *dev, struct sram_channel *ch)
+{
+        static char *name[] = {
+                "init risc lo",
+                "init risc hi",
+                "cdt base",
+                "cdt size",
+                "iq base",
+                "iq size",
+                "risc pc lo",
+                "risc pc hi",
+                "iq wr ptr",
+                "iq rd ptr",
+                "cdt current",
+                "pci target lo",
+                "pci target hi",
+                "line / byte",
+        };
+        u32 risc;
+        unsigned int i, j, n;
+
+        pr_warn("%s: %s - dma channel status dump\n", dev->name, ch->name);
+        for (i = 0; i < ARRAY_SIZE(name); i++)
+                pr_warn("cmds + 0x%2x:   %-15s: 0x%08x\n",
+                        i * 4, name[i], cx_read(ch->cmds_start + 4 * i));
+
+        j = i * 4;
+        for (i = 0; i < 4;) {
+                risc = cx_read(ch->cmds_start + 4 * (i + 14));
+                pr_warn("cmds + 0x%2x:   risc%d: ", j + i * 4, i);
+                i += cx25821_risc_decode(risc);
+        }
+
+        for (i = 0; i < (64 >> 2); i += n) {
+                risc = cx_read(ch->ctrl_start + 4 * i);
+                /* No consideration for bits 63-32 */
+
+                pr_warn("ctrl + 0x%2x (0x%08x): iq %x: ",
+                        i * 4, ch->ctrl_start + 4 * i, i);
+                n = cx25821_risc_decode(risc);
+                for (j = 1; j < n; j++) {
+                        risc = cx_read(ch->ctrl_start + 4 * (i + j));
+                        pr_warn("ctrl + 0x%2x :   iq %x: 0x%08x [ arg #%d ]\n",
+                                4 * (i + j), i + j, risc, j);
+                }
+        }
+
+        pr_warn("        :   fifo: 0x%08x -> 0x%x\n",
+                ch->fifo_start, ch->fifo_start + ch->fifo_size);
+        pr_warn("        :   ctrl: 0x%08x -> 0x%x\n",
+                ch->ctrl_start, ch->ctrl_start + 6 * 16);
+        pr_warn("        :   ptr1_reg: 0x%08x\n",
+                cx_read(ch->ptr1_reg));
+        pr_warn("        :   ptr2_reg: 0x%08x\n",
+                cx_read(ch->ptr2_reg));
+        pr_warn("        :   cnt1_reg: 0x%08x\n",
+                cx_read(ch->cnt1_reg));
+        pr_warn("        :   cnt2_reg: 0x%08x\n",
+                cx_read(ch->cnt2_reg));
+}
+EXPORT_SYMBOL(cx25821_sram_channel_dump);
+
+void cx25821_sram_channel_dump_audio(struct cx25821_dev *dev,
+                                     struct sram_channel *ch)
+{
+        static const char * const name[] = {
+                "init risc lo",
+                "init risc hi",
+                "cdt base",
+                "cdt size",
+                "iq base",
+                "iq size",
+                "risc pc lo",
+                "risc pc hi",
+                "iq wr ptr",
+                "iq rd ptr",
+                "cdt current",
+                "pci target lo",
+                "pci target hi",
+                "line / byte",
+        };
+
+        u32 risc, value, tmp;
+        unsigned int i, j, n;
+
+        pr_info("\n%s: %s - dma Audio channel status dump\n",
+                dev->name, ch->name);
+
+        for (i = 0; i < ARRAY_SIZE(name); i++)
+                pr_info("%s: cmds + 0x%2x:   %-15s: 0x%08x\n",
+                        dev->name, i * 4, name[i],
+                        cx_read(ch->cmds_start + 4 * i));
+
+        j = i * 4;
+        for (i = 0; i < 4;) {
+                risc = cx_read(ch->cmds_start + 4 * (i + 14));
+                pr_warn("cmds + 0x%2x:   risc%d: ", j + i * 4, i);
+                i += cx25821_risc_decode(risc);
+        }
+
+        for (i = 0; i < (64 >> 2); i += n) {
+                risc = cx_read(ch->ctrl_start + 4 * i);
+                /* No consideration for bits 63-32 */
+
+                pr_warn("ctrl + 0x%2x (0x%08x): iq %x: ",
+                        i * 4, ch->ctrl_start + 4 * i, i);
+                n = cx25821_risc_decode(risc);
+
+                for (j = 1; j < n; j++) {
+                        risc = cx_read(ch->ctrl_start + 4 * (i + j));
+                        pr_warn("ctrl + 0x%2x :   iq %x: 0x%08x [ arg #%d ]\n",
+                                4 * (i + j), i + j, risc, j);
+                }
+        }
+
+        pr_warn("        :   fifo: 0x%08x -> 0x%x\n",
+                ch->fifo_start, ch->fifo_start + ch->fifo_size);
+        pr_warn("        :   ctrl: 0x%08x -> 0x%x\n",
+                ch->ctrl_start, ch->ctrl_start + 6 * 16);
+        pr_warn("        :   ptr1_reg: 0x%08x\n",
+                cx_read(ch->ptr1_reg));
+        pr_warn("        :   ptr2_reg: 0x%08x\n",
+                cx_read(ch->ptr2_reg));
+        pr_warn("        :   cnt1_reg: 0x%08x\n",
+                cx_read(ch->cnt1_reg));
+        pr_warn("        :   cnt2_reg: 0x%08x\n",
+                cx_read(ch->cnt2_reg));
+
+        for (i = 0; i < 4; i++) {
+                risc = cx_read(ch->cmds_start + 56 + (i * 4));
+                pr_warn("instruction %d = 0x%x\n", i, risc);
+        }
+
+        /* read data from the first cdt buffer */
+        risc = cx_read(AUD_A_CDT);
+        pr_warn("\nread cdt loc=0x%x\n", risc);
+        for (i = 0; i < 8; i++) {
+                n = cx_read(risc + i * 4);
+                pr_cont("0x%x ", n);
+        }
+        pr_cont("\n\n");
+
+        value = cx_read(CLK_RST);
+        CX25821_INFO(" CLK_RST = 0x%x\n\n", value);
+
+        value = cx_read(PLL_A_POST_STAT_BIST);
+        CX25821_INFO(" PLL_A_POST_STAT_BIST = 0x%x\n\n", value);
+        value = cx_read(PLL_A_INT_FRAC);
+        CX25821_INFO(" PLL_A_INT_FRAC = 0x%x\n\n", value);
+
+        value = cx_read(PLL_B_POST_STAT_BIST);
+        CX25821_INFO(" PLL_B_POST_STAT_BIST = 0x%x\n\n", value);
+        value = cx_read(PLL_B_INT_FRAC);
+        CX25821_INFO(" PLL_B_INT_FRAC = 0x%x\n\n", value);
+
+        value = cx_read(PLL_C_POST_STAT_BIST);
+        CX25821_INFO(" PLL_C_POST_STAT_BIST = 0x%x\n\n", value);
+        value = cx_read(PLL_C_INT_FRAC);
+        CX25821_INFO(" PLL_C_INT_FRAC = 0x%x\n\n", value);
+
+        value = cx_read(PLL_D_POST_STAT_BIST);
+        CX25821_INFO(" PLL_D_POST_STAT_BIST = 0x%x\n\n", value);
+        value = cx_read(PLL_D_INT_FRAC);
+        CX25821_INFO(" PLL_D_INT_FRAC = 0x%x\n\n", value);
+
+        value = cx25821_i2c_read(&dev->i2c_bus[0], AFE_AB_DIAG_CTRL, &tmp);
+        CX25821_INFO(" AFE_AB_DIAG_CTRL (0x10900090) = 0x%x\n\n", value);
+}
+EXPORT_SYMBOL(cx25821_sram_channel_dump_audio);
+
+static void cx25821_shutdown(struct cx25821_dev *dev)
+{
+        int i;
+
+        /* disable RISC controller */
+        cx_write(DEV_CNTRL2, 0);
+
+        /* Disable Video A/B activity */
+        for (i = 0; i < VID_CHANNEL_NUM; i++) {
+                cx_write(dev->channels[i].sram_channels->dma_ctl, 0);
+                cx_write(dev->channels[i].sram_channels->int_msk, 0);
+        }
+
+        for (i = VID_UPSTREAM_SRAM_CHANNEL_I;
+                i <= VID_UPSTREAM_SRAM_CHANNEL_J; i++) {
+                cx_write(dev->channels[i].sram_channels->dma_ctl, 0);
+                cx_write(dev->channels[i].sram_channels->int_msk, 0);
+        }
+
+        /* Disable Audio activity */
+        cx_write(AUD_INT_DMA_CTL, 0);
+
+        /* Disable Serial port */
+        cx_write(UART_CTL, 0);
+
+        /* Disable Interrupts */
+        cx_write(PCI_INT_MSK, 0);
+        cx_write(AUD_A_INT_MSK, 0);
+}
+
+void cx25821_set_pixel_format(struct cx25821_dev *dev, int channel_select,
+                              u32 format)
+{
+        if (channel_select <= 7 && channel_select >= 0) {
+                cx_write(dev->channels[channel_select].
+                        sram_channels->pix_frmt, format);
+                dev->channels[channel_select].pixel_formats = format;
+        }
+}
+
+static void cx25821_set_vip_mode(struct cx25821_dev *dev,
+                                 struct sram_channel *ch)
+{
+        cx_write(ch->pix_frmt, PIXEL_FRMT_422);
+        cx_write(ch->vip_ctl, PIXEL_ENGINE_VIP1);
+}
+
+static void cx25821_initialize(struct cx25821_dev *dev)
+{
+        int i;
+
+        dprintk(1, "%s()\n", __func__);
+
+        cx25821_shutdown(dev);
+        cx_write(PCI_INT_STAT, 0xffffffff);
+
+        for (i = 0; i < VID_CHANNEL_NUM; i++)
+                cx_write(dev->channels[i].sram_channels->int_stat, 0xffffffff);
+
+        cx_write(AUD_A_INT_STAT, 0xffffffff);
+        cx_write(AUD_B_INT_STAT, 0xffffffff);
+        cx_write(AUD_C_INT_STAT, 0xffffffff);
+        cx_write(AUD_D_INT_STAT, 0xffffffff);
+        cx_write(AUD_E_INT_STAT, 0xffffffff);
+
+        cx_write(CLK_DELAY, cx_read(CLK_DELAY) & 0x80000000);
+        cx_write(PAD_CTRL, 0x12);       /* for I2C */
+        cx25821_registers_init(dev);    /* init Pecos registers */
+        mdelay(100);
+
+        for (i = 0; i < VID_CHANNEL_NUM; i++) {
+                cx25821_set_vip_mode(dev, dev->channels[i].sram_channels);
+                cx25821_sram_channel_setup(dev, dev->channels[i].sram_channels,
+                                                1440, 0);
+                dev->channels[i].pixel_formats = PIXEL_FRMT_422;
+                dev->channels[i].use_cif_resolution = FALSE;
+        }
+
+        /* Probably only affect Downstream */
+        for (i = VID_UPSTREAM_SRAM_CHANNEL_I;
+                i <= VID_UPSTREAM_SRAM_CHANNEL_J; i++) {
+                cx25821_set_vip_mode(dev, dev->channels[i].sram_channels);
+        }
+
+        cx25821_sram_channel_setup_audio(dev,
+                                dev->channels[SRAM_CH08].sram_channels,
+                                128, 0);
+
+        cx25821_gpio_init(dev);
+}
+
+static int cx25821_get_resources(struct cx25821_dev *dev)
+{
+        if (request_mem_region
+            (pci_resource_start(dev->pci, 0), pci_resource_len(dev->pci, 0),
+             dev->name))
+                return 0;
+
+        pr_err("%s: can't get MMIO memory @ 0x%llx\n",
+               dev->name, (unsigned long long)pci_resource_start(dev->pci, 0));
+
+        return -EBUSY;
+}
+
+static void cx25821_dev_checkrevision(struct cx25821_dev *dev)
+{
+        dev->hwrevision = cx_read(RDR_CFG2) & 0xff;
+
+        pr_info("%s(): Hardware revision = 0x%02x\n",
+                __func__, dev->hwrevision);
+}
+
+static void cx25821_iounmap(struct cx25821_dev *dev)
+{
+        if (dev == NULL)
+                return;
+
+        /* Releasing IO memory */
+        if (dev->lmmio != NULL) {
+                CX25821_INFO("Releasing lmmio.\n");
+                iounmap(dev->lmmio);
+                dev->lmmio = NULL;
+        }
+}
+
+static int cx25821_dev_setup(struct cx25821_dev *dev)
+{
+        int io_size = 0, i;
+
+        pr_info("\n***********************************\n");
+        pr_info("cx25821 set up\n");
+        pr_info("***********************************\n\n");
+
+        mutex_init(&dev->lock);
+
+        atomic_inc(&dev->refcount);
+
+        dev->nr = ++cx25821_devcount;
+        sprintf(dev->name, "cx25821[%d]", dev->nr);
+
+        mutex_lock(&cx25821_devlist_mutex);
+        list_add_tail(&dev->devlist, &cx25821_devlist);
+        mutex_unlock(&cx25821_devlist_mutex);
+
+        strcpy(cx25821_boards[UNKNOWN_BOARD].name, "unknown");
+        strcpy(cx25821_boards[CX25821_BOARD].name, "cx25821");
+
+        if (dev->pci->device != 0x8210) {
+                pr_info("%s(): Exiting. Incorrect Hardware device = 0x%02x\n",
+                        __func__, dev->pci->device);
+                return -1;
+        } else {
+                pr_info("Athena Hardware device = 0x%02x\n", dev->pci->device);
+        }
+
+        /* Apply a sensible clock frequency for the PCIe bridge */
+        dev->clk_freq = 28000000;
+        for (i = 0; i < MAX_VID_CHANNEL_NUM; i++)
+                dev->channels[i].sram_channels = &cx25821_sram_channels[i];
+
+        if (dev->nr > 1)
+                CX25821_INFO("dev->nr > 1!");
+
+        /* board config */
+        dev->board = 1;         /* card[dev->nr]; */
+        dev->_max_num_decoders = MAX_DECODERS;
+
+        dev->pci_bus = dev->pci->bus->number;
+        dev->pci_slot = PCI_SLOT(dev->pci->devfn);
+        dev->pci_irqmask = 0x001f00;
+
+        /* External Master 1 Bus */
+        dev->i2c_bus[0].nr = 0;
+        dev->i2c_bus[0].dev = dev;
+        dev->i2c_bus[0].reg_stat = I2C1_STAT;
+        dev->i2c_bus[0].reg_ctrl = I2C1_CTRL;
+        dev->i2c_bus[0].reg_addr = I2C1_ADDR;
+        dev->i2c_bus[0].reg_rdata = I2C1_RDATA;
+        dev->i2c_bus[0].reg_wdata = I2C1_WDATA;
+        dev->i2c_bus[0].i2c_period = (0x07 << 24);      /* 1.95MHz */
+
+        if (cx25821_get_resources(dev) < 0) {
+                pr_err("%s: No more PCIe resources for subsystem: %04x:%04x\n",
+                       dev->name, dev->pci->subsystem_vendor,
+                       dev->pci->subsystem_device);
+
+                cx25821_devcount--;
+                return -EBUSY;
+        }
+
+        /* PCIe stuff */
+        dev->base_io_addr = pci_resource_start(dev->pci, 0);
+        io_size = pci_resource_len(dev->pci, 0);
+
+        if (!dev->base_io_addr) {
+                CX25821_ERR("No PCI Memory resources, exiting!\n");
+                return -ENODEV;
+        }
+
+        dev->lmmio = ioremap(dev->base_io_addr, pci_resource_len(dev->pci, 0));
+
+        if (!dev->lmmio) {
+                CX25821_ERR
+                    ("ioremap failed, maybe increasing __VMALLOC_RESERVE in page.h\n");
+                cx25821_iounmap(dev);
+                return -ENOMEM;
+        }
+
+        dev->bmmio = (u8 __iomem *) dev->lmmio;
+
+        pr_info("%s: subsystem: %04x:%04x, board: %s [card=%d,%s]\n",
+                dev->name, dev->pci->subsystem_vendor,
+                dev->pci->subsystem_device, cx25821_boards[dev->board].name,
+                dev->board, card[dev->nr] == dev->board ?
+                "insmod option" : "autodetected");
+
+        /* init hardware */
+        cx25821_initialize(dev);
+
+        cx25821_i2c_register(&dev->i2c_bus[0]);
+/*  cx25821_i2c_register(&dev->i2c_bus[1]);
+ *  cx25821_i2c_register(&dev->i2c_bus[2]); */
+
+        CX25821_INFO("i2c register! bus->i2c_rc = %d\n",
+                     dev->i2c_bus[0].i2c_rc);
+
+        cx25821_card_setup(dev);
+
+        if (medusa_video_init(dev) < 0)
+                CX25821_ERR("%s(): Failed to initialize medusa!\n", __func__);
+
+        cx25821_video_register(dev);
+
+        /* register IOCTL device */
+        dev->ioctl_dev =
+           cx25821_vdev_init(dev, dev->pci, &cx25821_videoioctl_template,
+                              "video");
+
+        if (video_register_device
+            (dev->ioctl_dev, VFL_TYPE_GRABBER, VIDEO_IOCTL_CH) < 0) {
+                cx25821_videoioctl_unregister(dev);
+                pr_err("%s(): Failed to register video adapter for IOCTL, so unregistering videoioctl device\n",
+                       __func__);
+        }
+
+        cx25821_dev_checkrevision(dev);
+        CX25821_INFO("setup done!\n");
+
+        return 0;
+}
+
+void cx25821_start_upstream_video_ch1(struct cx25821_dev *dev,
+                                      struct upstream_user_struct *up_data)
+{
+        dev->_isNTSC = !strcmp(dev->vid_stdname, "NTSC") ? 1 : 0;
+
+        dev->tvnorm = !dev->_isNTSC ? V4L2_STD_PAL_BG : V4L2_STD_NTSC_M;
+        medusa_set_videostandard(dev);
+
+        cx25821_vidupstream_init_ch1(dev, dev->channel_select,
+                                     dev->pixel_format);
+}
+
+void cx25821_start_upstream_video_ch2(struct cx25821_dev *dev,
+                                      struct upstream_user_struct *up_data)
+{
+        dev->_isNTSC_ch2 = !strcmp(dev->vid_stdname_ch2, "NTSC") ? 1 : 0;
+
+        dev->tvnorm = !dev->_isNTSC_ch2 ? V4L2_STD_PAL_BG : V4L2_STD_NTSC_M;
+        medusa_set_videostandard(dev);
+
+        cx25821_vidupstream_init_ch2(dev, dev->channel_select_ch2,
+                                     dev->pixel_format_ch2);
+}
+
+void cx25821_start_upstream_audio(struct cx25821_dev *dev,
+                                  struct upstream_user_struct *up_data)
+{
+        cx25821_audio_upstream_init(dev, AUDIO_UPSTREAM_SRAM_CHANNEL_B);
+}
+
+void cx25821_dev_unregister(struct cx25821_dev *dev)
+{
+        int i;
+
+        if (!dev->base_io_addr)
+                return;
+
+        cx25821_free_mem_upstream_ch1(dev);
+        cx25821_free_mem_upstream_ch2(dev);
+        cx25821_free_mem_upstream_audio(dev);
+
+        release_mem_region(dev->base_io_addr, pci_resource_len(dev->pci, 0));
+
+        if (!atomic_dec_and_test(&dev->refcount))
+                return;
+
+        for (i = 0; i < VID_CHANNEL_NUM; i++)
+                cx25821_video_unregister(dev, i);
+
+        for (i = VID_UPSTREAM_SRAM_CHANNEL_I;
+             i <= AUDIO_UPSTREAM_SRAM_CHANNEL_B; i++) {
+                cx25821_video_unregister(dev, i);
+        }
+
+        cx25821_videoioctl_unregister(dev);
+
+        cx25821_i2c_unregister(&dev->i2c_bus[0]);
+        cx25821_iounmap(dev);
+}
+EXPORT_SYMBOL(cx25821_dev_unregister);
+
+static __le32 *cx25821_risc_field(__le32 * rp, struct scatterlist *sglist,
+                                  unsigned int offset, u32 sync_line,
+                                  unsigned int bpl, unsigned int padding,
+                                  unsigned int lines)
+{
+        struct scatterlist *sg;
+        unsigned int line, todo;
+
+        /* sync instruction */
+        if (sync_line != NO_SYNC_LINE)
+                *(rp++) = cpu_to_le32(RISC_RESYNC | sync_line);
+
+        /* scan lines */
+        sg = sglist;
+        for (line = 0; line < lines; line++) {
+                while (offset && offset >= sg_dma_len(sg)) {
+                        offset -= sg_dma_len(sg);
+                        sg++;
+                }
+                if (bpl <= sg_dma_len(sg) - offset) {
+                        /* fits into current chunk */
+                        *(rp++) =
+                            cpu_to_le32(RISC_WRITE | RISC_SOL | RISC_EOL | bpl);
+                        *(rp++) = cpu_to_le32(sg_dma_address(sg) + offset);
+                        *(rp++) = cpu_to_le32(0);       /* bits 63-32 */
+                        offset += bpl;
+                } else {
+                        /* scanline needs to be split */
+                        todo = bpl;
+                        *(rp++) =
+                            cpu_to_le32(RISC_WRITE | RISC_SOL |
+                                        (sg_dma_len(sg) - offset));
+                        *(rp++) = cpu_to_le32(sg_dma_address(sg) + offset);
+                        *(rp++) = cpu_to_le32(0);       /* bits 63-32 */
+                        todo -= (sg_dma_len(sg) - offset);
+                        offset = 0;
+                        sg++;
+                        while (todo > sg_dma_len(sg)) {
+                                *(rp++) =
+                                    cpu_to_le32(RISC_WRITE | sg_dma_len(sg));
+                                *(rp++) = cpu_to_le32(sg_dma_address(sg));
+                                *(rp++) = cpu_to_le32(0);       /* bits 63-32 */
+                                todo -= sg_dma_len(sg);
+                                sg++;
+                        }
+                        *(rp++) = cpu_to_le32(RISC_WRITE | RISC_EOL | todo);
+                        *(rp++) = cpu_to_le32(sg_dma_address(sg));
+                        *(rp++) = cpu_to_le32(0);       /* bits 63-32 */
+                        offset += todo;
+                }
+
+                offset += padding;
+        }
+
+        return rp;
+}
+
+int cx25821_risc_buffer(struct pci_dev *pci, struct btcx_riscmem *risc,
+                        struct scatterlist *sglist, unsigned int top_offset,
+                        unsigned int bottom_offset, unsigned int bpl,
+                        unsigned int padding, unsigned int lines)
+{
+        u32 instructions;
+        u32 fields;
+        __le32 *rp;
+        int rc;
+
+        fields = 0;
+        if (UNSET != top_offset)
+                fields++;
+        if (UNSET != bottom_offset)
+                fields++;
+
+        /* estimate risc mem: worst case is one write per page border +
+           one write per scan line + syncs + jump (all 2 dwords).  Padding
+           can cause next bpl to start close to a page border.  First DMA
+           region may be smaller than PAGE_SIZE */
+        /* write and jump need and extra dword */
+        instructions =
+            fields * (1 + ((bpl + padding) * lines) / PAGE_SIZE + lines);
+        instructions += 2;
+        rc = btcx_riscmem_alloc(pci, risc, instructions * 12);
+
+        if (rc < 0)
+                return rc;
+
+        /* write risc instructions */
+        rp = risc->cpu;
+
+        if (UNSET != top_offset) {
+                rp = cx25821_risc_field(rp, sglist, top_offset, 0, bpl, padding,
+                                        lines);
+        }
+
+        if (UNSET != bottom_offset) {
+                rp = cx25821_risc_field(rp, sglist, bottom_offset, 0x200, bpl,
+                                        padding, lines);
+        }
+
+        /* save pointer to jmp instruction address */
+        risc->jmp = rp;
+        BUG_ON((risc->jmp - risc->cpu + 2) * sizeof(*risc->cpu) > risc->size);
+
+        return 0;
+}
+
+static __le32 *cx25821_risc_field_audio(__le32 * rp, struct scatterlist *sglist,
+                                        unsigned int offset, u32 sync_line,
+                                        unsigned int bpl, unsigned int padding,
+                                        unsigned int lines, unsigned int lpi)
+{
+        struct scatterlist *sg;
+        unsigned int line, todo, sol;
+
+        /* sync instruction */
+        if (sync_line != NO_SYNC_LINE)
+                *(rp++) = cpu_to_le32(RISC_RESYNC | sync_line);
+
+        /* scan lines */
+        sg = sglist;
+        for (line = 0; line < lines; line++) {
+                while (offset && offset >= sg_dma_len(sg)) {
+                        offset -= sg_dma_len(sg);
+                        sg++;
+                }
+
+                if (lpi && line > 0 && !(line % lpi))
+                        sol = RISC_SOL | RISC_IRQ1 | RISC_CNT_INC;
+                else
+                        sol = RISC_SOL;
+
+                if (bpl <= sg_dma_len(sg) - offset) {
+                        /* fits into current chunk */
+                        *(rp++) =
+                            cpu_to_le32(RISC_WRITE | sol | RISC_EOL | bpl);
+                        *(rp++) = cpu_to_le32(sg_dma_address(sg) + offset);
+                        *(rp++) = cpu_to_le32(0);       /* bits 63-32 */
+                        offset += bpl;
+                } else {
+                        /* scanline needs to be split */
+                        todo = bpl;
+                        *(rp++) = cpu_to_le32(RISC_WRITE | sol |
+                                              (sg_dma_len(sg) - offset));
+                        *(rp++) = cpu_to_le32(sg_dma_address(sg) + offset);
+                        *(rp++) = cpu_to_le32(0);       /* bits 63-32 */
+                        todo -= (sg_dma_len(sg) - offset);
+                        offset = 0;
+                        sg++;
+                        while (todo > sg_dma_len(sg)) {
+                                *(rp++) = cpu_to_le32(RISC_WRITE |
+                                                      sg_dma_len(sg));
+                                *(rp++) = cpu_to_le32(sg_dma_address(sg));
+                                *(rp++) = cpu_to_le32(0);       /* bits 63-32 */
+                                todo -= sg_dma_len(sg);
+                                sg++;
+                        }
+                        *(rp++) = cpu_to_le32(RISC_WRITE | RISC_EOL | todo);
+                        *(rp++) = cpu_to_le32(sg_dma_address(sg));
+                        *(rp++) = cpu_to_le32(0);       /* bits 63-32 */
+                        offset += todo;
+                }
+                offset += padding;
+        }
+
+        return rp;
+}
+
+int cx25821_risc_databuffer_audio(struct pci_dev *pci,
+                                  struct btcx_riscmem *risc,
+                                  struct scatterlist *sglist,
+                                  unsigned int bpl,
+                                  unsigned int lines, unsigned int lpi)
+{
+        u32 instructions;
+        __le32 *rp;
+        int rc;
+
+        /* estimate risc mem: worst case is one write per page border +
+           one write per scan line + syncs + jump (all 2 dwords).  Here
+           there is no padding and no sync.  First DMA region may be smaller
+           than PAGE_SIZE */
+        /* Jump and write need an extra dword */
+        instructions = 1 + (bpl * lines) / PAGE_SIZE + lines;
+        instructions += 1;
+
+        rc = btcx_riscmem_alloc(pci, risc, instructions * 12);
+        if (rc < 0)
+                return rc;
+
+        /* write risc instructions */
+        rp = risc->cpu;
+        rp = cx25821_risc_field_audio(rp, sglist, 0, NO_SYNC_LINE, bpl, 0,
+                                      lines, lpi);
+
+        /* save pointer to jmp instruction address */
+        risc->jmp = rp;
+        BUG_ON((risc->jmp - risc->cpu + 2) * sizeof(*risc->cpu) > risc->size);
+        return 0;
+}
+EXPORT_SYMBOL(cx25821_risc_databuffer_audio);
+
+int cx25821_risc_stopper(struct pci_dev *pci, struct btcx_riscmem *risc,
+                         u32 reg, u32 mask, u32 value)
+{
+        __le32 *rp;
+        int rc;
+
+        rc = btcx_riscmem_alloc(pci, risc, 4 * 16);
+
+        if (rc < 0)
+                return rc;
+
+        /* write risc instructions */
+        rp = risc->cpu;
+
+        *(rp++) = cpu_to_le32(RISC_WRITECR | RISC_IRQ1);
+        *(rp++) = cpu_to_le32(reg);
+        *(rp++) = cpu_to_le32(value);
+        *(rp++) = cpu_to_le32(mask);
+        *(rp++) = cpu_to_le32(RISC_JUMP);
+        *(rp++) = cpu_to_le32(risc->dma);
+        *(rp++) = cpu_to_le32(0);       /* bits 63-32 */
+        return 0;
+}
+
+void cx25821_free_buffer(struct videobuf_queue *q, struct cx25821_buffer *buf)
+{
+        struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
+
+        BUG_ON(in_interrupt());
+        videobuf_waiton(q, &buf->vb, 0, 0);
+        videobuf_dma_unmap(q->dev, dma);
+        videobuf_dma_free(dma);
+        btcx_riscmem_free(to_pci_dev(q->dev), &buf->risc);
+        buf->vb.state = VIDEOBUF_NEEDS_INIT;
+}
+
+static irqreturn_t cx25821_irq(int irq, void *dev_id)
+{
+        struct cx25821_dev *dev = dev_id;
+        u32 pci_status, pci_mask;
+        u32 vid_status;
+        int i, handled = 0;
+        u32 mask[8] = { 1, 2, 4, 8, 16, 32, 64, 128 };
+
+        pci_status = cx_read(PCI_INT_STAT);
+        pci_mask = cx_read(PCI_INT_MSK);
+
+        if (pci_status == 0)
+                goto out;
+
+        for (i = 0; i < VID_CHANNEL_NUM; i++) {
+                if (pci_status & mask[i]) {
+                        vid_status = cx_read(dev->channels[i].
+                                sram_channels->int_stat);
+
+                        if (vid_status)
+                                handled +=
+                                cx25821_video_irq(dev, i, vid_status);
+
+                        cx_write(PCI_INT_STAT, mask[i]);
+                }
+        }
+
+out:
+        return IRQ_RETVAL(handled);
+}
+
+void cx25821_print_irqbits(char *name, char *tag, char **strings,
+                           int len, u32 bits, u32 mask)
+{
+        unsigned int i;
+
+        printk(KERN_DEBUG pr_fmt("%s: %s [0x%x]"), name, tag, bits);
+
+        for (i = 0; i < len; i++) {
+                if (!(bits & (1 << i)))
+                        continue;
+                if (strings[i])
+                        pr_cont(" %s", strings[i]);
+                else
+                        pr_cont(" %d", i);
+                if (!(mask & (1 << i)))
+                        continue;
+                pr_cont("*");
+        }
+        pr_cont("\n");
+}
+EXPORT_SYMBOL(cx25821_print_irqbits);
+
+struct cx25821_dev *cx25821_dev_get(struct pci_dev *pci)
+{
+        struct cx25821_dev *dev = pci_get_drvdata(pci);
+        return dev;
+}
+EXPORT_SYMBOL(cx25821_dev_get);
+
+static int __devinit cx25821_initdev(struct pci_dev *pci_dev,
+                                     const struct pci_device_id *pci_id)
+{
+        struct cx25821_dev *dev;
+        int err = 0;
+
+        dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+        if (NULL == dev)
+                return -ENOMEM;
+
+        err = v4l2_device_register(&pci_dev->dev, &dev->v4l2_dev);
+        if (err < 0)
+                goto fail_free;
+
+        /* pci init */
+        dev->pci = pci_dev;
+        if (pci_enable_device(pci_dev)) {
+                err = -EIO;
+
+                pr_info("pci enable failed!\n");
+
+                goto fail_unregister_device;
+        }
+
+        pr_info("Athena pci enable !\n");
+
+        err = cx25821_dev_setup(dev);
+        if (err) {
+                if (err == -EBUSY)
+                        goto fail_unregister_device;
+                else
+                        goto fail_unregister_pci;
+        }
+
+        /* print pci info */
+        pci_read_config_byte(pci_dev, PCI_CLASS_REVISION, &dev->pci_rev);
+        pci_read_config_byte(pci_dev, PCI_LATENCY_TIMER, &dev->pci_lat);
+        pr_info("%s/0: found at %s, rev: %d, irq: %d, latency: %d, mmio: 0x%llx\n",
+                dev->name, pci_name(pci_dev), dev->pci_rev, pci_dev->irq,
+                dev->pci_lat, (unsigned long long)dev->base_io_addr);
+
+        pci_set_master(pci_dev);
+        if (!pci_dma_supported(pci_dev, 0xffffffff)) {
+                pr_err("%s/0: Oops: no 32bit PCI DMA ???\n", dev->name);
+                err = -EIO;
+                goto fail_irq;
+        }
+
+        err =
+            request_irq(pci_dev->irq, cx25821_irq, IRQF_SHARED,
+                        dev->name, dev);
+
+        if (err < 0) {
+                pr_err("%s: can't get IRQ %d\n", dev->name, pci_dev->irq);
+                goto fail_irq;
+        }
+
+        return 0;
+
+fail_irq:
+        pr_info("cx25821_initdev() can't get IRQ !\n");
+        cx25821_dev_unregister(dev);
+
+fail_unregister_pci:
+        pci_disable_device(pci_dev);
+fail_unregister_device:
+        v4l2_device_unregister(&dev->v4l2_dev);
+
+fail_free:
+        kfree(dev);
+        return err;
+}
+
+static void __devexit cx25821_finidev(struct pci_dev *pci_dev)
+{
+        struct v4l2_device *v4l2_dev = pci_get_drvdata(pci_dev);
+        struct cx25821_dev *dev = get_cx25821(v4l2_dev);
+
+        cx25821_shutdown(dev);
+        pci_disable_device(pci_dev);
+
+        /* unregister stuff */
+        if (pci_dev->irq)
+                free_irq(pci_dev->irq, dev);
+
+        mutex_lock(&cx25821_devlist_mutex);
+        list_del(&dev->devlist);
+        mutex_unlock(&cx25821_devlist_mutex);
+
+        cx25821_dev_unregister(dev);
+        v4l2_device_unregister(v4l2_dev);
+        kfree(dev);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(cx25821_pci_tbl) = {
+        {
+                /* CX25821 Athena */
+                .vendor = 0x14f1,
+                .device = 0x8210,
+                .subvendor = 0x14f1,
+                .subdevice = 0x0920,
+        },
+        {
+                /* --- end of list --- */
+        }
+};
+
+MODULE_DEVICE_TABLE(pci, cx25821_pci_tbl);
+
+static struct pci_driver cx25821_pci_driver = {
+        .name = "cx25821",
+        .id_table = cx25821_pci_tbl,
+        .probe = cx25821_initdev,
+        .remove = __devexit_p(cx25821_finidev),
+        /* TODO */
+        .suspend = NULL,
+        .resume = NULL,
+};
+
+static int __init cx25821_init(void)
+{
+        pr_info("driver version %d.%d.%d loaded\n",
+                (CX25821_VERSION_CODE >> 16) & 0xff,
+                (CX25821_VERSION_CODE >> 8) & 0xff,
+                CX25821_VERSION_CODE & 0xff);
+        return pci_register_driver(&cx25821_pci_driver);
+}
+
+static void __exit cx25821_fini(void)
+{
+        pci_unregister_driver(&cx25821_pci_driver);
+}
+
+
+module_init(cx25821_init);
+module_exit(cx25821_fini);
diff --git a/drivers/media/video/cx25821/cx25821-gpio.c b/drivers/media/video/cx25821/cx25821-gpio.c
new file mode 100644
index 000000000000..29e43b03c85e
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-gpio.c
@@ -0,0 +1,98 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <shu.lin@conexant.com>, <hiep.huynh@conexant.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include "cx25821.h"
+
+/********************* GPIO stuffs *********************/
+void cx25821_set_gpiopin_direction(struct cx25821_dev *dev,
+                                   int pin_number, int pin_logic_value)
+{
+        int bit = pin_number;
+        u32 gpio_oe_reg = GPIO_LO_OE;
+        u32 gpio_register = 0;
+        u32 value = 0;
+
+        /* Check for valid pinNumber */
+        if (pin_number >= 47)
+                return;
+
+        if (pin_number > 31) {
+                bit = pin_number - 31;
+                gpio_oe_reg = GPIO_HI_OE;
+        }
+        /* Here we will make sure that the GPIOs 0 and 1 are output. keep the
+         * rest as is */
+        gpio_register = cx_read(gpio_oe_reg);
+
+        if (pin_logic_value == 1)
+                value = gpio_register | Set_GPIO_Bit(bit);
+        else
+                value = gpio_register & Clear_GPIO_Bit(bit);
+
+        cx_write(gpio_oe_reg, value);
+}
+EXPORT_SYMBOL(cx25821_set_gpiopin_direction);
+
+static void cx25821_set_gpiopin_logicvalue(struct cx25821_dev *dev,
+                                           int pin_number, int pin_logic_value)
+{
+        int bit = pin_number;
+        u32 gpio_reg = GPIO_LO;
+        u32 value = 0;
+
+        /* Check for valid pinNumber */
+        if (pin_number >= 47)
+                return;
+
+        /* change to output direction */
+        cx25821_set_gpiopin_direction(dev, pin_number, 0);
+
+        if (pin_number > 31) {
+                bit = pin_number - 31;
+                gpio_reg = GPIO_HI;
+        }
+
+        value = cx_read(gpio_reg);
+
+        if (pin_logic_value == 0)
+                value &= Clear_GPIO_Bit(bit);
+        else
+                value |= Set_GPIO_Bit(bit);
+
+        cx_write(gpio_reg, value);
+}
+
+void cx25821_gpio_init(struct cx25821_dev *dev)
+{
+        if (dev == NULL)
+                return;
+
+        switch (dev->board) {
+        case CX25821_BOARD_CONEXANT_ATHENA10:
+        default:
+                /* set GPIO 5 to select the path for Medusa/Athena */
+                cx25821_set_gpiopin_logicvalue(dev, 5, 1);
+                mdelay(20);
+                break;
+        }
+
+}
diff --git a/drivers/media/video/cx25821/cx25821-i2c.c b/drivers/media/video/cx25821/cx25821-i2c.c
new file mode 100644
index 000000000000..4d3d0ce40785
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-i2c.c
@@ -0,0 +1,425 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <shu.lin@conexant.com>, <hiep.huynh@conexant.com>
+ *      Based on Steven Toth <stoth@linuxtv.org> cx23885 driver
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include "cx25821.h"
+#include <linux/i2c.h>
+
+static unsigned int i2c_debug;
+module_param(i2c_debug, int, 0644);
+MODULE_PARM_DESC(i2c_debug, "enable debug messages [i2c]");
+
+static unsigned int i2c_scan;
+module_param(i2c_scan, int, 0444);
+MODULE_PARM_DESC(i2c_scan, "scan i2c bus at insmod time");
+
+#define dprintk(level, fmt, arg...)                                     \
+do {                                                                    \
+        if (i2c_debug >= level)                                         \
+                printk(KERN_DEBUG "%s/0: " fmt, dev->name, ##arg);      \
+} while (0)
+
+#define I2C_WAIT_DELAY 32
+#define I2C_WAIT_RETRY 64
+
+#define I2C_EXTEND  (1 << 3)
+#define I2C_NOSTOP  (1 << 4)
+
+static inline int i2c_slave_did_ack(struct i2c_adapter *i2c_adap)
+{
+        struct cx25821_i2c *bus = i2c_adap->algo_data;
+        struct cx25821_dev *dev = bus->dev;
+        return cx_read(bus->reg_stat) & 0x01;
+}
+
+static inline int i2c_is_busy(struct i2c_adapter *i2c_adap)
+{
+        struct cx25821_i2c *bus = i2c_adap->algo_data;
+        struct cx25821_dev *dev = bus->dev;
+        return cx_read(bus->reg_stat) & 0x02 ? 1 : 0;
+}
+
+static int i2c_wait_done(struct i2c_adapter *i2c_adap)
+{
+        int count;
+
+        for (count = 0; count < I2C_WAIT_RETRY; count++) {
+                if (!i2c_is_busy(i2c_adap))
+                        break;
+                udelay(I2C_WAIT_DELAY);
+        }
+
+        if (I2C_WAIT_RETRY == count)
+                return 0;
+
+        return 1;
+}
+
+static int i2c_sendbytes(struct i2c_adapter *i2c_adap,
+                         const struct i2c_msg *msg, int joined_rlen)
+{
+        struct cx25821_i2c *bus = i2c_adap->algo_data;
+        struct cx25821_dev *dev = bus->dev;
+        u32 wdata, addr, ctrl;
+        int retval, cnt;
+
+        if (joined_rlen)
+                dprintk(1, "%s(msg->wlen=%d, nextmsg->rlen=%d)\n", __func__,
+                        msg->len, joined_rlen);
+        else
+                dprintk(1, "%s(msg->len=%d)\n", __func__, msg->len);
+
+        /* Deal with i2c probe functions with zero payload */
+        if (msg->len == 0) {
+                cx_write(bus->reg_addr, msg->addr << 25);
+                cx_write(bus->reg_ctrl, bus->i2c_period | (1 << 2));
+
+                if (!i2c_wait_done(i2c_adap))
+                        return -EIO;
+
+                if (!i2c_slave_did_ack(i2c_adap))
+                        return -EIO;
+
+                dprintk(1, "%s(): returns 0\n", __func__);
+                return 0;
+        }
+
+        /* dev, reg + first byte */
+        addr = (msg->addr << 25) | msg->buf[0];
+        wdata = msg->buf[0];
+
+        ctrl = bus->i2c_period | (1 << 12) | (1 << 2);
+
+        if (msg->len > 1)
+                ctrl |= I2C_NOSTOP | I2C_EXTEND;
+        else if (joined_rlen)
+                ctrl |= I2C_NOSTOP;
+
+        cx_write(bus->reg_addr, addr);
+        cx_write(bus->reg_wdata, wdata);
+        cx_write(bus->reg_ctrl, ctrl);
+
+        retval = i2c_wait_done(i2c_adap);
+        if (retval < 0)
+                goto err;
+
+        if (retval == 0)
+                goto eio;
+
+        if (i2c_debug) {
+                if (!(ctrl & I2C_NOSTOP))
+                        printk(" >\n");
+        }
+
+        for (cnt = 1; cnt < msg->len; cnt++) {
+                /* following bytes */
+                wdata = msg->buf[cnt];
+                ctrl = bus->i2c_period | (1 << 12) | (1 << 2);
+
+                if (cnt < msg->len - 1)
+                        ctrl |= I2C_NOSTOP | I2C_EXTEND;
+                else if (joined_rlen)
+                        ctrl |= I2C_NOSTOP;
+
+                cx_write(bus->reg_addr, addr);
+                cx_write(bus->reg_wdata, wdata);
+                cx_write(bus->reg_ctrl, ctrl);
+
+                retval = i2c_wait_done(i2c_adap);
+                if (retval < 0)
+                        goto err;
+
+                if (retval == 0)
+                        goto eio;
+
+                if (i2c_debug) {
+                        dprintk(1, " %02x", msg->buf[cnt]);
+                        if (!(ctrl & I2C_NOSTOP))
+                                dprintk(1, " >\n");
+                }
+        }
+
+        return msg->len;
+
+eio:
+        retval = -EIO;
+err:
+        if (i2c_debug)
+                pr_err(" ERR: %d\n", retval);
+        return retval;
+}
+
+static int i2c_readbytes(struct i2c_adapter *i2c_adap,
+                         const struct i2c_msg *msg, int joined)
+{
+        struct cx25821_i2c *bus = i2c_adap->algo_data;
+        struct cx25821_dev *dev = bus->dev;
+        u32 ctrl, cnt;
+        int retval;
+
+        if (i2c_debug && !joined)
+                dprintk(1, "6-%s(msg->len=%d)\n", __func__, msg->len);
+
+        /* Deal with i2c probe functions with zero payload */
+        if (msg->len == 0) {
+                cx_write(bus->reg_addr, msg->addr << 25);
+                cx_write(bus->reg_ctrl, bus->i2c_period | (1 << 2) | 1);
+                if (!i2c_wait_done(i2c_adap))
+                        return -EIO;
+                if (!i2c_slave_did_ack(i2c_adap))
+                        return -EIO;
+
+                dprintk(1, "%s(): returns 0\n", __func__);
+                return 0;
+        }
+
+        if (i2c_debug) {
+                if (joined)
+                        dprintk(1, " R");
+                else
+                        dprintk(1, " <R %02x", (msg->addr << 1) + 1);
+        }
+
+        for (cnt = 0; cnt < msg->len; cnt++) {
+
+                ctrl = bus->i2c_period | (1 << 12) | (1 << 2) | 1;
+
+                if (cnt < msg->len - 1)
+                        ctrl |= I2C_NOSTOP | I2C_EXTEND;
+
+                cx_write(bus->reg_addr, msg->addr << 25);
+                cx_write(bus->reg_ctrl, ctrl);
+
+                retval = i2c_wait_done(i2c_adap);
+                if (retval < 0)
+                        goto err;
+                if (retval == 0)
+                        goto eio;
+                msg->buf[cnt] = cx_read(bus->reg_rdata) & 0xff;
+
+                if (i2c_debug) {
+                        dprintk(1, " %02x", msg->buf[cnt]);
+                        if (!(ctrl & I2C_NOSTOP))
+                                dprintk(1, " >\n");
+                }
+        }
+
+        return msg->len;
+eio:
+        retval = -EIO;
+err:
+        if (i2c_debug)
+                pr_err(" ERR: %d\n", retval);
+        return retval;
+}
+
+static int i2c_xfer(struct i2c_adapter *i2c_adap, struct i2c_msg *msgs, int num)
+{
+        struct cx25821_i2c *bus = i2c_adap->algo_data;
+        struct cx25821_dev *dev = bus->dev;
+        int i, retval = 0;
+
+        dprintk(1, "%s(num = %d)\n", __func__, num);
+
+        for (i = 0; i < num; i++) {
+                dprintk(1, "%s(num = %d) addr = 0x%02x  len = 0x%x\n",
+                        __func__, num, msgs[i].addr, msgs[i].len);
+
+                if (msgs[i].flags & I2C_M_RD) {
+                        /* read */
+                        retval = i2c_readbytes(i2c_adap, &msgs[i], 0);
+                } else if (i + 1 < num && (msgs[i + 1].flags & I2C_M_RD) &&
+                           msgs[i].addr == msgs[i + 1].addr) {
+                        /* write then read from same address */
+                        retval =
+                            i2c_sendbytes(i2c_adap, &msgs[i], msgs[i + 1].len);
+
+                        if (retval < 0)
+                                goto err;
+                        i++;
+                        retval = i2c_readbytes(i2c_adap, &msgs[i], 1);
+                } else {
+                        /* write */
+                        retval = i2c_sendbytes(i2c_adap, &msgs[i], 0);
+                }
+
+                if (retval < 0)
+                        goto err;
+        }
+        return num;
+
+err:
+        return retval;
+}
+
+
+static u32 cx25821_functionality(struct i2c_adapter *adap)
+{
+        return I2C_FUNC_SMBUS_EMUL |
+            I2C_FUNC_I2C |
+            I2C_FUNC_SMBUS_WORD_DATA |
+            I2C_FUNC_SMBUS_READ_WORD_DATA | I2C_FUNC_SMBUS_WRITE_WORD_DATA;
+}
+
+static struct i2c_algorithm cx25821_i2c_algo_template = {
+        .master_xfer = i2c_xfer,
+        .functionality = cx25821_functionality,
+#ifdef NEED_ALGO_CONTROL
+        .algo_control = dummy_algo_control,
+#endif
+};
+
+static struct i2c_adapter cx25821_i2c_adap_template = {
+        .name = "cx25821",
+        .owner = THIS_MODULE,
+        .algo = &cx25821_i2c_algo_template,
+};
+
+static struct i2c_client cx25821_i2c_client_template = {
+        .name = "cx25821 internal",
+};
+
+/* init + register i2c algo-bit adapter */
+int cx25821_i2c_register(struct cx25821_i2c *bus)
+{
+        struct cx25821_dev *dev = bus->dev;
+
+        dprintk(1, "%s(bus = %d)\n", __func__, bus->nr);
+
+        memcpy(&bus->i2c_adap, &cx25821_i2c_adap_template,
+               sizeof(bus->i2c_adap));
+        memcpy(&bus->i2c_algo, &cx25821_i2c_algo_template,
+               sizeof(bus->i2c_algo));
+        memcpy(&bus->i2c_client, &cx25821_i2c_client_template,
+               sizeof(bus->i2c_client));
+
+        bus->i2c_adap.dev.parent = &dev->pci->dev;
+
+        strlcpy(bus->i2c_adap.name, bus->dev->name, sizeof(bus->i2c_adap.name));
+
+        bus->i2c_algo.data = bus;
+        bus->i2c_adap.algo_data = bus;
+        i2c_set_adapdata(&bus->i2c_adap, &dev->v4l2_dev);
+        i2c_add_adapter(&bus->i2c_adap);
+
+        bus->i2c_client.adapter = &bus->i2c_adap;
+
+        /* set up the I2c */
+        bus->i2c_client.addr = (0x88 >> 1);
+
+        return bus->i2c_rc;
+}
+
+int cx25821_i2c_unregister(struct cx25821_i2c *bus)
+{
+        i2c_del_adapter(&bus->i2c_adap);
+        return 0;
+}
+
+void cx25821_av_clk(struct cx25821_dev *dev, int enable)
+{
+        /* write 0 to bus 2 addr 0x144 via i2x_xfer() */
+        char buffer[3];
+        struct i2c_msg msg;
+        dprintk(1, "%s(enabled = %d)\n", __func__, enable);
+
+        /* Register 0x144 */
+        buffer[0] = 0x01;
+        buffer[1] = 0x44;
+        if (enable == 1)
+                buffer[2] = 0x05;
+        else
+                buffer[2] = 0x00;
+
+        msg.addr = 0x44;
+        msg.flags = I2C_M_TEN;
+        msg.len = 3;
+        msg.buf = buffer;
+
+        i2c_xfer(&dev->i2c_bus[0].i2c_adap, &msg, 1);
+}
+
+int cx25821_i2c_read(struct cx25821_i2c *bus, u16 reg_addr, int *value)
+{
+        struct i2c_client *client = &bus->i2c_client;
+        int retval = 0;
+        int v = 0;
+        u8 addr[2] = { 0, 0 };
+        u8 buf[4] = { 0, 0, 0, 0 };
+
+        struct i2c_msg msgs[2] = {
+                {
+                        .addr = client->addr,
+                        .flags = 0,
+                        .len = 2,
+                        .buf = addr,
+                }, {
+                        .addr = client->addr,
+                        .flags = I2C_M_RD,
+                        .len = 4,
+                        .buf = buf,
+                }
+        };
+
+        addr[0] = (reg_addr >> 8);
+        addr[1] = (reg_addr & 0xff);
+        msgs[0].addr = 0x44;
+        msgs[1].addr = 0x44;
+
+        retval = i2c_xfer(client->adapter, msgs, 2);
+
+        v = (buf[3] << 24) | (buf[2] << 16) | (buf[1] << 8) | buf[0];
+        *value = v;
+
+        return v;
+}
+
+int cx25821_i2c_write(struct cx25821_i2c *bus, u16 reg_addr, int value)
+{
+        struct i2c_client *client = &bus->i2c_client;
+        int retval = 0;
+        u8 buf[6] = { 0, 0, 0, 0, 0, 0 };
+
+        struct i2c_msg msgs[1] = {
+                {
+                        .addr = client->addr,
+                        .flags = 0,
+                        .len = 6,
+                        .buf = buf,
+                }
+        };
+
+        buf[0] = reg_addr >> 8;
+        buf[1] = reg_addr & 0xff;
+        buf[5] = (value >> 24) & 0xff;
+        buf[4] = (value >> 16) & 0xff;
+        buf[3] = (value >> 8) & 0xff;
+        buf[2] = value & 0xff;
+        client->flags = 0;
+        msgs[0].addr = 0x44;
+
+        retval = i2c_xfer(client->adapter, msgs, 1);
+
+        return retval;
+}
diff --git a/drivers/media/video/cx25821/cx25821-medusa-defines.h b/drivers/media/video/cx25821/cx25821-medusa-defines.h
new file mode 100644
index 000000000000..60d197f57556
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-medusa-defines.h
@@ -0,0 +1,42 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <shu.lin@conexant.com>, <hiep.huynh@conexant.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _MEDUSA_DEF_H_
+#define _MEDUSA_DEF_H_
+
+/* Video deocder that we supported */
+#define VDEC_A          0
+#define VDEC_B          1
+#define VDEC_C          2
+#define VDEC_D          3
+#define VDEC_E          4
+#define VDEC_F          5
+#define VDEC_G          6
+#define VDEC_H          7
+
+/* end of display sequence */
+#define END_OF_SEQ                                      0xF;
+
+/* registry string size */
+#define MAX_REGISTRY_SZ                                 40;
+
+#endif
diff --git a/drivers/media/video/cx25821/cx25821-medusa-reg.h b/drivers/media/video/cx25821/cx25821-medusa-reg.h
new file mode 100644
index 000000000000..1c1c228352d1
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-medusa-reg.h
@@ -0,0 +1,455 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <shu.lin@conexant.com>, <hiep.huynh@conexant.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __MEDUSA_REGISTERS__
+#define __MEDUSA_REGISTERS__
+
+/* Serial Slave Registers */
+#define HOST_REGISTER1                          0x0000
+#define HOST_REGISTER2                          0x0001
+
+/* Chip Configuration Registers */
+#define CHIP_CTRL                                       0x0100
+#define AFE_AB_CTRL                                     0x0104
+#define AFE_CD_CTRL                                     0x0108
+#define AFE_EF_CTRL                                     0x010C
+#define AFE_GH_CTRL                                     0x0110
+#define DENC_AB_CTRL                            0x0114
+#define BYP_AB_CTRL                                     0x0118
+#define MON_A_CTRL                                      0x011C
+#define DISP_SEQ_A                                      0x0120
+#define DISP_SEQ_B                                      0x0124
+#define DISP_AB_CNT                                     0x0128
+#define DISP_CD_CNT                                     0x012C
+#define DISP_EF_CNT                                     0x0130
+#define DISP_GH_CNT                                     0x0134
+#define DISP_IJ_CNT                                     0x0138
+#define PIN_OE_CTRL                                     0x013C
+#define PIN_SPD_CTRL                            0x0140
+#define PIN_SPD_CTRL2                           0x0144
+#define IRQ_STAT_CTRL                           0x0148
+#define POWER_CTRL_AB                           0x014C
+#define POWER_CTRL_CD                           0x0150
+#define POWER_CTRL_EF                           0x0154
+#define POWER_CTRL_GH                           0x0158
+#define TUNE_CTRL                                       0x015C
+#define BIAS_CTRL                                       0x0160
+#define AFE_AB_DIAG_CTRL                        0x0164
+#define AFE_CD_DIAG_CTRL                        0x0168
+#define AFE_EF_DIAG_CTRL                        0x016C
+#define AFE_GH_DIAG_CTRL                        0x0170
+#define PLL_AB_DIAG_CTRL                        0x0174
+#define PLL_CD_DIAG_CTRL                        0x0178
+#define PLL_EF_DIAG_CTRL                        0x017C
+#define PLL_GH_DIAG_CTRL                        0x0180
+#define TEST_CTRL                                       0x0184
+#define BIST_STAT                                       0x0188
+#define BIST_STAT2                                      0x018C
+#define BIST_VID_PLL_AB_STAT            0x0190
+#define BIST_VID_PLL_CD_STAT            0x0194
+#define BIST_VID_PLL_EF_STAT            0x0198
+#define BIST_VID_PLL_GH_STAT            0x019C
+#define DLL_DIAG_CTRL                           0x01A0
+#define DEV_CH_ID_CTRL                          0x01A4
+#define ABIST_CTRL_STATUS                       0x01A8
+#define ABIST_FREQ                                      0x01AC
+#define ABIST_GOERT_SHIFT                       0x01B0
+#define ABIST_COEF12                            0x01B4
+#define ABIST_COEF34                            0x01B8
+#define ABIST_COEF56                            0x01BC
+#define ABIST_COEF7_SNR                         0x01C0
+#define ABIST_ADC_CAL                           0x01C4
+#define ABIST_BIN1_VGA0                         0x01C8
+#define ABIST_BIN2_VGA1                         0x01CC
+#define ABIST_BIN3_VGA2                         0x01D0
+#define ABIST_BIN4_VGA3                         0x01D4
+#define ABIST_BIN5_VGA4                         0x01D8
+#define ABIST_BIN6_VGA5                         0x01DC
+#define ABIST_BIN7_VGA6                         0x0x1E0
+#define ABIST_CLAMP_A                           0x0x1E4
+#define ABIST_CLAMP_B                           0x0x1E8
+#define ABIST_CLAMP_C                           0x01EC
+#define ABIST_CLAMP_D                           0x01F0
+#define ABIST_CLAMP_E                           0x01F4
+#define ABIST_CLAMP_F                           0x01F8
+
+/*              Digital Video Encoder A Registers */
+#define DENC_A_REG_1                                    0x0200
+#define DENC_A_REG_2                                    0x0204
+#define DENC_A_REG_3                                    0x0208
+#define DENC_A_REG_4                                    0x020C
+#define DENC_A_REG_5                                    0x0210
+#define DENC_A_REG_6                                    0x0214
+#define DENC_A_REG_7                                    0x0218
+#define DENC_A_REG_8                                    0x021C
+
+/*      Digital Video Encoder B Registers */
+#define DENC_B_REG_1                                    0x0300
+#define DENC_B_REG_2                                    0x0304
+#define DENC_B_REG_3                                    0x0308
+#define DENC_B_REG_4                                    0x030C
+#define DENC_B_REG_5                                    0x0310
+#define DENC_B_REG_6                                    0x0314
+#define DENC_B_REG_7                                    0x0318
+#define DENC_B_REG_8                                    0x031C
+
+/*              Video Decoder A Registers */
+#define MODE_CTRL                                               0x1000
+#define OUT_CTRL1                                               0x1004
+#define OUT_CTRL_NS                                             0x1008
+#define GEN_STAT                                                0x100C
+#define INT_STAT_MASK                                   0x1010
+#define LUMA_CTRL                                               0x1014
+#define CHROMA_CTRL                                             0x1018
+#define CRUSH_CTRL                                              0x101C
+#define HORIZ_TIM_CTRL                                  0x1020
+#define VERT_TIM_CTRL                                   0x1024
+#define MISC_TIM_CTRL                                   0x1028
+#define FIELD_COUNT                                             0x102C
+#define HSCALE_CTRL                                             0x1030
+#define VSCALE_CTRL                                             0x1034
+#define MAN_VGA_CTRL                                    0x1038
+#define MAN_AGC_CTRL                                    0x103C
+#define DFE_CTRL1                                               0x1040
+#define DFE_CTRL2                                               0x1044
+#define DFE_CTRL3                                               0x1048
+#define PLL_CTRL                                                0x104C
+#define PLL_CTRL_FAST                                   0x1050
+#define HTL_CTRL                                                0x1054
+#define SRC_CFG                                                 0x1058
+#define SC_STEP_SIZE                                    0x105C
+#define SC_CONVERGE_CTRL                                0x1060
+#define SC_LOOP_CTRL                                    0x1064
+#define COMB_2D_HFS_CFG                                 0x1068
+#define COMB_2D_HFD_CFG                                 0x106C
+#define COMB_2D_LF_CFG                                  0x1070
+#define COMB_2D_BLEND                                   0x1074
+#define COMB_MISC_CTRL                                  0x1078
+#define COMB_FLAT_THRESH_CTRL                   0x107C
+#define COMB_TEST                                               0x1080
+#define BP_MISC_CTRL                                    0x1084
+#define VCR_DET_CTRL                                    0x1088
+#define NOISE_DET_CTRL                                  0x108C
+#define COMB_FLAT_NOISE_CTRL                    0x1090
+#define VERSION                                                 0x11F8
+#define SOFT_RST_CTRL                                   0x11FC
+
+/*      Video Decoder B Registers */
+#define VDEC_B_MODE_CTRL                                0x1200
+#define VDEC_B_OUT_CTRL1                                0x1204
+#define VDEC_B_OUT_CTRL_NS                              0x1208
+#define VDEC_B_GEN_STAT                                 0x120C
+#define VDEC_B_INT_STAT_MASK                    0x1210
+#define VDEC_B_LUMA_CTRL                                0x1214
+#define VDEC_B_CHROMA_CTRL                              0x1218
+#define VDEC_B_CRUSH_CTRL                               0x121C
+#define VDEC_B_HORIZ_TIM_CTRL                   0x1220
+#define VDEC_B_VERT_TIM_CTRL                    0x1224
+#define VDEC_B_MISC_TIM_CTRL                    0x1228
+#define VDEC_B_FIELD_COUNT                              0x122C
+#define VDEC_B_HSCALE_CTRL                              0x1230
+#define VDEC_B_VSCALE_CTRL                              0x1234
+#define VDEC_B_MAN_VGA_CTRL                             0x1238
+#define VDEC_B_MAN_AGC_CTRL                             0x123C
+#define VDEC_B_DFE_CTRL1                                0x1240
+#define VDEC_B_DFE_CTRL2                                0x1244
+#define VDEC_B_DFE_CTRL3                                0x1248
+#define VDEC_B_PLL_CTRL                                 0x124C
+#define VDEC_B_PLL_CTRL_FAST                    0x1250
+#define VDEC_B_HTL_CTRL                                 0x1254
+#define VDEC_B_SRC_CFG                                  0x1258
+#define VDEC_B_SC_STEP_SIZE                             0x125C
+#define VDEC_B_SC_CONVERGE_CTRL                 0x1260
+#define VDEC_B_SC_LOOP_CTRL                             0x1264
+#define VDEC_B_COMB_2D_HFS_CFG                  0x1268
+#define VDEC_B_COMB_2D_HFD_CFG                  0x126C
+#define VDEC_B_COMB_2D_LF_CFG                   0x1270
+#define VDEC_B_COMB_2D_BLEND                    0x1274
+#define VDEC_B_COMB_MISC_CTRL                   0x1278
+#define VDEC_B_COMB_FLAT_THRESH_CTRL    0x127C
+#define VDEC_B_COMB_TEST                                0x1280
+#define VDEC_B_BP_MISC_CTRL                             0x1284
+#define VDEC_B_VCR_DET_CTRL                             0x1288
+#define VDEC_B_NOISE_DET_CTRL                   0x128C
+#define VDEC_B_COMB_FLAT_NOISE_CTRL             0x1290
+#define VDEC_B_VERSION                                  0x13F8
+#define VDEC_B_SOFT_RST_CTRL                    0x13FC
+
+/* Video Decoder C Registers */
+#define VDEC_C_MODE_CTRL                                0x1400
+#define VDEC_C_OUT_CTRL1                                0x1404
+#define VDEC_C_OUT_CTRL_NS                              0x1408
+#define VDEC_C_GEN_STAT                                 0x140C
+#define VDEC_C_INT_STAT_MASK                    0x1410
+#define VDEC_C_LUMA_CTRL                                0x1414
+#define VDEC_C_CHROMA_CTRL                              0x1418
+#define VDEC_C_CRUSH_CTRL                               0x141C
+#define VDEC_C_HORIZ_TIM_CTRL                   0x1420
+#define VDEC_C_VERT_TIM_CTRL                    0x1424
+#define VDEC_C_MISC_TIM_CTRL                    0x1428
+#define VDEC_C_FIELD_COUNT                              0x142C
+#define VDEC_C_HSCALE_CTRL                              0x1430
+#define VDEC_C_VSCALE_CTRL                              0x1434
+#define VDEC_C_MAN_VGA_CTRL                             0x1438
+#define VDEC_C_MAN_AGC_CTRL                             0x143C
+#define VDEC_C_DFE_CTRL1                                0x1440
+#define VDEC_C_DFE_CTRL2                                0x1444
+#define VDEC_C_DFE_CTRL3                                0x1448
+#define VDEC_C_PLL_CTRL                                 0x144C
+#define VDEC_C_PLL_CTRL_FAST                    0x1450
+#define VDEC_C_HTL_CTRL                                 0x1454
+#define VDEC_C_SRC_CFG                                  0x1458
+#define VDEC_C_SC_STEP_SIZE                             0x145C
+#define VDEC_C_SC_CONVERGE_CTRL                 0x1460
+#define VDEC_C_SC_LOOP_CTRL                             0x1464
+#define VDEC_C_COMB_2D_HFS_CFG                  0x1468
+#define VDEC_C_COMB_2D_HFD_CFG                  0x146C
+#define VDEC_C_COMB_2D_LF_CFG                   0x1470
+#define VDEC_C_COMB_2D_BLEND                    0x1474
+#define VDEC_C_COMB_MISC_CTRL                   0x1478
+#define VDEC_C_COMB_FLAT_THRESH_CTRL    0x147C
+#define VDEC_C_COMB_TEST                                0x1480
+#define VDEC_C_BP_MISC_CTRL                             0x1484
+#define VDEC_C_VCR_DET_CTRL                             0x1488
+#define VDEC_C_NOISE_DET_CTRL                   0x148C
+#define VDEC_C_COMB_FLAT_NOISE_CTRL             0x1490
+#define VDEC_C_VERSION                                  0x15F8
+#define VDEC_C_SOFT_RST_CTRL                    0x15FC
+
+/* Video Decoder D Registers */
+#define VDEC_D_MODE_CTRL                                0x1600
+#define VDEC_D_OUT_CTRL1                                0x1604
+#define VDEC_D_OUT_CTRL_NS                              0x1608
+#define VDEC_D_GEN_STAT                                 0x160C
+#define VDEC_D_INT_STAT_MASK                    0x1610
+#define VDEC_D_LUMA_CTRL                                0x1614
+#define VDEC_D_CHROMA_CTRL                              0x1618
+#define VDEC_D_CRUSH_CTRL                               0x161C
+#define VDEC_D_HORIZ_TIM_CTRL                   0x1620
+#define VDEC_D_VERT_TIM_CTRL                    0x1624
+#define VDEC_D_MISC_TIM_CTRL                    0x1628
+#define VDEC_D_FIELD_COUNT                              0x162C
+#define VDEC_D_HSCALE_CTRL                              0x1630
+#define VDEC_D_VSCALE_CTRL                              0x1634
+#define VDEC_D_MAN_VGA_CTRL                             0x1638
+#define VDEC_D_MAN_AGC_CTRL                             0x163C
+#define VDEC_D_DFE_CTRL1                                0x1640
+#define VDEC_D_DFE_CTRL2                                0x1644
+#define VDEC_D_DFE_CTRL3                                0x1648
+#define VDEC_D_PLL_CTRL                                 0x164C
+#define VDEC_D_PLL_CTRL_FAST                    0x1650
+#define VDEC_D_HTL_CTRL                                 0x1654
+#define VDEC_D_SRC_CFG                                  0x1658
+#define VDEC_D_SC_STEP_SIZE                             0x165C
+#define VDEC_D_SC_CONVERGE_CTRL                 0x1660
+#define VDEC_D_SC_LOOP_CTRL                             0x1664
+#define VDEC_D_COMB_2D_HFS_CFG                  0x1668
+#define VDEC_D_COMB_2D_HFD_CFG                  0x166C
+#define VDEC_D_COMB_2D_LF_CFG                   0x1670
+#define VDEC_D_COMB_2D_BLEND                    0x1674
+#define VDEC_D_COMB_MISC_CTRL                   0x1678
+#define VDEC_D_COMB_FLAT_THRESH_CTRL    0x167C
+#define VDEC_D_COMB_TEST                                0x1680
+#define VDEC_D_BP_MISC_CTRL                             0x1684
+#define VDEC_D_VCR_DET_CTRL                             0x1688
+#define VDEC_D_NOISE_DET_CTRL                   0x168C
+#define VDEC_D_COMB_FLAT_NOISE_CTRL             0x1690
+#define VDEC_D_VERSION                                  0x17F8
+#define VDEC_D_SOFT_RST_CTRL                    0x17FC
+
+/* Video Decoder E Registers */
+#define VDEC_E_MODE_CTRL                                0x1800
+#define VDEC_E_OUT_CTRL1                                0x1804
+#define VDEC_E_OUT_CTRL_NS                              0x1808
+#define VDEC_E_GEN_STAT                                 0x180C
+#define VDEC_E_INT_STAT_MASK                    0x1810
+#define VDEC_E_LUMA_CTRL                                0x1814
+#define VDEC_E_CHROMA_CTRL                              0x1818
+#define VDEC_E_CRUSH_CTRL                               0x181C
+#define VDEC_E_HORIZ_TIM_CTRL                   0x1820
+#define VDEC_E_VERT_TIM_CTRL                    0x1824
+#define VDEC_E_MISC_TIM_CTRL                    0x1828
+#define VDEC_E_FIELD_COUNT                              0x182C
+#define VDEC_E_HSCALE_CTRL                              0x1830
+#define VDEC_E_VSCALE_CTRL                              0x1834
+#define VDEC_E_MAN_VGA_CTRL                             0x1838
+#define VDEC_E_MAN_AGC_CTRL                             0x183C
+#define VDEC_E_DFE_CTRL1                                0x1840
+#define VDEC_E_DFE_CTRL2                                0x1844
+#define VDEC_E_DFE_CTRL3                                0x1848
+#define VDEC_E_PLL_CTRL                                 0x184C
+#define VDEC_E_PLL_CTRL_FAST                    0x1850
+#define VDEC_E_HTL_CTRL                                 0x1854
+#define VDEC_E_SRC_CFG                                  0x1858
+#define VDEC_E_SC_STEP_SIZE                             0x185C
+#define VDEC_E_SC_CONVERGE_CTRL                 0x1860
+#define VDEC_E_SC_LOOP_CTRL                             0x1864
+#define VDEC_E_COMB_2D_HFS_CFG                  0x1868
+#define VDEC_E_COMB_2D_HFD_CFG                  0x186C
+#define VDEC_E_COMB_2D_LF_CFG                   0x1870
+#define VDEC_E_COMB_2D_BLEND                    0x1874
+#define VDEC_E_COMB_MISC_CTRL                   0x1878
+#define VDEC_E_COMB_FLAT_THRESH_CTRL    0x187C
+#define VDEC_E_COMB_TEST                                0x1880
+#define VDEC_E_BP_MISC_CTRL                             0x1884
+#define VDEC_E_VCR_DET_CTRL                             0x1888
+#define VDEC_E_NOISE_DET_CTRL                   0x188C
+#define VDEC_E_COMB_FLAT_NOISE_CTRL             0x1890
+#define VDEC_E_VERSION                                  0x19F8
+#define VDEC_E_SOFT_RST_CTRL                    0x19FC
+
+/* Video Decoder F Registers */
+#define VDEC_F_MODE_CTRL                                0x1A00
+#define VDEC_F_OUT_CTRL1                                0x1A04
+#define VDEC_F_OUT_CTRL_NS                              0x1A08
+#define VDEC_F_GEN_STAT                                 0x1A0C
+#define VDEC_F_INT_STAT_MASK                    0x1A10
+#define VDEC_F_LUMA_CTRL                                0x1A14
+#define VDEC_F_CHROMA_CTRL                              0x1A18
+#define VDEC_F_CRUSH_CTRL                               0x1A1C
+#define VDEC_F_HORIZ_TIM_CTRL                   0x1A20
+#define VDEC_F_VERT_TIM_CTRL                    0x1A24
+#define VDEC_F_MISC_TIM_CTRL                    0x1A28
+#define VDEC_F_FIELD_COUNT                              0x1A2C
+#define VDEC_F_HSCALE_CTRL                              0x1A30
+#define VDEC_F_VSCALE_CTRL                              0x1A34
+#define VDEC_F_MAN_VGA_CTRL                             0x1A38
+#define VDEC_F_MAN_AGC_CTRL                             0x1A3C
+#define VDEC_F_DFE_CTRL1                                0x1A40
+#define VDEC_F_DFE_CTRL2                                0x1A44
+#define VDEC_F_DFE_CTRL3                                0x1A48
+#define VDEC_F_PLL_CTRL                                 0x1A4C
+#define VDEC_F_PLL_CTRL_FAST                    0x1A50
+#define VDEC_F_HTL_CTRL                                 0x1A54
+#define VDEC_F_SRC_CFG                                  0x1A58
+#define VDEC_F_SC_STEP_SIZE                             0x1A5C
+#define VDEC_F_SC_CONVERGE_CTRL                 0x1A60
+#define VDEC_F_SC_LOOP_CTRL                             0x1A64
+#define VDEC_F_COMB_2D_HFS_CFG                  0x1A68
+#define VDEC_F_COMB_2D_HFD_CFG                  0x1A6C
+#define VDEC_F_COMB_2D_LF_CFG                   0x1A70
+#define VDEC_F_COMB_2D_BLEND                    0x1A74
+#define VDEC_F_COMB_MISC_CTRL                   0x1A78
+#define VDEC_F_COMB_FLAT_THRESH_CTRL    0x1A7C
+#define VDEC_F_COMB_TEST                                0x1A80
+#define VDEC_F_BP_MISC_CTRL                             0x1A84
+#define VDEC_F_VCR_DET_CTRL                             0x1A88
+#define VDEC_F_NOISE_DET_CTRL                   0x1A8C
+#define VDEC_F_COMB_FLAT_NOISE_CTRL             0x1A90
+#define VDEC_F_VERSION                                  0x1BF8
+#define VDEC_F_SOFT_RST_CTRL                    0x1BFC
+
+/* Video Decoder G Registers */
+#define VDEC_G_MODE_CTRL                                0x1C00
+#define VDEC_G_OUT_CTRL1                                0x1C04
+#define VDEC_G_OUT_CTRL_NS                              0x1C08
+#define VDEC_G_GEN_STAT                                 0x1C0C
+#define VDEC_G_INT_STAT_MASK                    0x1C10
+#define VDEC_G_LUMA_CTRL                                0x1C14
+#define VDEC_G_CHROMA_CTRL                              0x1C18
+#define VDEC_G_CRUSH_CTRL                               0x1C1C
+#define VDEC_G_HORIZ_TIM_CTRL                   0x1C20
+#define VDEC_G_VERT_TIM_CTRL                    0x1C24
+#define VDEC_G_MISC_TIM_CTRL                    0x1C28
+#define VDEC_G_FIELD_COUNT                              0x1C2C
+#define VDEC_G_HSCALE_CTRL                              0x1C30
+#define VDEC_G_VSCALE_CTRL                              0x1C34
+#define VDEC_G_MAN_VGA_CTRL                             0x1C38
+#define VDEC_G_MAN_AGC_CTRL                             0x1C3C
+#define VDEC_G_DFE_CTRL1                                0x1C40
+#define VDEC_G_DFE_CTRL2                                0x1C44
+#define VDEC_G_DFE_CTRL3                                0x1C48
+#define VDEC_G_PLL_CTRL                                 0x1C4C
+#define VDEC_G_PLL_CTRL_FAST                    0x1C50
+#define VDEC_G_HTL_CTRL                                 0x1C54
+#define VDEC_G_SRC_CFG                                  0x1C58
+#define VDEC_G_SC_STEP_SIZE                             0x1C5C
+#define VDEC_G_SC_CONVERGE_CTRL                 0x1C60
+#define VDEC_G_SC_LOOP_CTRL                             0x1C64
+#define VDEC_G_COMB_2D_HFS_CFG                  0x1C68
+#define VDEC_G_COMB_2D_HFD_CFG                  0x1C6C
+#define VDEC_G_COMB_2D_LF_CFG                   0x1C70
+#define VDEC_G_COMB_2D_BLEND                    0x1C74
+#define VDEC_G_COMB_MISC_CTRL                   0x1C78
+#define VDEC_G_COMB_FLAT_THRESH_CTRL    0x1C7C
+#define VDEC_G_COMB_TEST                                0x1C80
+#define VDEC_G_BP_MISC_CTRL                             0x1C84
+#define VDEC_G_VCR_DET_CTRL                             0x1C88
+#define VDEC_G_NOISE_DET_CTRL                   0x1C8C
+#define VDEC_G_COMB_FLAT_NOISE_CTRL             0x1C90
+#define VDEC_G_VERSION                                  0x1DF8
+#define VDEC_G_SOFT_RST_CTRL                    0x1DFC
+
+/*              Video Decoder H Registers  */
+#define VDEC_H_MODE_CTRL                                0x1E00
+#define VDEC_H_OUT_CTRL1                                0x1E04
+#define VDEC_H_OUT_CTRL_NS                              0x1E08
+#define VDEC_H_GEN_STAT                                 0x1E0C
+#define VDEC_H_INT_STAT_MASK                    0x1E1E
+#define VDEC_H_LUMA_CTRL                                0x1E14
+#define VDEC_H_CHROMA_CTRL                              0x1E18
+#define VDEC_H_CRUSH_CTRL                               0x1E1C
+#define VDEC_H_HORIZ_TIM_CTRL                   0x1E20
+#define VDEC_H_VERT_TIM_CTRL                    0x1E24
+#define VDEC_H_MISC_TIM_CTRL                    0x1E28
+#define VDEC_H_FIELD_COUNT                              0x1E2C
+#define VDEC_H_HSCALE_CTRL                              0x1E30
+#define VDEC_H_VSCALE_CTRL                              0x1E34
+#define VDEC_H_MAN_VGA_CTRL                             0x1E38
+#define VDEC_H_MAN_AGC_CTRL                             0x1E3C
+#define VDEC_H_DFE_CTRL1                                0x1E40
+#define VDEC_H_DFE_CTRL2                                0x1E44
+#define VDEC_H_DFE_CTRL3                                0x1E48
+#define VDEC_H_PLL_CTRL                                 0x1E4C
+#define VDEC_H_PLL_CTRL_FAST                    0x1E50
+#define VDEC_H_HTL_CTRL                                 0x1E54
+#define VDEC_H_SRC_CFG                                  0x1E58
+#define VDEC_H_SC_STEP_SIZE                             0x1E5C
+#define VDEC_H_SC_CONVERGE_CTRL                 0x1E60
+#define VDEC_H_SC_LOOP_CTRL                             0x1E64
+#define VDEC_H_COMB_2D_HFS_CFG                  0x1E68
+#define VDEC_H_COMB_2D_HFD_CFG                  0x1E6C
+#define VDEC_H_COMB_2D_LF_CFG                   0x1E70
+#define VDEC_H_COMB_2D_BLEND                    0x1E74
+#define VDEC_H_COMB_MISC_CTRL                   0x1E78
+#define VDEC_H_COMB_FLAT_THRESH_CTRL    0x1E7C
+#define VDEC_H_COMB_TEST                                0x1E80
+#define VDEC_H_BP_MISC_CTRL                             0x1E84
+#define VDEC_H_VCR_DET_CTRL                             0x1E88
+#define VDEC_H_NOISE_DET_CTRL                   0x1E8C
+#define VDEC_H_COMB_FLAT_NOISE_CTRL             0x1E90
+#define VDEC_H_VERSION                                  0x1FF8
+#define VDEC_H_SOFT_RST_CTRL                    0x1FFC
+
+/*****************************************************************************/
+/* LUMA_CTRL register fields */
+#define VDEC_A_BRITE_CTRL                               0x1014
+#define VDEC_A_CNTRST_CTRL                      0x1015
+#define VDEC_A_PEAK_SEL                         0x1016
+
+/*****************************************************************************/
+/* CHROMA_CTRL register fields */
+#define VDEC_A_USAT_CTRL                        0x1018
+#define VDEC_A_VSAT_CTRL                        0x1019
+#define VDEC_A_HUE_CTRL                         0x101A
+
+#endif
diff --git a/drivers/media/video/cx25821/cx25821-medusa-video.c b/drivers/media/video/cx25821/cx25821-medusa-video.c
new file mode 100644
index 000000000000..fc780d0908dc
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-medusa-video.c
@@ -0,0 +1,872 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <shu.lin@conexant.com>, <hiep.huynh@conexant.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include "cx25821.h"
+#include "cx25821-medusa-video.h"
+#include "cx25821-biffuncs.h"
+
+/*
+ * medusa_enable_bluefield_output()
+ *
+ * Enable the generation of blue filed output if no video
+ *
+ */
+static void medusa_enable_bluefield_output(struct cx25821_dev *dev, int channel,
+                                           int enable)
+{
+        int ret_val = 1;
+        u32 value = 0;
+        u32 tmp = 0;
+        int out_ctrl = OUT_CTRL1;
+        int out_ctrl_ns = OUT_CTRL_NS;
+
+        switch (channel) {
+        default:
+        case VDEC_A:
+                break;
+        case VDEC_B:
+                out_ctrl = VDEC_B_OUT_CTRL1;
+                out_ctrl_ns = VDEC_B_OUT_CTRL_NS;
+                break;
+        case VDEC_C:
+                out_ctrl = VDEC_C_OUT_CTRL1;
+                out_ctrl_ns = VDEC_C_OUT_CTRL_NS;
+                break;
+        case VDEC_D:
+                out_ctrl = VDEC_D_OUT_CTRL1;
+                out_ctrl_ns = VDEC_D_OUT_CTRL_NS;
+                break;
+        case VDEC_E:
+                out_ctrl = VDEC_E_OUT_CTRL1;
+                out_ctrl_ns = VDEC_E_OUT_CTRL_NS;
+                return;
+        case VDEC_F:
+                out_ctrl = VDEC_F_OUT_CTRL1;
+                out_ctrl_ns = VDEC_F_OUT_CTRL_NS;
+                return;
+        case VDEC_G:
+                out_ctrl = VDEC_G_OUT_CTRL1;
+                out_ctrl_ns = VDEC_G_OUT_CTRL_NS;
+                return;
+        case VDEC_H:
+                out_ctrl = VDEC_H_OUT_CTRL1;
+                out_ctrl_ns = VDEC_H_OUT_CTRL_NS;
+                return;
+        }
+
+        value = cx25821_i2c_read(&dev->i2c_bus[0], out_ctrl, &tmp);
+        value &= 0xFFFFFF7F;    /* clear BLUE_FIELD_EN */
+        if (enable)
+                value |= 0x00000080;    /* set BLUE_FIELD_EN */
+        ret_val = cx25821_i2c_write(&dev->i2c_bus[0], out_ctrl, value);
+
+        value = cx25821_i2c_read(&dev->i2c_bus[0], out_ctrl_ns, &tmp);
+        value &= 0xFFFFFF7F;
+        if (enable)
+                value |= 0x00000080;    /* set BLUE_FIELD_EN */
+        ret_val = cx25821_i2c_write(&dev->i2c_bus[0], out_ctrl_ns, value);
+}
+
+static int medusa_initialize_ntsc(struct cx25821_dev *dev)
+{
+        int ret_val = 0;
+        int i = 0;
+        u32 value = 0;
+        u32 tmp = 0;
+
+        mutex_lock(&dev->lock);
+
+        for (i = 0; i < MAX_DECODERS; i++) {
+                /* set video format NTSC-M */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0], MODE_CTRL + (0x200 * i),
+                                     &tmp);
+                value &= 0xFFFFFFF0;
+                /* enable the fast locking mode bit[16] */
+                value |= 0x10001;
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0], MODE_CTRL + (0x200 * i),
+                                      value);
+
+                /* resolution NTSC 720x480 */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0],
+                                     HORIZ_TIM_CTRL + (0x200 * i), &tmp);
+                value &= 0x00C00C00;
+                value |= 0x612D0074;
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      HORIZ_TIM_CTRL + (0x200 * i), value);
+
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0],
+                                     VERT_TIM_CTRL + (0x200 * i), &tmp);
+                value &= 0x00C00C00;
+                value |= 0x1C1E001A;    /* vblank_cnt + 2 to get camera ID */
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      VERT_TIM_CTRL + (0x200 * i), value);
+
+                /* chroma subcarrier step size */
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      SC_STEP_SIZE + (0x200 * i), 0x43E00000);
+
+                /* enable VIP optional active */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0],
+                                     OUT_CTRL_NS + (0x200 * i), &tmp);
+                value &= 0xFFFBFFFF;
+                value |= 0x00040000;
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      OUT_CTRL_NS + (0x200 * i), value);
+
+                /* enable VIP optional active (VIP_OPT_AL) for direct output. */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0], OUT_CTRL1 + (0x200 * i),
+                                     &tmp);
+                value &= 0xFFFBFFFF;
+                value |= 0x00040000;
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0], OUT_CTRL1 + (0x200 * i),
+                                      value);
+
+                /*
+                 * clear VPRES_VERT_EN bit, fixes the chroma run away problem
+                 * when the input switching rate < 16 fields
+                */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0],
+                                     MISC_TIM_CTRL + (0x200 * i), &tmp);
+                /* disable special play detection */
+                value = setBitAtPos(value, 14);
+                value = clearBitAtPos(value, 15);
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      MISC_TIM_CTRL + (0x200 * i), value);
+
+                /* set vbi_gate_en to 0 */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0], DFE_CTRL1 + (0x200 * i),
+                                     &tmp);
+                value = clearBitAtPos(value, 29);
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0], DFE_CTRL1 + (0x200 * i),
+                                      value);
+
+                /* Enable the generation of blue field output if no video */
+                medusa_enable_bluefield_output(dev, i, 1);
+        }
+
+        for (i = 0; i < MAX_ENCODERS; i++) {
+                /* NTSC hclock */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0],
+                                     DENC_A_REG_1 + (0x100 * i), &tmp);
+                value &= 0xF000FC00;
+                value |= 0x06B402D0;
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      DENC_A_REG_1 + (0x100 * i), value);
+
+                /* burst begin and burst end */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0],
+                                     DENC_A_REG_2 + (0x100 * i), &tmp);
+                value &= 0xFF000000;
+                value |= 0x007E9054;
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      DENC_A_REG_2 + (0x100 * i), value);
+
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0],
+                                     DENC_A_REG_3 + (0x100 * i), &tmp);
+                value &= 0xFC00FE00;
+                value |= 0x00EC00F0;
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      DENC_A_REG_3 + (0x100 * i), value);
+
+                /* set NTSC vblank, no phase alternation, 7.5 IRE pedestal */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0],
+                                     DENC_A_REG_4 + (0x100 * i), &tmp);
+                value &= 0x00FCFFFF;
+                value |= 0x13020000;
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      DENC_A_REG_4 + (0x100 * i), value);
+
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0],
+                                     DENC_A_REG_5 + (0x100 * i), &tmp);
+                value &= 0xFFFF0000;
+                value |= 0x0000E575;
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      DENC_A_REG_5 + (0x100 * i), value);
+
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      DENC_A_REG_6 + (0x100 * i), 0x009A89C1);
+
+                /* Subcarrier Increment */
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      DENC_A_REG_7 + (0x100 * i), 0x21F07C1F);
+        }
+
+        /* set picture resolutions */
+        /* 0 - 720 */
+        ret_val = cx25821_i2c_write(&dev->i2c_bus[0], HSCALE_CTRL, 0x0);
+        /* 0 - 480 */
+        ret_val = cx25821_i2c_write(&dev->i2c_bus[0], VSCALE_CTRL, 0x0);
+
+        /* set Bypass input format to NTSC 525 lines */
+        value = cx25821_i2c_read(&dev->i2c_bus[0], BYP_AB_CTRL, &tmp);
+        value |= 0x00080200;
+        ret_val = cx25821_i2c_write(&dev->i2c_bus[0], BYP_AB_CTRL, value);
+
+        mutex_unlock(&dev->lock);
+
+        return ret_val;
+}
+
+static int medusa_PALCombInit(struct cx25821_dev *dev, int dec)
+{
+        int ret_val = -1;
+        u32 value = 0, tmp = 0;
+
+        /* Setup for 2D threshold */
+        ret_val =
+            cx25821_i2c_write(&dev->i2c_bus[0], COMB_2D_HFS_CFG + (0x200 * dec),
+                              0x20002861);
+        ret_val =
+            cx25821_i2c_write(&dev->i2c_bus[0], COMB_2D_HFD_CFG + (0x200 * dec),
+                              0x20002861);
+        ret_val =
+            cx25821_i2c_write(&dev->i2c_bus[0], COMB_2D_LF_CFG + (0x200 * dec),
+                              0x200A1023);
+
+        /* Setup flat chroma and luma thresholds */
+        value =
+            cx25821_i2c_read(&dev->i2c_bus[0],
+                             COMB_FLAT_THRESH_CTRL + (0x200 * dec), &tmp);
+        value &= 0x06230000;
+        ret_val =
+            cx25821_i2c_write(&dev->i2c_bus[0],
+                              COMB_FLAT_THRESH_CTRL + (0x200 * dec), value);
+
+        /* set comb 2D blend */
+        ret_val =
+            cx25821_i2c_write(&dev->i2c_bus[0], COMB_2D_BLEND + (0x200 * dec),
+                              0x210F0F0F);
+
+        /* COMB MISC CONTROL */
+        ret_val =
+            cx25821_i2c_write(&dev->i2c_bus[0], COMB_MISC_CTRL + (0x200 * dec),
+                              0x41120A7F);
+
+        return ret_val;
+}
+
+static int medusa_initialize_pal(struct cx25821_dev *dev)
+{
+        int ret_val = 0;
+        int i = 0;
+        u32 value = 0;
+        u32 tmp = 0;
+
+        mutex_lock(&dev->lock);
+
+        for (i = 0; i < MAX_DECODERS; i++) {
+                /* set video format PAL-BDGHI */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0], MODE_CTRL + (0x200 * i),
+                                     &tmp);
+                value &= 0xFFFFFFF0;
+                /* enable the fast locking mode bit[16] */
+                value |= 0x10004;
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0], MODE_CTRL + (0x200 * i),
+                                      value);
+
+                /* resolution PAL 720x576 */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0],
+                                     HORIZ_TIM_CTRL + (0x200 * i), &tmp);
+                value &= 0x00C00C00;
+                value |= 0x632D007D;
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      HORIZ_TIM_CTRL + (0x200 * i), value);
+
+                /* vblank656_cnt=x26, vactive_cnt=240h, vblank_cnt=x24 */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0],
+                                     VERT_TIM_CTRL + (0x200 * i), &tmp);
+                value &= 0x00C00C00;
+                value |= 0x28240026;    /* vblank_cnt + 2 to get camera ID */
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      VERT_TIM_CTRL + (0x200 * i), value);
+
+                /* chroma subcarrier step size */
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      SC_STEP_SIZE + (0x200 * i), 0x5411E2D0);
+
+                /* enable VIP optional active */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0],
+                                     OUT_CTRL_NS + (0x200 * i), &tmp);
+                value &= 0xFFFBFFFF;
+                value |= 0x00040000;
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      OUT_CTRL_NS + (0x200 * i), value);
+
+                /* enable VIP optional active (VIP_OPT_AL) for direct output. */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0], OUT_CTRL1 + (0x200 * i),
+                                     &tmp);
+                value &= 0xFFFBFFFF;
+                value |= 0x00040000;
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0], OUT_CTRL1 + (0x200 * i),
+                                      value);
+
+                /*
+                 * clear VPRES_VERT_EN bit, fixes the chroma run away problem
+                 * when the input switching rate < 16 fields
+                 */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0],
+                                     MISC_TIM_CTRL + (0x200 * i), &tmp);
+                /* disable special play detection */
+                value = setBitAtPos(value, 14);
+                value = clearBitAtPos(value, 15);
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      MISC_TIM_CTRL + (0x200 * i), value);
+
+                /* set vbi_gate_en to 0 */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0], DFE_CTRL1 + (0x200 * i),
+                                     &tmp);
+                value = clearBitAtPos(value, 29);
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0], DFE_CTRL1 + (0x200 * i),
+                                      value);
+
+                medusa_PALCombInit(dev, i);
+
+                /* Enable the generation of blue field output if no video */
+                medusa_enable_bluefield_output(dev, i, 1);
+        }
+
+        for (i = 0; i < MAX_ENCODERS; i++) {
+                /* PAL hclock */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0],
+                                     DENC_A_REG_1 + (0x100 * i), &tmp);
+                value &= 0xF000FC00;
+                value |= 0x06C002D0;
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      DENC_A_REG_1 + (0x100 * i), value);
+
+                /* burst begin and burst end */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0],
+                                     DENC_A_REG_2 + (0x100 * i), &tmp);
+                value &= 0xFF000000;
+                value |= 0x007E9754;
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      DENC_A_REG_2 + (0x100 * i), value);
+
+                /* hblank and vactive */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0],
+                                     DENC_A_REG_3 + (0x100 * i), &tmp);
+                value &= 0xFC00FE00;
+                value |= 0x00FC0120;
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      DENC_A_REG_3 + (0x100 * i), value);
+
+                /* set PAL vblank, phase alternation, 0 IRE pedestal */
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0],
+                                     DENC_A_REG_4 + (0x100 * i), &tmp);
+                value &= 0x00FCFFFF;
+                value |= 0x14010000;
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      DENC_A_REG_4 + (0x100 * i), value);
+
+                value =
+                    cx25821_i2c_read(&dev->i2c_bus[0],
+                                     DENC_A_REG_5 + (0x100 * i), &tmp);
+                value &= 0xFFFF0000;
+                value |= 0x0000F078;
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      DENC_A_REG_5 + (0x100 * i), value);
+
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      DENC_A_REG_6 + (0x100 * i), 0x00A493CF);
+
+                /* Subcarrier Increment */
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      DENC_A_REG_7 + (0x100 * i), 0x2A098ACB);
+        }
+
+        /* set picture resolutions */
+        /* 0 - 720 */
+        ret_val = cx25821_i2c_write(&dev->i2c_bus[0], HSCALE_CTRL, 0x0);
+        /* 0 - 576 */
+        ret_val = cx25821_i2c_write(&dev->i2c_bus[0], VSCALE_CTRL, 0x0);
+
+        /* set Bypass input format to PAL 625 lines */
+        value = cx25821_i2c_read(&dev->i2c_bus[0], BYP_AB_CTRL, &tmp);
+        value &= 0xFFF7FDFF;
+        ret_val = cx25821_i2c_write(&dev->i2c_bus[0], BYP_AB_CTRL, value);
+
+        mutex_unlock(&dev->lock);
+
+        return ret_val;
+}
+
+int medusa_set_videostandard(struct cx25821_dev *dev)
+{
+        int status = STATUS_SUCCESS;
+        u32 value = 0, tmp = 0;
+
+        if (dev->tvnorm & V4L2_STD_PAL_BG || dev->tvnorm & V4L2_STD_PAL_DK)
+                status = medusa_initialize_pal(dev);
+        else
+                status = medusa_initialize_ntsc(dev);
+
+        /* Enable DENC_A output */
+        value = cx25821_i2c_read(&dev->i2c_bus[0], DENC_A_REG_4, &tmp);
+        value = setBitAtPos(value, 4);
+        status = cx25821_i2c_write(&dev->i2c_bus[0], DENC_A_REG_4, value);
+
+        /* Enable DENC_B output */
+        value = cx25821_i2c_read(&dev->i2c_bus[0], DENC_B_REG_4, &tmp);
+        value = setBitAtPos(value, 4);
+        status = cx25821_i2c_write(&dev->i2c_bus[0], DENC_B_REG_4, value);
+
+        return status;
+}
+
+void medusa_set_resolution(struct cx25821_dev *dev, int width,
+                           int decoder_select)
+{
+        int decoder = 0;
+        int decoder_count = 0;
+        int ret_val = 0;
+        u32 hscale = 0x0;
+        u32 vscale = 0x0;
+        const int MAX_WIDTH = 720;
+
+        mutex_lock(&dev->lock);
+
+        /* validate the width - cannot be negative */
+        if (width > MAX_WIDTH) {
+                pr_info("%s(): width %d > MAX_WIDTH %d ! resetting to MAX_WIDTH\n",
+                        __func__, width, MAX_WIDTH);
+                width = MAX_WIDTH;
+        }
+
+        if (decoder_select <= 7 && decoder_select >= 0) {
+                decoder = decoder_select;
+                decoder_count = decoder_select + 1;
+        } else {
+                decoder = 0;
+                decoder_count = _num_decoders;
+        }
+
+        switch (width) {
+        case 320:
+                hscale = 0x13E34B;
+                vscale = 0x0;
+                break;
+
+        case 352:
+                hscale = 0x10A273;
+                vscale = 0x0;
+                break;
+
+        case 176:
+                hscale = 0x3115B2;
+                vscale = 0x1E00;
+                break;
+
+        case 160:
+                hscale = 0x378D84;
+                vscale = 0x1E00;
+                break;
+
+        default:                /* 720 */
+                hscale = 0x0;
+                vscale = 0x0;
+                break;
+        }
+
+        for (; decoder < decoder_count; decoder++) {
+                /* write scaling values for each decoder */
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      HSCALE_CTRL + (0x200 * decoder), hscale);
+                ret_val =
+                    cx25821_i2c_write(&dev->i2c_bus[0],
+                                      VSCALE_CTRL + (0x200 * decoder), vscale);
+        }
+
+        mutex_unlock(&dev->lock);
+}
+
+static void medusa_set_decoderduration(struct cx25821_dev *dev, int decoder,
+                                       int duration)
+{
+        int ret_val = 0;
+        u32 fld_cnt = 0;
+        u32 tmp = 0;
+        u32 disp_cnt_reg = DISP_AB_CNT;
+
+        mutex_lock(&dev->lock);
+
+        /* no support */
+        if (decoder < VDEC_A && decoder > VDEC_H) {
+                mutex_unlock(&dev->lock);
+                return;
+        }
+
+        switch (decoder) {
+        default:
+                break;
+        case VDEC_C:
+        case VDEC_D:
+                disp_cnt_reg = DISP_CD_CNT;
+                break;
+        case VDEC_E:
+        case VDEC_F:
+                disp_cnt_reg = DISP_EF_CNT;
+                break;
+        case VDEC_G:
+        case VDEC_H:
+                disp_cnt_reg = DISP_GH_CNT;
+                break;
+        }
+
+        _display_field_cnt[decoder] = duration;
+
+        /* update hardware */
+        fld_cnt = cx25821_i2c_read(&dev->i2c_bus[0], disp_cnt_reg, &tmp);
+
+        if (!(decoder % 2)) {   /* EVEN decoder */
+                fld_cnt &= 0xFFFF0000;
+                fld_cnt |= duration;
+        } else {
+                fld_cnt &= 0x0000FFFF;
+                fld_cnt |= ((u32) duration) << 16;
+        }
+
+        ret_val = cx25821_i2c_write(&dev->i2c_bus[0], disp_cnt_reg, fld_cnt);
+
+        mutex_unlock(&dev->lock);
+}
+
+/* Map to Medusa register setting */
+static int mapM(int srcMin,
+                int srcMax, int srcVal, int dstMin, int dstMax, int *dstVal)
+{
+        int numerator;
+        int denominator;
+        int quotient;
+
+        if ((srcMin == srcMax) || (srcVal < srcMin) || (srcVal > srcMax))
+                return -1;
+        /*
+         * This is the overall expression used:
+         * *dstVal =
+         *   (srcVal - srcMin)*(dstMax - dstMin) / (srcMax - srcMin) + dstMin;
+         * but we need to account for rounding so below we use the modulus
+         * operator to find the remainder and increment if necessary.
+         */
+        numerator = (srcVal - srcMin) * (dstMax - dstMin);
+        denominator = srcMax - srcMin;
+        quotient = numerator / denominator;
+
+        if (2 * (numerator % denominator) >= denominator)
+                quotient++;
+
+        *dstVal = quotient + dstMin;
+
+        return 0;
+}
+
+static unsigned long convert_to_twos(long numeric, unsigned long bits_len)
+{
+        unsigned char temp;
+
+        if (numeric >= 0)
+                return numeric;
+        else {
+                temp = ~(abs(numeric) & 0xFF);
+                temp += 1;
+                return temp;
+        }
+}
+
+int medusa_set_brightness(struct cx25821_dev *dev, int brightness, int decoder)
+{
+        int ret_val = 0;
+        int value = 0;
+        u32 val = 0, tmp = 0;
+
+        mutex_lock(&dev->lock);
+        if ((brightness > VIDEO_PROCAMP_MAX)
+            || (brightness < VIDEO_PROCAMP_MIN)) {
+                mutex_unlock(&dev->lock);
+                return -1;
+        }
+        ret_val =
+            mapM(VIDEO_PROCAMP_MIN, VIDEO_PROCAMP_MAX, brightness,
+                 SIGNED_BYTE_MIN, SIGNED_BYTE_MAX, &value);
+        value = convert_to_twos(value, 8);
+        val =
+            cx25821_i2c_read(&dev->i2c_bus[0],
+                             VDEC_A_BRITE_CTRL + (0x200 * decoder), &tmp);
+        val &= 0xFFFFFF00;
+        ret_val |=
+            cx25821_i2c_write(&dev->i2c_bus[0],
+                              VDEC_A_BRITE_CTRL + (0x200 * decoder),
+                              val | value);
+        mutex_unlock(&dev->lock);
+        return ret_val;
+}
+
+int medusa_set_contrast(struct cx25821_dev *dev, int contrast, int decoder)
+{
+        int ret_val = 0;
+        int value = 0;
+        u32 val = 0, tmp = 0;
+
+        mutex_lock(&dev->lock);
+
+        if ((contrast > VIDEO_PROCAMP_MAX) || (contrast < VIDEO_PROCAMP_MIN)) {
+                mutex_unlock(&dev->lock);
+                return -1;
+        }
+
+        ret_val =
+            mapM(VIDEO_PROCAMP_MIN, VIDEO_PROCAMP_MAX, contrast,
+                 UNSIGNED_BYTE_MIN, UNSIGNED_BYTE_MAX, &value);
+        val =
+            cx25821_i2c_read(&dev->i2c_bus[0],
+                             VDEC_A_CNTRST_CTRL + (0x200 * decoder), &tmp);
+        val &= 0xFFFFFF00;
+        ret_val |=
+            cx25821_i2c_write(&dev->i2c_bus[0],
+                              VDEC_A_CNTRST_CTRL + (0x200 * decoder),
+                              val | value);
+
+        mutex_unlock(&dev->lock);
+        return ret_val;
+}
+
+int medusa_set_hue(struct cx25821_dev *dev, int hue, int decoder)
+{
+        int ret_val = 0;
+        int value = 0;
+        u32 val = 0, tmp = 0;
+
+        mutex_lock(&dev->lock);
+
+        if ((hue > VIDEO_PROCAMP_MAX) || (hue < VIDEO_PROCAMP_MIN)) {
+                mutex_unlock(&dev->lock);
+                return -1;
+        }
+
+        ret_val =
+            mapM(VIDEO_PROCAMP_MIN, VIDEO_PROCAMP_MAX, hue, SIGNED_BYTE_MIN,
+                 SIGNED_BYTE_MAX, &value);
+
+        value = convert_to_twos(value, 8);
+        val =
+            cx25821_i2c_read(&dev->i2c_bus[0],
+                             VDEC_A_HUE_CTRL + (0x200 * decoder), &tmp);
+        val &= 0xFFFFFF00;
+
+        ret_val |=
+            cx25821_i2c_write(&dev->i2c_bus[0],
+                              VDEC_A_HUE_CTRL + (0x200 * decoder), val | value);
+
+        mutex_unlock(&dev->lock);
+        return ret_val;
+}
+
+int medusa_set_saturation(struct cx25821_dev *dev, int saturation, int decoder)
+{
+        int ret_val = 0;
+        int value = 0;
+        u32 val = 0, tmp = 0;
+
+        mutex_lock(&dev->lock);
+
+        if ((saturation > VIDEO_PROCAMP_MAX)
+            || (saturation < VIDEO_PROCAMP_MIN)) {
+                mutex_unlock(&dev->lock);
+                return -1;
+        }
+
+        ret_val =
+            mapM(VIDEO_PROCAMP_MIN, VIDEO_PROCAMP_MAX, saturation,
+                 UNSIGNED_BYTE_MIN, UNSIGNED_BYTE_MAX, &value);
+
+        val =
+            cx25821_i2c_read(&dev->i2c_bus[0],
+                             VDEC_A_USAT_CTRL + (0x200 * decoder), &tmp);
+        val &= 0xFFFFFF00;
+        ret_val |=
+            cx25821_i2c_write(&dev->i2c_bus[0],
+                              VDEC_A_USAT_CTRL + (0x200 * decoder),
+                              val | value);
+
+        val =
+            cx25821_i2c_read(&dev->i2c_bus[0],
+                             VDEC_A_VSAT_CTRL + (0x200 * decoder), &tmp);
+        val &= 0xFFFFFF00;
+        ret_val |=
+            cx25821_i2c_write(&dev->i2c_bus[0],
+                              VDEC_A_VSAT_CTRL + (0x200 * decoder),
+                              val | value);
+
+        mutex_unlock(&dev->lock);
+        return ret_val;
+}
+
+/* Program the display sequence and monitor output. */
+
+int medusa_video_init(struct cx25821_dev *dev)
+{
+        u32 value = 0, tmp = 0;
+        int ret_val = 0;
+        int i = 0;
+
+        mutex_lock(&dev->lock);
+
+        _num_decoders = dev->_max_num_decoders;
+
+        /* disable Auto source selection on all video decoders */
+        value = cx25821_i2c_read(&dev->i2c_bus[0], MON_A_CTRL, &tmp);
+        value &= 0xFFFFF0FF;
+        ret_val = cx25821_i2c_write(&dev->i2c_bus[0], MON_A_CTRL, value);
+
+        if (ret_val < 0)
+                goto error;
+
+        /* Turn off Master source switch enable */
+        value = cx25821_i2c_read(&dev->i2c_bus[0], MON_A_CTRL, &tmp);
+        value &= 0xFFFFFFDF;
+        ret_val = cx25821_i2c_write(&dev->i2c_bus[0], MON_A_CTRL, value);
+
+        if (ret_val < 0)
+                goto error;
+
+        mutex_unlock(&dev->lock);
+
+        for (i = 0; i < _num_decoders; i++)
+                medusa_set_decoderduration(dev, i, _display_field_cnt[i]);
+
+        mutex_lock(&dev->lock);
+
+        /* Select monitor as DENC A input, power up the DAC */
+        value = cx25821_i2c_read(&dev->i2c_bus[0], DENC_AB_CTRL, &tmp);
+        value &= 0xFF70FF70;
+        value |= 0x00090008;    /* set en_active */
+        ret_val = cx25821_i2c_write(&dev->i2c_bus[0], DENC_AB_CTRL, value);
+
+        if (ret_val < 0)
+                goto error;
+
+        /* enable input is VIP/656 */
+        value = cx25821_i2c_read(&dev->i2c_bus[0], BYP_AB_CTRL, &tmp);
+        value |= 0x00040100;    /* enable VIP */
+        ret_val = cx25821_i2c_write(&dev->i2c_bus[0], BYP_AB_CTRL, value);
+
+        if (ret_val < 0)
+                goto error;
+
+        /* select AFE clock to output mode */
+        value = cx25821_i2c_read(&dev->i2c_bus[0], AFE_AB_DIAG_CTRL, &tmp);
+        value &= 0x83FFFFFF;
+        ret_val =
+           cx25821_i2c_write(&dev->i2c_bus[0], AFE_AB_DIAG_CTRL,
+                             value | 0x10000000);
+
+        if (ret_val < 0)
+                goto error;
+
+        /* Turn on all of the data out and control output pins. */
+        value = cx25821_i2c_read(&dev->i2c_bus[0], PIN_OE_CTRL, &tmp);
+        value &= 0xFEF0FE00;
+        if (_num_decoders == MAX_DECODERS) {
+                /*
+                 * Note: The octal board does not support control pins(bit16-19)
+                 * These bits are ignored in the octal board.
+                 *
+                 * disable VDEC A-C port, default to Mobilygen Interface
+                 */
+                value |= 0x010001F8;
+        } else {
+                /* disable VDEC A-C port, default to Mobilygen Interface */
+                value |= 0x010F0108;
+        }
+
+        value |= 7;
+        ret_val = cx25821_i2c_write(&dev->i2c_bus[0], PIN_OE_CTRL, value);
+
+        if (ret_val < 0)
+                goto error;
+
+
+        mutex_unlock(&dev->lock);
+
+        ret_val = medusa_set_videostandard(dev);
+
+        return ret_val;
+
+error:
+        mutex_unlock(&dev->lock);
+        return ret_val;
+}
diff --git a/drivers/media/video/cx25821/cx25821-medusa-video.h b/drivers/media/video/cx25821/cx25821-medusa-video.h
new file mode 100644
index 000000000000..6175e0961855
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-medusa-video.h
@@ -0,0 +1,49 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <shu.lin@conexant.com>, <hiep.huynh@conexant.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _MEDUSA_VIDEO_H
+#define _MEDUSA_VIDEO_H
+
+#include "cx25821-medusa-defines.h"
+
+/* Color control constants */
+#define VIDEO_PROCAMP_MIN                 0
+#define VIDEO_PROCAMP_MAX                 10000
+#define UNSIGNED_BYTE_MIN                 0
+#define UNSIGNED_BYTE_MAX                 0xFF
+#define SIGNED_BYTE_MIN                   -128
+#define SIGNED_BYTE_MAX                   127
+
+/* Default video color settings */
+#define SHARPNESS_DEFAULT                 50
+#define SATURATION_DEFAULT              5000
+#define BRIGHTNESS_DEFAULT              6200
+#define CONTRAST_DEFAULT                5000
+#define HUE_DEFAULT                     5000
+
+unsigned short _num_decoders;
+unsigned short _num_cameras;
+
+unsigned int _video_standard;
+int _display_field_cnt[MAX_DECODERS];
+
+#endif
diff --git a/drivers/media/video/cx25821/cx25821-reg.h b/drivers/media/video/cx25821/cx25821-reg.h
new file mode 100644
index 000000000000..a3fc25a4dc0b
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-reg.h
@@ -0,0 +1,1592 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <shu.lin@conexant.com>, <hiep.huynh@conexant.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __CX25821_REGISTERS__
+#define __CX25821_REGISTERS__
+
+/* Risc Instructions */
+#define RISC_CNT_INC     0x00010000
+#define RISC_CNT_RESET   0x00030000
+#define RISC_IRQ1                0x01000000
+#define RISC_IRQ2                0x02000000
+#define RISC_EOL                 0x04000000
+#define RISC_SOL                 0x08000000
+#define RISC_WRITE               0x10000000
+#define RISC_SKIP                0x20000000
+#define RISC_JUMP                0x70000000
+#define RISC_SYNC                0x80000000
+#define RISC_RESYNC              0x80008000
+#define RISC_READ                0x90000000
+#define RISC_WRITERM     0xB0000000
+#define RISC_WRITECM     0xC0000000
+#define RISC_WRITECR     0xD0000000
+#define RISC_WRITEC              0x50000000
+#define RISC_READC               0xA0000000
+
+#define RISC_SYNC_ODD            0x00000000
+#define RISC_SYNC_EVEN           0x00000200
+#define RISC_SYNC_ODD_VBI        0x00000006
+#define RISC_SYNC_EVEN_VBI       0x00000207
+#define RISC_NOOP                        0xF0000000
+
+/*****************************************************************************
+* ASB SRAM
+ *****************************************************************************/
+#define  TX_SRAM                   0x000000     /* Transmit SRAM */
+
+/*****************************************************************************/
+#define  RX_RAM                    0x010000     /* Receive SRAM */
+
+/*****************************************************************************
+* Application Layer (AL)
+ *****************************************************************************/
+#define  DEV_CNTRL2                0x040000     /* Device control */
+#define  FLD_RUN_RISC              0x00000020
+
+/* ***************************************************************************** */
+#define  PCI_INT_MSK               0x040010     /* PCI interrupt mask */
+#define  PCI_INT_STAT              0x040014     /* PCI interrupt status */
+#define  PCI_INT_MSTAT             0x040018     /* PCI interrupt masked status */
+#define  FLD_HAMMERHEAD_INT        (1 << 27)
+#define  FLD_UART_INT              (1 << 26)
+#define  FLD_IRQN_INT              (1 << 25)
+#define  FLD_TM_INT                (1 << 28)
+#define  FLD_I2C_3_RACK            (1 << 27)
+#define  FLD_I2C_3_INT             (1 << 26)
+#define  FLD_I2C_2_RACK            (1 << 25)
+#define  FLD_I2C_2_INT             (1 << 24)
+#define  FLD_I2C_1_RACK            (1 << 23)
+#define  FLD_I2C_1_INT             (1 << 22)
+
+#define  FLD_APB_DMA_BERR_INT      (1 << 21)
+#define  FLD_AL_WR_BERR_INT        (1 << 20)
+#define  FLD_AL_RD_BERR_INT        (1 << 19)
+#define  FLD_RISC_WR_BERR_INT      (1 << 18)
+#define  FLD_RISC_RD_BERR_INT      (1 << 17)
+
+#define  FLD_VID_I_INT             (1 << 8)
+#define  FLD_VID_H_INT             (1 << 7)
+#define  FLD_VID_G_INT             (1 << 6)
+#define  FLD_VID_F_INT             (1 << 5)
+#define  FLD_VID_E_INT             (1 << 4)
+#define  FLD_VID_D_INT             (1 << 3)
+#define  FLD_VID_C_INT             (1 << 2)
+#define  FLD_VID_B_INT             (1 << 1)
+#define  FLD_VID_A_INT             (1 << 0)
+
+/* ***************************************************************************** */
+#define  VID_A_INT_MSK             0x040020     /* Video A interrupt mask */
+#define  VID_A_INT_STAT            0x040024     /* Video A interrupt status */
+#define  VID_A_INT_MSTAT           0x040028     /* Video A interrupt masked status */
+#define  VID_A_INT_SSTAT           0x04002C     /* Video A interrupt set status */
+
+/* ***************************************************************************** */
+#define  VID_B_INT_MSK             0x040030     /* Video B interrupt mask */
+#define  VID_B_INT_STAT            0x040034     /* Video B interrupt status */
+#define  VID_B_INT_MSTAT           0x040038     /* Video B interrupt masked status */
+#define  VID_B_INT_SSTAT           0x04003C     /* Video B interrupt set status */
+
+/* ***************************************************************************** */
+#define  VID_C_INT_MSK             0x040040     /* Video C interrupt mask */
+#define  VID_C_INT_STAT            0x040044     /* Video C interrupt status */
+#define  VID_C_INT_MSTAT           0x040048     /* Video C interrupt masked status */
+#define  VID_C_INT_SSTAT           0x04004C     /* Video C interrupt set status */
+
+/* ***************************************************************************** */
+#define  VID_D_INT_MSK             0x040050     /* Video D interrupt mask */
+#define  VID_D_INT_STAT            0x040054     /* Video D interrupt status */
+#define  VID_D_INT_MSTAT           0x040058     /* Video D interrupt masked status */
+#define  VID_D_INT_SSTAT           0x04005C     /* Video D interrupt set status */
+
+/* ***************************************************************************** */
+#define  VID_E_INT_MSK             0x040060     /* Video E interrupt mask */
+#define  VID_E_INT_STAT            0x040064     /* Video E interrupt status */
+#define  VID_E_INT_MSTAT           0x040068     /* Video E interrupt masked status */
+#define  VID_E_INT_SSTAT           0x04006C     /* Video E interrupt set status */
+
+/* ***************************************************************************** */
+#define  VID_F_INT_MSK             0x040070     /* Video F interrupt mask */
+#define  VID_F_INT_STAT            0x040074     /* Video F interrupt status */
+#define  VID_F_INT_MSTAT           0x040078     /* Video F interrupt masked status */
+#define  VID_F_INT_SSTAT           0x04007C     /* Video F interrupt set status */
+
+/* ***************************************************************************** */
+#define  VID_G_INT_MSK             0x040080     /* Video G interrupt mask */
+#define  VID_G_INT_STAT            0x040084     /* Video G interrupt status */
+#define  VID_G_INT_MSTAT           0x040088     /* Video G interrupt masked status */
+#define  VID_G_INT_SSTAT           0x04008C     /* Video G interrupt set status */
+
+/* ***************************************************************************** */
+#define  VID_H_INT_MSK             0x040090     /* Video H interrupt mask */
+#define  VID_H_INT_STAT            0x040094     /* Video H interrupt status */
+#define  VID_H_INT_MSTAT           0x040098     /* Video H interrupt masked status */
+#define  VID_H_INT_SSTAT           0x04009C     /* Video H interrupt set status */
+
+/* ***************************************************************************** */
+#define  VID_I_INT_MSK             0x0400A0     /* Video I interrupt mask */
+#define  VID_I_INT_STAT            0x0400A4     /* Video I interrupt status */
+#define  VID_I_INT_MSTAT           0x0400A8     /* Video I interrupt masked status */
+#define  VID_I_INT_SSTAT           0x0400AC     /* Video I interrupt set status */
+
+/* ***************************************************************************** */
+#define  VID_J_INT_MSK             0x0400B0     /* Video J interrupt mask */
+#define  VID_J_INT_STAT            0x0400B4     /* Video J interrupt status */
+#define  VID_J_INT_MSTAT           0x0400B8     /* Video J interrupt masked status */
+#define  VID_J_INT_SSTAT           0x0400BC     /* Video J interrupt set status */
+
+#define  FLD_VID_SRC_OPC_ERR       0x00020000
+#define  FLD_VID_DST_OPC_ERR       0x00010000
+#define  FLD_VID_SRC_SYNC          0x00002000
+#define  FLD_VID_DST_SYNC          0x00001000
+#define  FLD_VID_SRC_UF            0x00000200
+#define  FLD_VID_DST_OF            0x00000100
+#define  FLD_VID_SRC_RISC2         0x00000020
+#define  FLD_VID_DST_RISC2         0x00000010
+#define  FLD_VID_SRC_RISC1         0x00000002
+#define  FLD_VID_DST_RISC1         0x00000001
+#define  FLD_VID_SRC_ERRORS             (FLD_VID_SRC_OPC_ERR | FLD_VID_SRC_SYNC | FLD_VID_SRC_UF)
+#define  FLD_VID_DST_ERRORS             (FLD_VID_DST_OPC_ERR | FLD_VID_DST_SYNC | FLD_VID_DST_OF)
+
+/* ***************************************************************************** */
+#define  AUD_A_INT_MSK             0x0400C0     /* Audio Int interrupt mask */
+#define  AUD_A_INT_STAT            0x0400C4     /* Audio Int interrupt status */
+#define  AUD_A_INT_MSTAT           0x0400C8     /* Audio Int interrupt masked status */
+#define  AUD_A_INT_SSTAT           0x0400CC     /* Audio Int interrupt set status */
+
+/* ***************************************************************************** */
+#define  AUD_B_INT_MSK             0x0400D0     /* Audio Int interrupt mask */
+#define  AUD_B_INT_STAT            0x0400D4     /* Audio Int interrupt status */
+#define  AUD_B_INT_MSTAT           0x0400D8     /* Audio Int interrupt masked status */
+#define  AUD_B_INT_SSTAT           0x0400DC     /* Audio Int interrupt set status */
+
+/* ***************************************************************************** */
+#define  AUD_C_INT_MSK             0x0400E0     /* Audio Int interrupt mask */
+#define  AUD_C_INT_STAT            0x0400E4     /* Audio Int interrupt status */
+#define  AUD_C_INT_MSTAT           0x0400E8     /* Audio Int interrupt masked status */
+#define  AUD_C_INT_SSTAT           0x0400EC     /* Audio Int interrupt set status */
+
+/* ***************************************************************************** */
+#define  AUD_D_INT_MSK             0x0400F0     /* Audio Int interrupt mask */
+#define  AUD_D_INT_STAT            0x0400F4     /* Audio Int interrupt status */
+#define  AUD_D_INT_MSTAT           0x0400F8     /* Audio Int interrupt masked status */
+#define  AUD_D_INT_SSTAT           0x0400FC     /* Audio Int interrupt set status */
+
+/* ***************************************************************************** */
+#define  AUD_E_INT_MSK             0x040100     /* Audio Int interrupt mask */
+#define  AUD_E_INT_STAT            0x040104     /* Audio Int interrupt status */
+#define  AUD_E_INT_MSTAT           0x040108     /* Audio Int interrupt masked status */
+#define  AUD_E_INT_SSTAT           0x04010C     /* Audio Int interrupt set status */
+
+#define  FLD_AUD_SRC_OPC_ERR       0x00020000
+#define  FLD_AUD_DST_OPC_ERR       0x00010000
+#define  FLD_AUD_SRC_SYNC          0x00002000
+#define  FLD_AUD_DST_SYNC          0x00001000
+#define  FLD_AUD_SRC_OF            0x00000200
+#define  FLD_AUD_DST_OF            0x00000100
+#define  FLD_AUD_SRC_RISCI2        0x00000020
+#define  FLD_AUD_DST_RISCI2        0x00000010
+#define  FLD_AUD_SRC_RISCI1        0x00000002
+#define  FLD_AUD_DST_RISCI1        0x00000001
+
+/* ***************************************************************************** */
+#define  MBIF_A_INT_MSK             0x040110    /* MBIF Int interrupt mask */
+#define  MBIF_A_INT_STAT            0x040114    /* MBIF Int interrupt status */
+#define  MBIF_A_INT_MSTAT           0x040118    /* MBIF Int interrupt masked status */
+#define  MBIF_A_INT_SSTAT           0x04011C    /* MBIF Int interrupt set status */
+
+/* ***************************************************************************** */
+#define  MBIF_B_INT_MSK             0x040120    /* MBIF Int interrupt mask */
+#define  MBIF_B_INT_STAT            0x040124    /* MBIF Int interrupt status */
+#define  MBIF_B_INT_MSTAT           0x040128    /* MBIF Int interrupt masked status */
+#define  MBIF_B_INT_SSTAT           0x04012C    /* MBIF Int interrupt set status */
+
+#define  FLD_MBIF_DST_OPC_ERR       0x00010000
+#define  FLD_MBIF_DST_SYNC          0x00001000
+#define  FLD_MBIF_DST_OF            0x00000100
+#define  FLD_MBIF_DST_RISCI2        0x00000010
+#define  FLD_MBIF_DST_RISCI1        0x00000001
+
+/* ***************************************************************************** */
+#define  AUD_EXT_INT_MSK           0x040060     /* Audio Ext interrupt mask */
+#define  AUD_EXT_INT_STAT          0x040064     /* Audio Ext interrupt status */
+#define  AUD_EXT_INT_MSTAT         0x040068     /* Audio Ext interrupt masked status */
+#define  AUD_EXT_INT_SSTAT         0x04006C     /* Audio Ext interrupt set status */
+#define  FLD_AUD_EXT_OPC_ERR       0x00010000
+#define  FLD_AUD_EXT_SYNC          0x00001000
+#define  FLD_AUD_EXT_OF            0x00000100
+#define  FLD_AUD_EXT_RISCI2        0x00000010
+#define  FLD_AUD_EXT_RISCI1        0x00000001
+
+/* ***************************************************************************** */
+#define  GPIO_LO                   0x110010     /* Lower  of GPIO pins [31:0] */
+#define  GPIO_HI                   0x110014     /* Upper WORD  of GPIO pins [47:31] */
+
+#define  GPIO_LO_OE                0x110018     /* Lower  of GPIO output enable [31:0] */
+#define  GPIO_HI_OE                0x11001C     /* Upper word  of GPIO output enable [47:32] */
+
+#define  GPIO_LO_INT_MSK           0x11003C     /* GPIO interrupt mask */
+#define  GPIO_LO_INT_STAT          0x110044     /* GPIO interrupt status */
+#define  GPIO_LO_INT_MSTAT         0x11004C     /* GPIO interrupt masked status */
+#define  GPIO_LO_ISM_SNS           0x110054     /* GPIO interrupt sensitivity */
+#define  GPIO_LO_ISM_POL           0x11005C     /* GPIO interrupt polarity */
+
+#define  GPIO_HI_INT_MSK           0x110040     /* GPIO interrupt mask */
+#define  GPIO_HI_INT_STAT          0x110048     /* GPIO interrupt status */
+#define  GPIO_HI_INT_MSTAT         0x110050     /* GPIO interrupt masked status */
+#define  GPIO_HI_ISM_SNS           0x110058     /* GPIO interrupt sensitivity */
+#define  GPIO_HI_ISM_POL           0x110060     /* GPIO interrupt polarity */
+
+#define  FLD_GPIO43_INT            (1 << 11)
+#define  FLD_GPIO42_INT            (1 << 10)
+#define  FLD_GPIO41_INT            (1 << 9)
+#define  FLD_GPIO40_INT            (1 << 8)
+
+#define  FLD_GPIO9_INT             (1 << 9)
+#define  FLD_GPIO8_INT             (1 << 8)
+#define  FLD_GPIO7_INT             (1 << 7)
+#define  FLD_GPIO6_INT             (1 << 6)
+#define  FLD_GPIO5_INT             (1 << 5)
+#define  FLD_GPIO4_INT             (1 << 4)
+#define  FLD_GPIO3_INT             (1 << 3)
+#define  FLD_GPIO2_INT             (1 << 2)
+#define  FLD_GPIO1_INT             (1 << 1)
+#define  FLD_GPIO0_INT             (1 << 0)
+
+/* ***************************************************************************** */
+#define  TC_REQ                    0x040090     /* Rider PCI Express traFFic class request */
+
+/* ***************************************************************************** */
+#define  TC_REQ_SET                0x040094     /* Rider PCI Express traFFic class request set */
+
+/* ***************************************************************************** */
+/* Rider */
+/* ***************************************************************************** */
+
+/* PCI Compatible Header */
+/* ***************************************************************************** */
+#define  RDR_CFG0                  0x050000
+#define  RDR_VENDOR_DEVICE_ID_CFG  0x050000
+
+/* ***************************************************************************** */
+#define  RDR_CFG1                  0x050004
+
+/* ***************************************************************************** */
+#define  RDR_CFG2                  0x050008
+
+/* ***************************************************************************** */
+#define  RDR_CFG3                  0x05000C
+
+/* ***************************************************************************** */
+#define  RDR_CFG4                  0x050010
+
+/* ***************************************************************************** */
+#define  RDR_CFG5                  0x050014
+
+/* ***************************************************************************** */
+#define  RDR_CFG6                  0x050018
+
+/* ***************************************************************************** */
+#define  RDR_CFG7                  0x05001C
+
+/* ***************************************************************************** */
+#define  RDR_CFG8                  0x050020
+
+/* ***************************************************************************** */
+#define  RDR_CFG9                  0x050024
+
+/* ***************************************************************************** */
+#define  RDR_CFGA                  0x050028
+
+/* ***************************************************************************** */
+#define  RDR_CFGB                  0x05002C
+#define  RDR_SUSSYSTEM_ID_CFG      0x05002C
+
+/* ***************************************************************************** */
+#define  RDR_CFGC                  0x050030
+
+/* ***************************************************************************** */
+#define  RDR_CFGD                  0x050034
+
+/* ***************************************************************************** */
+#define  RDR_CFGE                  0x050038
+
+/* ***************************************************************************** */
+#define  RDR_CFGF                  0x05003C
+
+/* ***************************************************************************** */
+/* PCI-Express Capabilities */
+/* ***************************************************************************** */
+#define  RDR_PECAP                 0x050040
+
+/* ***************************************************************************** */
+#define  RDR_PEDEVCAP              0x050044
+
+/* ***************************************************************************** */
+#define  RDR_PEDEVSC               0x050048
+
+/* ***************************************************************************** */
+#define  RDR_PELINKCAP             0x05004C
+
+/* ***************************************************************************** */
+#define  RDR_PELINKSC              0x050050
+
+/* ***************************************************************************** */
+#define  RDR_PMICAP                0x050080
+
+/* ***************************************************************************** */
+#define  RDR_PMCSR                 0x050084
+
+/* ***************************************************************************** */
+#define  RDR_VPDCAP                0x050090
+
+/* ***************************************************************************** */
+#define  RDR_VPDDATA               0x050094
+
+/* ***************************************************************************** */
+#define  RDR_MSICAP                0x0500A0
+
+/* ***************************************************************************** */
+#define  RDR_MSIARL                0x0500A4
+
+/* ***************************************************************************** */
+#define  RDR_MSIARU                0x0500A8
+
+/* ***************************************************************************** */
+#define  RDR_MSIDATA               0x0500AC
+
+/* ***************************************************************************** */
+/* PCI Express Extended Capabilities */
+/* ***************************************************************************** */
+#define  RDR_AERXCAP               0x050100
+
+/* ***************************************************************************** */
+#define  RDR_AERUESTA              0x050104
+
+/* ***************************************************************************** */
+#define  RDR_AERUEMSK              0x050108
+
+/* ***************************************************************************** */
+#define  RDR_AERUESEV              0x05010C
+
+/* ***************************************************************************** */
+#define  RDR_AERCESTA              0x050110
+
+/* ***************************************************************************** */
+#define  RDR_AERCEMSK              0x050114
+
+/* ***************************************************************************** */
+#define  RDR_AERCC                 0x050118
+
+/* ***************************************************************************** */
+#define  RDR_AERHL0                0x05011C
+
+/* ***************************************************************************** */
+#define  RDR_AERHL1                0x050120
+
+/* ***************************************************************************** */
+#define  RDR_AERHL2                0x050124
+
+/* ***************************************************************************** */
+#define  RDR_AERHL3                0x050128
+
+/* ***************************************************************************** */
+#define  RDR_VCXCAP                0x050200
+
+/* ***************************************************************************** */
+#define  RDR_VCCAP1                0x050204
+
+/* ***************************************************************************** */
+#define  RDR_VCCAP2                0x050208
+
+/* ***************************************************************************** */
+#define  RDR_VCSC                  0x05020C
+
+/* ***************************************************************************** */
+#define  RDR_VCR0_CAP              0x050210
+
+/* ***************************************************************************** */
+#define  RDR_VCR0_CTRL             0x050214
+
+/* ***************************************************************************** */
+#define  RDR_VCR0_STAT             0x050218
+
+/* ***************************************************************************** */
+#define  RDR_VCR1_CAP              0x05021C
+
+/* ***************************************************************************** */
+#define  RDR_VCR1_CTRL             0x050220
+
+/* ***************************************************************************** */
+#define  RDR_VCR1_STAT             0x050224
+
+/* ***************************************************************************** */
+#define  RDR_VCR2_CAP              0x050228
+
+/* ***************************************************************************** */
+#define  RDR_VCR2_CTRL             0x05022C
+
+/* ***************************************************************************** */
+#define  RDR_VCR2_STAT             0x050230
+
+/* ***************************************************************************** */
+#define  RDR_VCR3_CAP              0x050234
+
+/* ***************************************************************************** */
+#define  RDR_VCR3_CTRL             0x050238
+
+/* ***************************************************************************** */
+#define  RDR_VCR3_STAT             0x05023C
+
+/* ***************************************************************************** */
+#define  RDR_VCARB0                0x050240
+
+/* ***************************************************************************** */
+#define  RDR_VCARB1                0x050244
+
+/* ***************************************************************************** */
+#define  RDR_VCARB2                0x050248
+
+/* ***************************************************************************** */
+#define  RDR_VCARB3                0x05024C
+
+/* ***************************************************************************** */
+#define  RDR_VCARB4                0x050250
+
+/* ***************************************************************************** */
+#define  RDR_VCARB5                0x050254
+
+/* ***************************************************************************** */
+#define  RDR_VCARB6                0x050258
+
+/* ***************************************************************************** */
+#define  RDR_VCARB7                0x05025C
+
+/* ***************************************************************************** */
+#define  RDR_RDRSTAT0              0x050300
+
+/* ***************************************************************************** */
+#define  RDR_RDRSTAT1              0x050304
+
+/* ***************************************************************************** */
+#define  RDR_RDRCTL0               0x050308
+
+/* ***************************************************************************** */
+#define  RDR_RDRCTL1               0x05030C
+
+/* ***************************************************************************** */
+/* Transaction Layer Registers */
+/* ***************************************************************************** */
+#define  RDR_TLSTAT0               0x050310
+
+/* ***************************************************************************** */
+#define  RDR_TLSTAT1               0x050314
+
+/* ***************************************************************************** */
+#define  RDR_TLCTL0                0x050318
+#define  FLD_CFG_UR_CPL_MODE       0x00000040
+#define  FLD_CFG_CORR_ERR_QUITE    0x00000020
+#define  FLD_CFG_RCB_CK_EN         0x00000010
+#define  FLD_CFG_BNDRY_CK_EN       0x00000008
+#define  FLD_CFG_BYTE_EN_CK_EN     0x00000004
+#define  FLD_CFG_RELAX_ORDER_MSK   0x00000002
+#define  FLD_CFG_TAG_ORDER_EN      0x00000001
+
+/* ***************************************************************************** */
+#define  RDR_TLCTL1                0x05031C
+
+/* ***************************************************************************** */
+#define  RDR_REQRCAL               0x050320
+
+/* ***************************************************************************** */
+#define  RDR_REQRCAU               0x050324
+
+/* ***************************************************************************** */
+#define  RDR_REQEPA                0x050328
+
+/* ***************************************************************************** */
+#define  RDR_REQCTRL               0x05032C
+
+/* ***************************************************************************** */
+#define  RDR_REQSTAT               0x050330
+
+/* ***************************************************************************** */
+#define  RDR_TL_TEST               0x050334
+
+/* ***************************************************************************** */
+#define  RDR_VCR01_CTL             0x050348
+
+/* ***************************************************************************** */
+#define  RDR_VCR23_CTL             0x05034C
+
+/* ***************************************************************************** */
+#define  RDR_RX_VCR0_FC            0x050350
+
+/* ***************************************************************************** */
+#define  RDR_RX_VCR1_FC            0x050354
+
+/* ***************************************************************************** */
+#define  RDR_RX_VCR2_FC            0x050358
+
+/* ***************************************************************************** */
+#define  RDR_RX_VCR3_FC            0x05035C
+
+/* ***************************************************************************** */
+/* Data Link Layer Registers */
+/* ***************************************************************************** */
+#define  RDR_DLLSTAT               0x050360
+
+/* ***************************************************************************** */
+#define  RDR_DLLCTRL               0x050364
+
+/* ***************************************************************************** */
+#define  RDR_REPLAYTO              0x050368
+
+/* ***************************************************************************** */
+#define  RDR_ACKLATTO              0x05036C
+
+/* ***************************************************************************** */
+/* MAC Layer Registers */
+/* ***************************************************************************** */
+#define  RDR_MACSTAT0              0x050380
+
+/* ***************************************************************************** */
+#define  RDR_MACSTAT1              0x050384
+
+/* ***************************************************************************** */
+#define  RDR_MACCTRL0              0x050388
+
+/* ***************************************************************************** */
+#define  RDR_MACCTRL1              0x05038C
+
+/* ***************************************************************************** */
+#define  RDR_MACCTRL2              0x050390
+
+/* ***************************************************************************** */
+#define  RDR_MAC_LB_DATA           0x050394
+
+/* ***************************************************************************** */
+#define  RDR_L0S_EXIT_LAT          0x050398
+
+/* ***************************************************************************** */
+/* DMAC */
+/* ***************************************************************************** */
+#define  DMA1_PTR1                 0x100000     /* DMA Current Ptr : Ch#1 */
+
+/* ***************************************************************************** */
+#define  DMA2_PTR1                 0x100004     /* DMA Current Ptr : Ch#2 */
+
+/* ***************************************************************************** */
+#define  DMA3_PTR1                 0x100008     /* DMA Current Ptr : Ch#3 */
+
+/* ***************************************************************************** */
+#define  DMA4_PTR1                 0x10000C     /* DMA Current Ptr : Ch#4 */
+
+/* ***************************************************************************** */
+#define  DMA5_PTR1                 0x100010     /* DMA Current Ptr : Ch#5 */
+
+/* ***************************************************************************** */
+#define  DMA6_PTR1                 0x100014     /* DMA Current Ptr : Ch#6 */
+
+/* ***************************************************************************** */
+#define  DMA7_PTR1                 0x100018     /* DMA Current Ptr : Ch#7 */
+
+/* ***************************************************************************** */
+#define  DMA8_PTR1                 0x10001C     /* DMA Current Ptr : Ch#8 */
+
+/* ***************************************************************************** */
+#define  DMA9_PTR1                 0x100020     /* DMA Current Ptr : Ch#9 */
+
+/* ***************************************************************************** */
+#define  DMA10_PTR1                0x100024     /* DMA Current Ptr : Ch#10 */
+
+/* ***************************************************************************** */
+#define  DMA11_PTR1                0x100028     /* DMA Current Ptr : Ch#11 */
+
+/* ***************************************************************************** */
+#define  DMA12_PTR1                0x10002C     /* DMA Current Ptr : Ch#12 */
+
+/* ***************************************************************************** */
+#define  DMA13_PTR1                0x100030     /* DMA Current Ptr : Ch#13 */
+
+/* ***************************************************************************** */
+#define  DMA14_PTR1                0x100034     /* DMA Current Ptr : Ch#14 */
+
+/* ***************************************************************************** */
+#define  DMA15_PTR1                0x100038     /* DMA Current Ptr : Ch#15 */
+
+/* ***************************************************************************** */
+#define  DMA16_PTR1                0x10003C     /* DMA Current Ptr : Ch#16 */
+
+/* ***************************************************************************** */
+#define  DMA17_PTR1                0x100040     /* DMA Current Ptr : Ch#17 */
+
+/* ***************************************************************************** */
+#define  DMA18_PTR1                0x100044     /* DMA Current Ptr : Ch#18 */
+
+/* ***************************************************************************** */
+#define  DMA19_PTR1                0x100048     /* DMA Current Ptr : Ch#19 */
+
+/* ***************************************************************************** */
+#define  DMA20_PTR1                0x10004C     /* DMA Current Ptr : Ch#20 */
+
+/* ***************************************************************************** */
+#define  DMA21_PTR1                0x100050     /* DMA Current Ptr : Ch#21 */
+
+/* ***************************************************************************** */
+#define  DMA22_PTR1                0x100054     /* DMA Current Ptr : Ch#22 */
+
+/* ***************************************************************************** */
+#define  DMA23_PTR1                0x100058     /* DMA Current Ptr : Ch#23 */
+
+/* ***************************************************************************** */
+#define  DMA24_PTR1                0x10005C     /* DMA Current Ptr : Ch#24 */
+
+/* ***************************************************************************** */
+#define  DMA25_PTR1                0x100060     /* DMA Current Ptr : Ch#25 */
+
+/* ***************************************************************************** */
+#define  DMA26_PTR1                0x100064     /* DMA Current Ptr : Ch#26 */
+
+/* ***************************************************************************** */
+#define  DMA1_PTR2                 0x100080     /* DMA Tab Ptr : Ch#1 */
+
+/* ***************************************************************************** */
+#define  DMA2_PTR2                 0x100084     /* DMA Tab Ptr : Ch#2 */
+
+/* ***************************************************************************** */
+#define  DMA3_PTR2                 0x100088     /* DMA Tab Ptr : Ch#3 */
+
+/* ***************************************************************************** */
+#define  DMA4_PTR2                 0x10008C     /* DMA Tab Ptr : Ch#4 */
+
+/* ***************************************************************************** */
+#define  DMA5_PTR2                 0x100090     /* DMA Tab Ptr : Ch#5 */
+
+/* ***************************************************************************** */
+#define  DMA6_PTR2                 0x100094     /* DMA Tab Ptr : Ch#6 */
+
+/* ***************************************************************************** */
+#define  DMA7_PTR2                 0x100098     /* DMA Tab Ptr : Ch#7 */
+
+/* ***************************************************************************** */
+#define  DMA8_PTR2                 0x10009C     /* DMA Tab Ptr : Ch#8 */
+
+/* ***************************************************************************** */
+#define  DMA9_PTR2                 0x1000A0     /* DMA Tab Ptr : Ch#9 */
+
+/* ***************************************************************************** */
+#define  DMA10_PTR2                0x1000A4     /* DMA Tab Ptr : Ch#10 */
+
+/* ***************************************************************************** */
+#define  DMA11_PTR2                0x1000A8     /* DMA Tab Ptr : Ch#11 */
+
+/* ***************************************************************************** */
+#define  DMA12_PTR2                0x1000AC     /* DMA Tab Ptr : Ch#12 */
+
+/* ***************************************************************************** */
+#define  DMA13_PTR2                0x1000B0     /* DMA Tab Ptr : Ch#13 */
+
+/* ***************************************************************************** */
+#define  DMA14_PTR2                0x1000B4     /* DMA Tab Ptr : Ch#14 */
+
+/* ***************************************************************************** */
+#define  DMA15_PTR2                0x1000B8     /* DMA Tab Ptr : Ch#15 */
+
+/* ***************************************************************************** */
+#define  DMA16_PTR2                0x1000BC     /* DMA Tab Ptr : Ch#16 */
+
+/* ***************************************************************************** */
+#define  DMA17_PTR2                0x1000C0     /* DMA Tab Ptr : Ch#17 */
+
+/* ***************************************************************************** */
+#define  DMA18_PTR2                0x1000C4     /* DMA Tab Ptr : Ch#18 */
+
+/* ***************************************************************************** */
+#define  DMA19_PTR2                0x1000C8     /* DMA Tab Ptr : Ch#19 */
+
+/* ***************************************************************************** */
+#define  DMA20_PTR2                0x1000CC     /* DMA Tab Ptr : Ch#20 */
+
+/* ***************************************************************************** */
+#define  DMA21_PTR2                0x1000D0     /* DMA Tab Ptr : Ch#21 */
+
+/* ***************************************************************************** */
+#define  DMA22_PTR2                0x1000D4     /* DMA Tab Ptr : Ch#22 */
+
+/* ***************************************************************************** */
+#define  DMA23_PTR2                0x1000D8     /* DMA Tab Ptr : Ch#23 */
+
+/* ***************************************************************************** */
+#define  DMA24_PTR2                0x1000DC     /* DMA Tab Ptr : Ch#24 */
+
+/* ***************************************************************************** */
+#define  DMA25_PTR2                0x1000E0     /* DMA Tab Ptr : Ch#25 */
+
+/* ***************************************************************************** */
+#define  DMA26_PTR2                0x1000E4     /* DMA Tab Ptr : Ch#26 */
+
+/* ***************************************************************************** */
+#define  DMA1_CNT1                 0x100100     /* DMA BuFFer Size : Ch#1 */
+
+/* ***************************************************************************** */
+#define  DMA2_CNT1                 0x100104     /* DMA BuFFer Size : Ch#2 */
+
+/* ***************************************************************************** */
+#define  DMA3_CNT1                 0x100108     /* DMA BuFFer Size : Ch#3 */
+
+/* ***************************************************************************** */
+#define  DMA4_CNT1                 0x10010C     /* DMA BuFFer Size : Ch#4 */
+
+/* ***************************************************************************** */
+#define  DMA5_CNT1                 0x100110     /* DMA BuFFer Size : Ch#5 */
+
+/* ***************************************************************************** */
+#define  DMA6_CNT1                 0x100114     /* DMA BuFFer Size : Ch#6 */
+
+/* ***************************************************************************** */
+#define  DMA7_CNT1                 0x100118     /* DMA BuFFer Size : Ch#7 */
+
+/* ***************************************************************************** */
+#define  DMA8_CNT1                 0x10011C     /* DMA BuFFer Size : Ch#8 */
+
+/* ***************************************************************************** */
+#define  DMA9_CNT1                 0x100120     /* DMA BuFFer Size : Ch#9 */
+
+/* ***************************************************************************** */
+#define  DMA10_CNT1                0x100124     /* DMA BuFFer Size : Ch#10 */
+
+/* ***************************************************************************** */
+#define  DMA11_CNT1                0x100128     /* DMA BuFFer Size : Ch#11 */
+
+/* ***************************************************************************** */
+#define  DMA12_CNT1                0x10012C     /* DMA BuFFer Size : Ch#12 */
+
+/* ***************************************************************************** */
+#define  DMA13_CNT1                0x100130     /* DMA BuFFer Size : Ch#13 */
+
+/* ***************************************************************************** */
+#define  DMA14_CNT1                0x100134     /* DMA BuFFer Size : Ch#14 */
+
+/* ***************************************************************************** */
+#define  DMA15_CNT1                0x100138     /* DMA BuFFer Size : Ch#15 */
+
+/* ***************************************************************************** */
+#define  DMA16_CNT1                0x10013C     /* DMA BuFFer Size : Ch#16 */
+
+/* ***************************************************************************** */
+#define  DMA17_CNT1                0x100140     /* DMA BuFFer Size : Ch#17 */
+
+/* ***************************************************************************** */
+#define  DMA18_CNT1                0x100144     /* DMA BuFFer Size : Ch#18 */
+
+/* ***************************************************************************** */
+#define  DMA19_CNT1                0x100148     /* DMA BuFFer Size : Ch#19 */
+
+/* ***************************************************************************** */
+#define  DMA20_CNT1                0x10014C     /* DMA BuFFer Size : Ch#20 */
+
+/* ***************************************************************************** */
+#define  DMA21_CNT1                0x100150     /* DMA BuFFer Size : Ch#21 */
+
+/* ***************************************************************************** */
+#define  DMA22_CNT1                0x100154     /* DMA BuFFer Size : Ch#22 */
+
+/* ***************************************************************************** */
+#define  DMA23_CNT1                0x100158     /* DMA BuFFer Size : Ch#23 */
+
+/* ***************************************************************************** */
+#define  DMA24_CNT1                0x10015C     /* DMA BuFFer Size : Ch#24 */
+
+/* ***************************************************************************** */
+#define  DMA25_CNT1                0x100160     /* DMA BuFFer Size : Ch#25 */
+
+/* ***************************************************************************** */
+#define  DMA26_CNT1                0x100164     /* DMA BuFFer Size : Ch#26 */
+
+/* ***************************************************************************** */
+#define  DMA1_CNT2                 0x100180     /* DMA Table Size : Ch#1 */
+
+/* ***************************************************************************** */
+#define  DMA2_CNT2                 0x100184     /* DMA Table Size : Ch#2 */
+
+/* ***************************************************************************** */
+#define  DMA3_CNT2                 0x100188     /* DMA Table Size : Ch#3 */
+
+/* ***************************************************************************** */
+#define  DMA4_CNT2                 0x10018C     /* DMA Table Size : Ch#4 */
+
+/* ***************************************************************************** */
+#define  DMA5_CNT2                 0x100190     /* DMA Table Size : Ch#5 */
+
+/* ***************************************************************************** */
+#define  DMA6_CNT2                 0x100194     /* DMA Table Size : Ch#6 */
+
+/* ***************************************************************************** */
+#define  DMA7_CNT2                 0x100198     /* DMA Table Size : Ch#7 */
+
+/* ***************************************************************************** */
+#define  DMA8_CNT2                 0x10019C     /* DMA Table Size : Ch#8 */
+
+/* ***************************************************************************** */
+#define  DMA9_CNT2                 0x1001A0     /* DMA Table Size : Ch#9 */
+
+/* ***************************************************************************** */
+#define  DMA10_CNT2                0x1001A4     /* DMA Table Size : Ch#10 */
+
+/* ***************************************************************************** */
+#define  DMA11_CNT2                0x1001A8     /* DMA Table Size : Ch#11 */
+
+/* ***************************************************************************** */
+#define  DMA12_CNT2                0x1001AC     /* DMA Table Size : Ch#12 */
+
+/* ***************************************************************************** */
+#define  DMA13_CNT2                0x1001B0     /* DMA Table Size : Ch#13 */
+
+/* ***************************************************************************** */
+#define  DMA14_CNT2                0x1001B4     /* DMA Table Size : Ch#14 */
+
+/* ***************************************************************************** */
+#define  DMA15_CNT2                0x1001B8     /* DMA Table Size : Ch#15 */
+
+/* ***************************************************************************** */
+#define  DMA16_CNT2                0x1001BC     /* DMA Table Size : Ch#16 */
+
+/* ***************************************************************************** */
+#define  DMA17_CNT2                0x1001C0     /* DMA Table Size : Ch#17 */
+
+/* ***************************************************************************** */
+#define  DMA18_CNT2                0x1001C4     /* DMA Table Size : Ch#18 */
+
+/* ***************************************************************************** */
+#define  DMA19_CNT2                0x1001C8     /* DMA Table Size : Ch#19 */
+
+/* ***************************************************************************** */
+#define  DMA20_CNT2                0x1001CC     /* DMA Table Size : Ch#20 */
+
+/* ***************************************************************************** */
+#define  DMA21_CNT2                0x1001D0     /* DMA Table Size : Ch#21 */
+
+/* ***************************************************************************** */
+#define  DMA22_CNT2                0x1001D4     /* DMA Table Size : Ch#22 */
+
+/* ***************************************************************************** */
+#define  DMA23_CNT2                0x1001D8     /* DMA Table Size : Ch#23 */
+
+/* ***************************************************************************** */
+#define  DMA24_CNT2                0x1001DC     /* DMA Table Size : Ch#24 */
+
+/* ***************************************************************************** */
+#define  DMA25_CNT2                0x1001E0     /* DMA Table Size : Ch#25 */
+
+/* ***************************************************************************** */
+#define  DMA26_CNT2                0x1001E4     /* DMA Table Size : Ch#26 */
+
+/* ***************************************************************************** */
+ /* ITG */
+/* ***************************************************************************** */
+#define  TM_CNT_LDW                0x110000     /* Timer : Counter low */
+
+/* ***************************************************************************** */
+#define  TM_CNT_UW                 0x110004     /* Timer : Counter high word */
+
+/* ***************************************************************************** */
+#define  TM_LMT_LDW                0x110008     /* Timer : Limit low */
+
+/* ***************************************************************************** */
+#define  TM_LMT_UW                 0x11000C     /* Timer : Limit high word */
+
+/* ***************************************************************************** */
+#define  GP0_IO                    0x110010     /* GPIO output enables data I/O */
+#define  FLD_GP_OE                 0x00FF0000   /* GPIO: GP_OE output enable */
+#define  FLD_GP_IN                 0x0000FF00   /* GPIO: GP_IN status */
+#define  FLD_GP_OUT                0x000000FF   /* GPIO: GP_OUT control */
+
+/* ***************************************************************************** */
+#define  GPIO_ISM                  0x110014     /* GPIO interrupt sensitivity mode */
+#define  FLD_GP_ISM_SNS            0x00000070
+#define  FLD_GP_ISM_POL            0x00000007
+
+/* ***************************************************************************** */
+#define  SOFT_RESET                0x11001C     /* Output system reset reg */
+#define  FLD_PECOS_SOFT_RESET      0x00000001
+
+/* ***************************************************************************** */
+#define  MC416_RWD                 0x110020     /* MC416 GPIO[18:3] pin */
+#define  MC416_OEN                 0x110024     /* Output enable of GPIO[18:3] */
+#define  MC416_CTL                 0x110028
+
+/* ***************************************************************************** */
+#define  ALT_PIN_OUT_SEL           0x11002C     /* Alternate GPIO output select */
+
+#define  FLD_ALT_GPIO_OUT_SEL      0xF0000000
+/* 0          Disabled <-- default */
+/* 1          GPIO[0] */
+/* 2          GPIO[10] */
+/* 3          VIP_656_DATA_VAL */
+/* 4          VIP_656_DATA[0] */
+/* 5          VIP_656_CLK */
+/* 6          VIP_656_DATA_EXT[1] */
+/* 7          VIP_656_DATA_EXT[0] */
+/* 8          ATT_IF */
+
+#define  FLD_AUX_PLL_CLK_ALT_SEL   0x0F000000
+/* 0          AUX_PLL_CLK<-- default */
+/* 1          GPIO[2] */
+/* 2          GPIO[10] */
+/* 3          VIP_656_DATA_VAL */
+/* 4          VIP_656_DATA[0] */
+/* 5          VIP_656_CLK */
+/* 6          VIP_656_DATA_EXT[1] */
+/* 7          VIP_656_DATA_EXT[0] */
+
+#define  FLD_IR_TX_ALT_SEL         0x00F00000
+/* 0          IR_TX <-- default */
+/* 1          GPIO[1] */
+/* 2          GPIO[10] */
+/* 3          VIP_656_DATA_VAL */
+/* 4          VIP_656_DATA[0] */
+/* 5          VIP_656_CLK */
+/* 6          VIP_656_DATA_EXT[1] */
+/* 7          VIP_656_DATA_EXT[0] */
+
+#define  FLD_IR_RX_ALT_SEL         0x000F0000
+/* 0          IR_RX <-- default */
+/* 1          GPIO[0] */
+/* 2          GPIO[10] */
+/* 3          VIP_656_DATA_VAL */
+/* 4          VIP_656_DATA[0] */
+/* 5          VIP_656_CLK */
+/* 6          VIP_656_DATA_EXT[1] */
+/* 7          VIP_656_DATA_EXT[0] */
+
+#define  FLD_GPIO10_ALT_SEL        0x0000F000
+/* 0          GPIO[10] <-- default */
+/* 1          GPIO[0] */
+/* 2          GPIO[10] */
+/* 3          VIP_656_DATA_VAL */
+/* 4          VIP_656_DATA[0] */
+/* 5          VIP_656_CLK */
+/* 6          VIP_656_DATA_EXT[1] */
+/* 7          VIP_656_DATA_EXT[0] */
+
+#define  FLD_GPIO2_ALT_SEL         0x00000F00
+/* 0          GPIO[2] <-- default */
+/* 1          GPIO[1] */
+/* 2          GPIO[10] */
+/* 3          VIP_656_DATA_VAL */
+/* 4          VIP_656_DATA[0] */
+/* 5          VIP_656_CLK */
+/* 6          VIP_656_DATA_EXT[1] */
+/* 7          VIP_656_DATA_EXT[0] */
+
+#define  FLD_GPIO1_ALT_SEL         0x000000F0
+/* 0          GPIO[1] <-- default */
+/* 1          GPIO[0] */
+/* 2          GPIO[10] */
+/* 3          VIP_656_DATA_VAL */
+/* 4          VIP_656_DATA[0] */
+/* 5          VIP_656_CLK */
+/* 6          VIP_656_DATA_EXT[1] */
+/* 7          VIP_656_DATA_EXT[0] */
+
+#define  FLD_GPIO0_ALT_SEL         0x0000000F
+/* 0          GPIO[0] <-- default */
+/* 1          GPIO[1] */
+/* 2          GPIO[10] */
+/* 3          VIP_656_DATA_VAL */
+/* 4          VIP_656_DATA[0] */
+/* 5          VIP_656_CLK */
+/* 6          VIP_656_DATA_EXT[1] */
+/* 7          VIP_656_DATA_EXT[0] */
+
+#define  ALT_PIN_IN_SEL            0x110030     /* Alternate GPIO input select */
+
+#define  FLD_GPIO10_ALT_IN_SEL     0x0000F000
+/* 0          GPIO[10] <-- default */
+/* 1          IR_RX */
+/* 2          IR_TX */
+/* 3          AUX_PLL_CLK */
+/* 4          IF_ATT_SEL */
+/* 5          GPIO[0] */
+/* 6          GPIO[1] */
+/* 7          GPIO[2] */
+
+#define  FLD_GPIO2_ALT_IN_SEL      0x00000F00
+/* 0          GPIO[2] <-- default */
+/* 1          IR_RX */
+/* 2          IR_TX */
+/* 3          AUX_PLL_CLK */
+/* 4          IF_ATT_SEL */
+
+#define  FLD_GPIO1_ALT_IN_SEL      0x000000F0
+/* 0          GPIO[1] <-- default */
+/* 1          IR_RX */
+/* 2          IR_TX */
+/* 3          AUX_PLL_CLK */
+/* 4          IF_ATT_SEL */
+
+#define  FLD_GPIO0_ALT_IN_SEL      0x0000000F
+/* 0          GPIO[0] <-- default */
+/* 1          IR_RX */
+/* 2          IR_TX */
+/* 3          AUX_PLL_CLK */
+/* 4          IF_ATT_SEL */
+
+/* ***************************************************************************** */
+#define  TEST_BUS_CTL1             0x110040     /* Test bus control register #1 */
+
+/* ***************************************************************************** */
+#define  TEST_BUS_CTL2             0x110044     /* Test bus control register #2 */
+
+/* ***************************************************************************** */
+#define  CLK_DELAY                 0x110048     /* Clock delay */
+#define  FLD_MOE_CLK_DIS           0x80000000   /* Disable MoE clock */
+
+/* ***************************************************************************** */
+#define  PAD_CTRL                  0x110068     /* Pad drive strength control */
+
+/* ***************************************************************************** */
+#define  MBIST_CTRL                0x110050     /* SRAM memory built-in self test control */
+
+/* ***************************************************************************** */
+#define  MBIST_STAT                0x110054     /* SRAM memory built-in self test status */
+
+/* ***************************************************************************** */
+/* PLL registers */
+/* ***************************************************************************** */
+#define  PLL_A_INT_FRAC            0x110088
+#define  PLL_A_POST_STAT_BIST      0x11008C
+#define  PLL_B_INT_FRAC            0x110090
+#define  PLL_B_POST_STAT_BIST      0x110094
+#define  PLL_C_INT_FRAC            0x110098
+#define  PLL_C_POST_STAT_BIST      0x11009C
+#define  PLL_D_INT_FRAC            0x1100A0
+#define  PLL_D_POST_STAT_BIST      0x1100A4
+
+#define  CLK_RST                   0x11002C
+#define  FLD_VID_I_CLK_NOE         0x00001000
+#define  FLD_VID_J_CLK_NOE         0x00002000
+#define  FLD_USE_ALT_PLL_REF       0x00004000
+
+#define  VID_CH_MODE_SEL           0x110078
+#define  VID_CH_CLK_SEL            0x11007C
+
+/* ***************************************************************************** */
+#define  VBI_A_DMA                 0x130008     /* VBI A DMA data port */
+
+/* ***************************************************************************** */
+#define  VID_A_VIP_CTL             0x130080     /* Video A VIP format control */
+#define  FLD_VIP_MODE              0x00000001
+
+/* ***************************************************************************** */
+#define  VID_A_PIXEL_FRMT          0x130084     /* Video A pixel format */
+#define  FLD_VID_A_GAMMA_DIS       0x00000008
+#define  FLD_VID_A_FORMAT          0x00000007
+#define  FLD_VID_A_GAMMA_FACTOR    0x00000010
+
+/* ***************************************************************************** */
+#define  VID_A_VBI_CTL             0x130088     /* Video A VBI miscellaneous control */
+#define  FLD_VID_A_VIP_EXT         0x00000003
+
+/* ***************************************************************************** */
+#define  VID_B_DMA                 0x130100     /* Video B DMA data port */
+
+/* ***************************************************************************** */
+#define  VBI_B_DMA                 0x130108     /* VBI B DMA data port */
+
+/* ***************************************************************************** */
+#define  VID_B_SRC_SEL             0x130144     /* Video B source select */
+#define  FLD_VID_B_SRC_SEL         0x00000000
+
+/* ***************************************************************************** */
+#define  VID_B_LNGTH               0x130150     /* Video B line length */
+#define  FLD_VID_B_LN_LNGTH        0x00000FFF
+
+/* ***************************************************************************** */
+#define  VID_B_VIP_CTL             0x130180     /* Video B VIP format control */
+
+/* ***************************************************************************** */
+#define  VID_B_PIXEL_FRMT          0x130184     /* Video B pixel format */
+#define  FLD_VID_B_GAMMA_DIS       0x00000008
+#define  FLD_VID_B_FORMAT          0x00000007
+#define  FLD_VID_B_GAMMA_FACTOR    0x00000010
+
+/* ***************************************************************************** */
+#define  VID_C_DMA                 0x130200     /* Video C DMA data port */
+
+/* ***************************************************************************** */
+#define  VID_C_LNGTH               0x130250     /* Video C line length */
+#define  FLD_VID_C_LN_LNGTH        0x00000FFF
+
+/* ***************************************************************************** */
+/* Video Destination Channels */
+/* ***************************************************************************** */
+
+#define  VID_DST_A_GPCNT           0x130020     /* Video A general purpose counter */
+#define  VID_DST_B_GPCNT           0x130120     /* Video B general purpose counter */
+#define  VID_DST_C_GPCNT           0x130220     /* Video C general purpose counter */
+#define  VID_DST_D_GPCNT           0x130320     /* Video D general purpose counter */
+#define  VID_DST_E_GPCNT           0x130420     /* Video E general purpose counter */
+#define  VID_DST_F_GPCNT           0x130520     /* Video F general purpose counter */
+#define  VID_DST_G_GPCNT           0x130620     /* Video G general purpose counter */
+#define  VID_DST_H_GPCNT           0x130720     /* Video H general purpose counter */
+
+/* ***************************************************************************** */
+
+#define  VID_DST_A_GPCNT_CTL       0x130030     /* Video A general purpose control */
+#define  VID_DST_B_GPCNT_CTL       0x130130     /* Video B general purpose control */
+#define  VID_DST_C_GPCNT_CTL       0x130230     /* Video C general purpose control */
+#define  VID_DST_D_GPCNT_CTL       0x130330     /* Video D general purpose control */
+#define  VID_DST_E_GPCNT_CTL       0x130430     /* Video E general purpose control */
+#define  VID_DST_F_GPCNT_CTL       0x130530     /* Video F general purpose control */
+#define  VID_DST_G_GPCNT_CTL       0x130630     /* Video G general purpose control */
+#define  VID_DST_H_GPCNT_CTL       0x130730     /* Video H general purpose control */
+
+/* ***************************************************************************** */
+
+#define  VID_DST_A_DMA_CTL         0x130040     /* Video A DMA control */
+#define  VID_DST_B_DMA_CTL         0x130140     /* Video B DMA control */
+#define  VID_DST_C_DMA_CTL         0x130240     /* Video C DMA control */
+#define  VID_DST_D_DMA_CTL         0x130340     /* Video D DMA control */
+#define  VID_DST_E_DMA_CTL         0x130440     /* Video E DMA control */
+#define  VID_DST_F_DMA_CTL         0x130540     /* Video F DMA control */
+#define  VID_DST_G_DMA_CTL         0x130640     /* Video G DMA control */
+#define  VID_DST_H_DMA_CTL         0x130740     /* Video H DMA control */
+
+#define  FLD_VID_RISC_EN           0x00000010
+#define  FLD_VID_FIFO_EN           0x00000001
+
+/* ***************************************************************************** */
+
+#define  VID_DST_A_VIP_CTL         0x130080     /* Video A VIP control */
+#define  VID_DST_B_VIP_CTL         0x130180     /* Video B VIP control */
+#define  VID_DST_C_VIP_CTL         0x130280     /* Video C VIP control */
+#define  VID_DST_D_VIP_CTL         0x130380     /* Video D VIP control */
+#define  VID_DST_E_VIP_CTL         0x130480     /* Video E VIP control */
+#define  VID_DST_F_VIP_CTL         0x130580     /* Video F VIP control */
+#define  VID_DST_G_VIP_CTL         0x130680     /* Video G VIP control */
+#define  VID_DST_H_VIP_CTL         0x130780     /* Video H VIP control */
+
+/* ***************************************************************************** */
+
+#define  VID_DST_A_PIX_FRMT        0x130084     /* Video A Pixel format */
+#define  VID_DST_B_PIX_FRMT        0x130184     /* Video B Pixel format */
+#define  VID_DST_C_PIX_FRMT        0x130284     /* Video C Pixel format */
+#define  VID_DST_D_PIX_FRMT        0x130384     /* Video D Pixel format */
+#define  VID_DST_E_PIX_FRMT        0x130484     /* Video E Pixel format */
+#define  VID_DST_F_PIX_FRMT        0x130584     /* Video F Pixel format */
+#define  VID_DST_G_PIX_FRMT        0x130684     /* Video G Pixel format */
+#define  VID_DST_H_PIX_FRMT        0x130784     /* Video H Pixel format */
+
+/* ***************************************************************************** */
+/* Video Source Channels */
+/* ***************************************************************************** */
+
+#define  VID_SRC_A_GPCNT_CTL       0x130804     /* Video A general purpose control */
+#define  VID_SRC_B_GPCNT_CTL       0x130904     /* Video B general purpose control */
+#define  VID_SRC_C_GPCNT_CTL       0x130A04     /* Video C general purpose control */
+#define  VID_SRC_D_GPCNT_CTL       0x130B04     /* Video D general purpose control */
+#define  VID_SRC_E_GPCNT_CTL       0x130C04     /* Video E general purpose control */
+#define  VID_SRC_F_GPCNT_CTL       0x130D04     /* Video F general purpose control */
+#define  VID_SRC_I_GPCNT_CTL       0x130E04     /* Video I general purpose control */
+#define  VID_SRC_J_GPCNT_CTL       0x130F04     /* Video J general purpose control */
+
+/* ***************************************************************************** */
+
+#define  VID_SRC_A_GPCNT           0x130808     /* Video A general purpose counter */
+#define  VID_SRC_B_GPCNT           0x130908     /* Video B general purpose counter */
+#define  VID_SRC_C_GPCNT           0x130A08     /* Video C general purpose counter */
+#define  VID_SRC_D_GPCNT           0x130B08     /* Video D general purpose counter */
+#define  VID_SRC_E_GPCNT           0x130C08     /* Video E general purpose counter */
+#define  VID_SRC_F_GPCNT           0x130D08     /* Video F general purpose counter */
+#define  VID_SRC_I_GPCNT           0x130E08     /* Video I general purpose counter */
+#define  VID_SRC_J_GPCNT           0x130F08     /* Video J general purpose counter */
+
+/* ***************************************************************************** */
+
+#define  VID_SRC_A_DMA_CTL         0x13080C     /* Video A DMA control */
+#define  VID_SRC_B_DMA_CTL         0x13090C     /* Video B DMA control */
+#define  VID_SRC_C_DMA_CTL         0x130A0C     /* Video C DMA control */
+#define  VID_SRC_D_DMA_CTL         0x130B0C     /* Video D DMA control */
+#define  VID_SRC_E_DMA_CTL         0x130C0C     /* Video E DMA control */
+#define  VID_SRC_F_DMA_CTL         0x130D0C     /* Video F DMA control */
+#define  VID_SRC_I_DMA_CTL         0x130E0C     /* Video I DMA control */
+#define  VID_SRC_J_DMA_CTL         0x130F0C     /* Video J DMA control */
+
+#define  FLD_APB_RISC_EN           0x00000010
+#define  FLD_APB_FIFO_EN           0x00000001
+
+/* ***************************************************************************** */
+
+#define  VID_SRC_A_FMT_CTL         0x130810     /* Video A format control */
+#define  VID_SRC_B_FMT_CTL         0x130910     /* Video B format control */
+#define  VID_SRC_C_FMT_CTL         0x130A10     /* Video C format control */
+#define  VID_SRC_D_FMT_CTL         0x130B10     /* Video D format control */
+#define  VID_SRC_E_FMT_CTL         0x130C10     /* Video E format control */
+#define  VID_SRC_F_FMT_CTL         0x130D10     /* Video F format control */
+#define  VID_SRC_I_FMT_CTL         0x130E10     /* Video I format control */
+#define  VID_SRC_J_FMT_CTL         0x130F10     /* Video J format control */
+
+/* ***************************************************************************** */
+
+#define  VID_SRC_A_ACTIVE_CTL1     0x130814     /* Video A active control      1 */
+#define  VID_SRC_B_ACTIVE_CTL1     0x130914     /* Video B active control      1 */
+#define  VID_SRC_C_ACTIVE_CTL1     0x130A14     /* Video C active control      1 */
+#define  VID_SRC_D_ACTIVE_CTL1     0x130B14     /* Video D active control      1 */
+#define  VID_SRC_E_ACTIVE_CTL1     0x130C14     /* Video E active control      1 */
+#define  VID_SRC_F_ACTIVE_CTL1     0x130D14     /* Video F active control      1 */
+#define  VID_SRC_I_ACTIVE_CTL1     0x130E14     /* Video I active control      1 */
+#define  VID_SRC_J_ACTIVE_CTL1     0x130F14     /* Video J active control      1 */
+
+/* ***************************************************************************** */
+
+#define  VID_SRC_A_ACTIVE_CTL2     0x130818     /* Video A active control      2 */
+#define  VID_SRC_B_ACTIVE_CTL2     0x130918     /* Video B active control      2 */
+#define  VID_SRC_C_ACTIVE_CTL2     0x130A18     /* Video C active control      2 */
+#define  VID_SRC_D_ACTIVE_CTL2     0x130B18     /* Video D active control      2 */
+#define  VID_SRC_E_ACTIVE_CTL2     0x130C18     /* Video E active control      2 */
+#define  VID_SRC_F_ACTIVE_CTL2     0x130D18     /* Video F active control      2 */
+#define  VID_SRC_I_ACTIVE_CTL2     0x130E18     /* Video I active control      2 */
+#define  VID_SRC_J_ACTIVE_CTL2     0x130F18     /* Video J active control      2 */
+
+/* ***************************************************************************** */
+
+#define  VID_SRC_A_CDT_SZ          0x13081C     /* Video A CDT size */
+#define  VID_SRC_B_CDT_SZ          0x13091C     /* Video B CDT size */
+#define  VID_SRC_C_CDT_SZ          0x130A1C     /* Video C CDT size */
+#define  VID_SRC_D_CDT_SZ          0x130B1C     /* Video D CDT size */
+#define  VID_SRC_E_CDT_SZ          0x130C1C     /* Video E CDT size */
+#define  VID_SRC_F_CDT_SZ          0x130D1C     /* Video F CDT size */
+#define  VID_SRC_I_CDT_SZ          0x130E1C     /* Video I CDT size */
+#define  VID_SRC_J_CDT_SZ          0x130F1C     /* Video J CDT size */
+
+/* ***************************************************************************** */
+/* Audio I/F */
+/* ***************************************************************************** */
+#define  AUD_DST_A_DMA             0x140000     /* Audio Int A DMA data port */
+#define  AUD_SRC_A_DMA             0x140008     /* Audio Int A DMA data port */
+
+#define  AUD_A_GPCNT               0x140010     /* Audio Int A gp counter */
+#define  FLD_AUD_A_GP_CNT          0x0000FFFF
+
+#define  AUD_A_GPCNT_CTL           0x140014     /* Audio Int A gp control */
+
+#define  AUD_A_LNGTH               0x140018     /* Audio Int A line length */
+
+#define  AUD_A_CFG                 0x14001C     /* Audio Int A configuration */
+
+/* ***************************************************************************** */
+#define  AUD_DST_B_DMA             0x140100     /* Audio Int B DMA data port */
+#define  AUD_SRC_B_DMA             0x140108     /* Audio Int B DMA data port */
+
+#define  AUD_B_GPCNT               0x140110     /* Audio Int B gp counter */
+#define  FLD_AUD_B_GP_CNT          0x0000FFFF
+
+#define  AUD_B_GPCNT_CTL           0x140114     /* Audio Int B gp control */
+
+#define  AUD_B_LNGTH               0x140118     /* Audio Int B line length */
+
+#define  AUD_B_CFG                 0x14011C     /* Audio Int B configuration */
+
+/* ***************************************************************************** */
+#define  AUD_DST_C_DMA             0x140200     /* Audio Int C DMA data port */
+#define  AUD_SRC_C_DMA             0x140208     /* Audio Int C DMA data port */
+
+#define  AUD_C_GPCNT               0x140210     /* Audio Int C gp counter */
+#define  FLD_AUD_C_GP_CNT          0x0000FFFF
+
+#define  AUD_C_GPCNT_CTL           0x140214     /* Audio Int C gp control */
+
+#define  AUD_C_LNGTH               0x140218     /* Audio Int C line length */
+
+#define  AUD_C_CFG                 0x14021C     /* Audio Int C configuration */
+
+/* ***************************************************************************** */
+#define  AUD_DST_D_DMA             0x140300     /* Audio Int D DMA data port */
+#define  AUD_SRC_D_DMA             0x140308     /* Audio Int D DMA data port */
+
+#define  AUD_D_GPCNT               0x140310     /* Audio Int D gp counter */
+#define  FLD_AUD_D_GP_CNT          0x0000FFFF
+
+#define  AUD_D_GPCNT_CTL           0x140314     /* Audio Int D gp control */
+
+#define  AUD_D_LNGTH               0x140318     /* Audio Int D line length */
+
+#define  AUD_D_CFG                 0x14031C     /* Audio Int D configuration */
+
+/* ***************************************************************************** */
+#define  AUD_SRC_E_DMA             0x140400     /* Audio Int E DMA data port */
+
+#define  AUD_E_GPCNT               0x140410     /* Audio Int E gp counter */
+#define  FLD_AUD_E_GP_CNT          0x0000FFFF
+
+#define  AUD_E_GPCNT_CTL           0x140414     /* Audio Int E gp control */
+
+#define  AUD_E_CFG                 0x14041C     /* Audio Int E configuration */
+
+/* ***************************************************************************** */
+
+#define  FLD_AUD_DST_LN_LNGTH      0x00000FFF
+
+#define  FLD_AUD_DST_PK_MODE       0x00004000
+
+#define  FLD_AUD_CLK_ENABLE        0x00000200
+
+#define  FLD_AUD_MASTER_MODE       0x00000002
+
+#define  FLD_AUD_SONY_MODE         0x00000001
+
+#define  FLD_AUD_CLK_SELECT_PLL_D  0x00001800
+
+#define  FLD_AUD_DST_ENABLE        0x00020000
+
+#define  FLD_AUD_SRC_ENABLE        0x00010000
+
+/* ***************************************************************************** */
+#define  AUD_INT_DMA_CTL           0x140500     /* Audio Int DMA control */
+
+#define  FLD_AUD_SRC_E_RISC_EN     0x00008000
+#define  FLD_AUD_SRC_C_RISC_EN     0x00004000
+#define  FLD_AUD_SRC_B_RISC_EN     0x00002000
+#define  FLD_AUD_SRC_A_RISC_EN     0x00001000
+
+#define  FLD_AUD_DST_D_RISC_EN     0x00000800
+#define  FLD_AUD_DST_C_RISC_EN     0x00000400
+#define  FLD_AUD_DST_B_RISC_EN     0x00000200
+#define  FLD_AUD_DST_A_RISC_EN     0x00000100
+
+#define  FLD_AUD_SRC_E_FIFO_EN     0x00000080
+#define  FLD_AUD_SRC_C_FIFO_EN     0x00000040
+#define  FLD_AUD_SRC_B_FIFO_EN     0x00000020
+#define  FLD_AUD_SRC_A_FIFO_EN     0x00000010
+
+#define  FLD_AUD_DST_D_FIFO_EN     0x00000008
+#define  FLD_AUD_DST_C_FIFO_EN     0x00000004
+#define  FLD_AUD_DST_B_FIFO_EN     0x00000002
+#define  FLD_AUD_DST_A_FIFO_EN     0x00000001
+
+/* ***************************************************************************** */
+/*  */
+/* Mobilygen Interface Registers */
+/*  */
+/* ***************************************************************************** */
+/* Mobilygen Interface A */
+/* ***************************************************************************** */
+#define  MB_IF_A_DMA               0x150000     /* MBIF A DMA data port */
+#define  MB_IF_A_GPCN              0x150008     /* MBIF A GP counter */
+#define  MB_IF_A_GPCN_CTRL         0x15000C
+#define  MB_IF_A_DMA_CTRL          0x150010
+#define  MB_IF_A_LENGTH            0x150014
+#define  MB_IF_A_HDMA_XFER_SZ      0x150018
+#define  MB_IF_A_HCMD              0x15001C
+#define  MB_IF_A_HCONFIG           0x150020
+#define  MB_IF_A_DATA_STRUCT_0     0x150024
+#define  MB_IF_A_DATA_STRUCT_1     0x150028
+#define  MB_IF_A_DATA_STRUCT_2     0x15002C
+#define  MB_IF_A_DATA_STRUCT_3     0x150030
+#define  MB_IF_A_DATA_STRUCT_4     0x150034
+#define  MB_IF_A_DATA_STRUCT_5     0x150038
+#define  MB_IF_A_DATA_STRUCT_6     0x15003C
+#define  MB_IF_A_DATA_STRUCT_7     0x150040
+#define  MB_IF_A_DATA_STRUCT_8     0x150044
+#define  MB_IF_A_DATA_STRUCT_9     0x150048
+#define  MB_IF_A_DATA_STRUCT_A     0x15004C
+#define  MB_IF_A_DATA_STRUCT_B     0x150050
+#define  MB_IF_A_DATA_STRUCT_C     0x150054
+#define  MB_IF_A_DATA_STRUCT_D     0x150058
+#define  MB_IF_A_DATA_STRUCT_E     0x15005C
+#define  MB_IF_A_DATA_STRUCT_F     0x150060
+/* ***************************************************************************** */
+/* Mobilygen Interface B */
+/* ***************************************************************************** */
+#define  MB_IF_B_DMA               0x160000     /* MBIF A DMA data port */
+#define  MB_IF_B_GPCN              0x160008     /* MBIF A GP counter */
+#define  MB_IF_B_GPCN_CTRL         0x16000C
+#define  MB_IF_B_DMA_CTRL          0x160010
+#define  MB_IF_B_LENGTH            0x160014
+#define  MB_IF_B_HDMA_XFER_SZ      0x160018
+#define  MB_IF_B_HCMD              0x16001C
+#define  MB_IF_B_HCONFIG           0x160020
+#define  MB_IF_B_DATA_STRUCT_0     0x160024
+#define  MB_IF_B_DATA_STRUCT_1     0x160028
+#define  MB_IF_B_DATA_STRUCT_2     0x16002C
+#define  MB_IF_B_DATA_STRUCT_3     0x160030
+#define  MB_IF_B_DATA_STRUCT_4     0x160034
+#define  MB_IF_B_DATA_STRUCT_5     0x160038
+#define  MB_IF_B_DATA_STRUCT_6     0x16003C
+#define  MB_IF_B_DATA_STRUCT_7     0x160040
+#define  MB_IF_B_DATA_STRUCT_8     0x160044
+#define  MB_IF_B_DATA_STRUCT_9     0x160048
+#define  MB_IF_B_DATA_STRUCT_A     0x16004C
+#define  MB_IF_B_DATA_STRUCT_B     0x160050
+#define  MB_IF_B_DATA_STRUCT_C     0x160054
+#define  MB_IF_B_DATA_STRUCT_D     0x160058
+#define  MB_IF_B_DATA_STRUCT_E     0x16005C
+#define  MB_IF_B_DATA_STRUCT_F     0x160060
+
+/* MB_DMA_CTRL */
+#define  FLD_MB_IF_RISC_EN         0x00000010
+#define  FLD_MB_IF_FIFO_EN         0x00000001
+
+/* MB_LENGTH */
+#define  FLD_MB_IF_LN_LNGTH        0x00000FFF
+
+/* MB_HCMD register */
+#define  FLD_MB_HCMD_H_GO          0x80000000
+#define  FLD_MB_HCMD_H_BUSY        0x40000000
+#define  FLD_MB_HCMD_H_DMA_HOLD    0x10000000
+#define  FLD_MB_HCMD_H_DMA_BUSY    0x08000000
+#define  FLD_MB_HCMD_H_DMA_TYPE    0x04000000
+#define  FLD_MB_HCMD_H_DMA_XACT    0x02000000
+#define  FLD_MB_HCMD_H_RW_N        0x01000000
+#define  FLD_MB_HCMD_H_ADDR        0x00FF0000
+#define  FLD_MB_HCMD_H_DATA        0x0000FFFF
+
+/* ***************************************************************************** */
+/* I2C #1 */
+/* ***************************************************************************** */
+#define  I2C1_ADDR                 0x180000     /* I2C #1 address */
+#define  FLD_I2C_DADDR             0xfe000000   /* RW [31:25] I2C Device Address */
+                                                 /* RO [24] reserved */
+/* ***************************************************************************** */
+#define  FLD_I2C_SADDR             0x00FFFFFF   /* RW [23:0]  I2C Sub-address */
+
+/* ***************************************************************************** */
+#define  I2C1_WDATA                0x180004     /* I2C #1 write data */
+#define  FLD_I2C_WDATA             0xFFFFFFFF   /* RW [31:0] */
+
+/* ***************************************************************************** */
+#define  I2C1_CTRL                 0x180008     /* I2C #1 control */
+#define  FLD_I2C_PERIOD            0xFF000000   /* RW [31:24] */
+#define  FLD_I2C_SCL_IN            0x00200000   /* RW [21] */
+#define  FLD_I2C_SDA_IN            0x00100000   /* RW [20] */
+                                                 /* RO [19:18] reserved */
+#define  FLD_I2C_SCL_OUT           0x00020000   /* RW [17] */
+#define  FLD_I2C_SDA_OUT           0x00010000   /* RW [16] */
+                                                 /* RO [15] reserved */
+#define  FLD_I2C_DATA_LEN          0x00007000   /* RW [14:12] */
+#define  FLD_I2C_SADDR_INC         0x00000800   /* RW [11] */
+                                                 /* RO [10:9] reserved */
+#define  FLD_I2C_SADDR_LEN         0x00000300   /* RW [9:8] */
+                                                 /* RO [7:6] reserved */
+#define  FLD_I2C_SOFT              0x00000020   /* RW [5] */
+#define  FLD_I2C_NOSTOP            0x00000010   /* RW [4] */
+#define  FLD_I2C_EXTEND            0x00000008   /* RW [3] */
+#define  FLD_I2C_SYNC              0x00000004   /* RW [2] */
+#define  FLD_I2C_READ_SA           0x00000002   /* RW [1] */
+#define  FLD_I2C_READ_WRN          0x00000001   /* RW [0] */
+
+/* ***************************************************************************** */
+#define  I2C1_RDATA                0x18000C     /* I2C #1 read data */
+#define  FLD_I2C_RDATA             0xFFFFFFFF   /* RO [31:0] */
+
+/* ***************************************************************************** */
+#define  I2C1_STAT                 0x180010     /* I2C #1 status */
+#define  FLD_I2C_XFER_IN_PROG      0x00000002   /* RO [1] */
+#define  FLD_I2C_RACK              0x00000001   /* RO [0] */
+
+/* ***************************************************************************** */
+/* I2C #2 */
+/* ***************************************************************************** */
+#define  I2C2_ADDR                 0x190000     /* I2C #2 address */
+
+/* ***************************************************************************** */
+#define  I2C2_WDATA                0x190004     /* I2C #2 write data */
+
+/* ***************************************************************************** */
+#define  I2C2_CTRL                 0x190008     /* I2C #2 control */
+
+/* ***************************************************************************** */
+#define  I2C2_RDATA                0x19000C     /* I2C #2 read data */
+
+/* ***************************************************************************** */
+#define  I2C2_STAT                 0x190010     /* I2C #2 status */
+
+/* ***************************************************************************** */
+/* I2C #3 */
+/* ***************************************************************************** */
+#define  I2C3_ADDR                 0x1A0000     /* I2C #3 address */
+
+/* ***************************************************************************** */
+#define  I2C3_WDATA                0x1A0004     /* I2C #3 write data */
+
+/* ***************************************************************************** */
+#define  I2C3_CTRL                 0x1A0008     /* I2C #3 control */
+
+/* ***************************************************************************** */
+#define  I2C3_RDATA                0x1A000C     /* I2C #3 read data */
+
+/* ***************************************************************************** */
+#define  I2C3_STAT                 0x1A0010     /* I2C #3 status */
+
+/* ***************************************************************************** */
+/* UART */
+/* ***************************************************************************** */
+#define  UART_CTL                  0x1B0000     /* UART Control Register */
+#define  FLD_LOOP_BACK_EN          (1 << 7)     /* RW field - default 0 */
+#define  FLD_RX_TRG_SZ             (3 << 2)     /* RW field - default 0 */
+#define  FLD_RX_EN                 (1 << 1)     /* RW field - default 0 */
+#define  FLD_TX_EN                 (1 << 0)     /* RW field - default 0 */
+
+/* ***************************************************************************** */
+#define  UART_BRD                  0x1B0004     /* UART Baud Rate Divisor */
+#define  FLD_BRD                   0x0000FFFF   /* RW field - default 0x197 */
+
+/* ***************************************************************************** */
+#define  UART_DBUF                 0x1B0008     /* UART Tx/Rx Data BuFFer */
+#define  FLD_DB                    0xFFFFFFFF   /* RW field - default 0 */
+
+/* ***************************************************************************** */
+#define  UART_ISR                  0x1B000C     /* UART Interrupt Status */
+#define  FLD_RXD_TIMEOUT_EN        (1 << 7)     /* RW field - default 0 */
+#define  FLD_FRM_ERR_EN            (1 << 6)     /* RW field - default 0 */
+#define  FLD_RXD_RDY_EN            (1 << 5)     /* RW field - default 0 */
+#define  FLD_TXD_EMPTY_EN          (1 << 4)     /* RW field - default 0 */
+#define  FLD_RXD_OVERFLOW          (1 << 3)     /* RW field - default 0 */
+#define  FLD_FRM_ERR               (1 << 2)     /* RW field - default 0 */
+#define  FLD_RXD_RDY               (1 << 1)     /* RW field - default 0 */
+#define  FLD_TXD_EMPTY             (1 << 0)     /* RW field - default 0 */
+
+/* ***************************************************************************** */
+#define  UART_CNT                  0x1B0010     /* UART Tx/Rx FIFO Byte Count */
+#define  FLD_TXD_CNT               (0x1F << 8)  /* RW field - default 0 */
+#define  FLD_RXD_CNT               (0x1F << 0)  /* RW field - default 0 */
+
+/* ***************************************************************************** */
+/* Motion Detection */
+#define  MD_CH0_GRID_BLOCK_YCNT    0x170014
+#define  MD_CH1_GRID_BLOCK_YCNT    0x170094
+#define  MD_CH2_GRID_BLOCK_YCNT    0x170114
+#define  MD_CH3_GRID_BLOCK_YCNT    0x170194
+#define  MD_CH4_GRID_BLOCK_YCNT    0x170214
+#define  MD_CH5_GRID_BLOCK_YCNT    0x170294
+#define  MD_CH6_GRID_BLOCK_YCNT    0x170314
+#define  MD_CH7_GRID_BLOCK_YCNT    0x170394
+
+#define PIXEL_FRMT_422    4
+#define PIXEL_FRMT_411    5
+#define PIXEL_FRMT_Y8     6
+
+#define PIXEL_ENGINE_VIP1 0
+#define PIXEL_ENGINE_VIP2 1
+
+#endif /* Athena_REGISTERS */
diff --git a/drivers/media/video/cx25821/cx25821-sram.h b/drivers/media/video/cx25821/cx25821-sram.h
new file mode 100644
index 000000000000..5f05d153bc4d
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-sram.h
@@ -0,0 +1,261 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <shu.lin@conexant.com>, <hiep.huynh@conexant.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __ATHENA_SRAM_H__
+#define __ATHENA_SRAM_H__
+
+/* #define RX_SRAM_START_SIZE        = 0;  //  Start of reserved SRAM */
+#define VID_CMDS_SIZE             80    /* Video CMDS size in bytes */
+#define AUDIO_CMDS_SIZE           80    /* AUDIO CMDS size in bytes */
+#define MBIF_CMDS_SIZE            80    /* MBIF  CMDS size in bytes */
+
+/* #define RX_SRAM_POOL_START_SIZE   = 0;  //  Start of useable RX SRAM for buffers */
+#define VID_IQ_SIZE               64    /* VID instruction queue size in bytes */
+#define MBIF_IQ_SIZE              64
+#define AUDIO_IQ_SIZE             64    /* AUD instruction queue size in bytes */
+
+#define VID_CDT_SIZE              64    /* VID cluster descriptor table size in bytes */
+#define MBIF_CDT_SIZE             64    /* MBIF/HBI cluster descriptor table size in bytes */
+#define AUDIO_CDT_SIZE            48    /* AUD cluster descriptor table size in bytes */
+
+/* #define RX_SRAM_POOL_FREE_SIZE    = 16; //  Start of available RX SRAM */
+/* #define RX_SRAM_END_SIZE          = 0;  //  End of RX SRAM */
+
+/* #define TX_SRAM_POOL_START_SIZE   = 0;  //  Start of transmit pool SRAM */
+/* #define MSI_DATA_SIZE             = 64; //  Reserved (MSI Data, RISC working stora */
+
+#define VID_CLUSTER_SIZE          1440  /* VID cluster data line */
+#define AUDIO_CLUSTER_SIZE        128   /* AUDIO cluster data line */
+#define MBIF_CLUSTER_SIZE         1440  /* MBIF/HBI cluster data line */
+
+/* #define TX_SRAM_POOL_FREE_SIZE    = 704;    //  Start of available TX SRAM */
+/* #define TX_SRAM_END_SIZE          = 0;      //  End of TX SRAM */
+
+/* Receive SRAM */
+#define RX_SRAM_START             0x10000
+#define VID_A_DOWN_CMDS           0x10000
+#define VID_B_DOWN_CMDS           0x10050
+#define VID_C_DOWN_CMDS           0x100A0
+#define VID_D_DOWN_CMDS           0x100F0
+#define VID_E_DOWN_CMDS           0x10140
+#define VID_F_DOWN_CMDS           0x10190
+#define VID_G_DOWN_CMDS           0x101E0
+#define VID_H_DOWN_CMDS           0x10230
+#define VID_A_UP_CMDS             0x10280
+#define VID_B_UP_CMDS             0x102D0
+#define VID_C_UP_CMDS             0x10320
+#define VID_D_UP_CMDS             0x10370
+#define VID_E_UP_CMDS             0x103C0
+#define VID_F_UP_CMDS             0x10410
+#define VID_I_UP_CMDS             0x10460
+#define VID_J_UP_CMDS             0x104B0
+#define AUD_A_DOWN_CMDS           0x10500
+#define AUD_B_DOWN_CMDS           0x10550
+#define AUD_C_DOWN_CMDS           0x105A0
+#define AUD_D_DOWN_CMDS           0x105F0
+#define AUD_A_UP_CMDS             0x10640
+#define AUD_B_UP_CMDS             0x10690
+#define AUD_C_UP_CMDS             0x106E0
+#define AUD_E_UP_CMDS             0x10730
+#define MBIF_A_DOWN_CMDS          0x10780
+#define MBIF_B_DOWN_CMDS          0x107D0
+#define DMA_SCRATCH_PAD           0x10820       /* Scratch pad area from 0x10820 to 0x10B40 */
+
+/* #define RX_SRAM_POOL_START        = 0x105B0; */
+
+#define VID_A_IQ                  0x11000
+#define VID_B_IQ                  0x11040
+#define VID_C_IQ                  0x11080
+#define VID_D_IQ                  0x110C0
+#define VID_E_IQ                  0x11100
+#define VID_F_IQ                  0x11140
+#define VID_G_IQ                  0x11180
+#define VID_H_IQ                  0x111C0
+#define VID_I_IQ                  0x11200
+#define VID_J_IQ                  0x11240
+#define AUD_A_IQ                  0x11280
+#define AUD_B_IQ                  0x112C0
+#define AUD_C_IQ                  0x11300
+#define AUD_D_IQ                  0x11340
+#define AUD_E_IQ                  0x11380
+#define MBIF_A_IQ                 0x11000
+#define MBIF_B_IQ                 0x110C0
+
+#define VID_A_CDT                 0x10C00
+#define VID_B_CDT                 0x10C40
+#define VID_C_CDT                 0x10C80
+#define VID_D_CDT                 0x10CC0
+#define VID_E_CDT                 0x10D00
+#define VID_F_CDT                 0x10D40
+#define VID_G_CDT                 0x10D80
+#define VID_H_CDT                 0x10DC0
+#define VID_I_CDT                 0x10E00
+#define VID_J_CDT                 0x10E40
+#define AUD_A_CDT                 0x10E80
+#define AUD_B_CDT                 0x10EB0
+#define AUD_C_CDT                 0x10EE0
+#define AUD_D_CDT                 0x10F10
+#define AUD_E_CDT                 0x10F40
+#define MBIF_A_CDT                0x10C00
+#define MBIF_B_CDT                0x10CC0
+
+/* Cluster Buffer for RX */
+#define VID_A_UP_CLUSTER_1        0x11400
+#define VID_A_UP_CLUSTER_2        0x119A0
+#define VID_A_UP_CLUSTER_3        0x11F40
+#define VID_A_UP_CLUSTER_4        0x124E0
+
+#define VID_B_UP_CLUSTER_1        0x12A80
+#define VID_B_UP_CLUSTER_2        0x13020
+#define VID_B_UP_CLUSTER_3        0x135C0
+#define VID_B_UP_CLUSTER_4        0x13B60
+
+#define VID_C_UP_CLUSTER_1        0x14100
+#define VID_C_UP_CLUSTER_2        0x146A0
+#define VID_C_UP_CLUSTER_3        0x14C40
+#define VID_C_UP_CLUSTER_4        0x151E0
+
+#define VID_D_UP_CLUSTER_1        0x15780
+#define VID_D_UP_CLUSTER_2        0x15D20
+#define VID_D_UP_CLUSTER_3        0x162C0
+#define VID_D_UP_CLUSTER_4        0x16860
+
+#define VID_E_UP_CLUSTER_1        0x16E00
+#define VID_E_UP_CLUSTER_2        0x173A0
+#define VID_E_UP_CLUSTER_3        0x17940
+#define VID_E_UP_CLUSTER_4        0x17EE0
+
+#define VID_F_UP_CLUSTER_1        0x18480
+#define VID_F_UP_CLUSTER_2        0x18A20
+#define VID_F_UP_CLUSTER_3        0x18FC0
+#define VID_F_UP_CLUSTER_4        0x19560
+
+#define VID_I_UP_CLUSTER_1        0x19B00
+#define VID_I_UP_CLUSTER_2        0x1A0A0
+#define VID_I_UP_CLUSTER_3        0x1A640
+#define VID_I_UP_CLUSTER_4        0x1ABE0
+
+#define VID_J_UP_CLUSTER_1        0x1B180
+#define VID_J_UP_CLUSTER_2        0x1B720
+#define VID_J_UP_CLUSTER_3        0x1BCC0
+#define VID_J_UP_CLUSTER_4        0x1C260
+
+#define AUD_A_UP_CLUSTER_1        0x1C800
+#define AUD_A_UP_CLUSTER_2        0x1C880
+#define AUD_A_UP_CLUSTER_3        0x1C900
+
+#define AUD_B_UP_CLUSTER_1        0x1C980
+#define AUD_B_UP_CLUSTER_2        0x1CA00
+#define AUD_B_UP_CLUSTER_3        0x1CA80
+
+#define AUD_C_UP_CLUSTER_1        0x1CB00
+#define AUD_C_UP_CLUSTER_2        0x1CB80
+#define AUD_C_UP_CLUSTER_3        0x1CC00
+
+#define AUD_E_UP_CLUSTER_1        0x1CC80
+#define AUD_E_UP_CLUSTER_2        0x1CD00
+#define AUD_E_UP_CLUSTER_3        0x1CD80
+
+#define RX_SRAM_POOL_FREE         0x1CE00
+#define RX_SRAM_END               0x1D000
+
+/* Free Receive SRAM    144 Bytes */
+
+/* Transmit SRAM */
+#define TX_SRAM_POOL_START        0x00000
+
+#define VID_A_DOWN_CLUSTER_1      0x00040
+#define VID_A_DOWN_CLUSTER_2      0x005E0
+#define VID_A_DOWN_CLUSTER_3      0x00B80
+#define VID_A_DOWN_CLUSTER_4      0x01120
+
+#define VID_B_DOWN_CLUSTER_1      0x016C0
+#define VID_B_DOWN_CLUSTER_2      0x01C60
+#define VID_B_DOWN_CLUSTER_3      0x02200
+#define VID_B_DOWN_CLUSTER_4      0x027A0
+
+#define VID_C_DOWN_CLUSTER_1      0x02D40
+#define VID_C_DOWN_CLUSTER_2      0x032E0
+#define VID_C_DOWN_CLUSTER_3      0x03880
+#define VID_C_DOWN_CLUSTER_4      0x03E20
+
+#define VID_D_DOWN_CLUSTER_1      0x043C0
+#define VID_D_DOWN_CLUSTER_2      0x04960
+#define VID_D_DOWN_CLUSTER_3      0x04F00
+#define VID_D_DOWN_CLUSTER_4      0x054A0
+
+#define VID_E_DOWN_CLUSTER_1      0x05a40
+#define VID_E_DOWN_CLUSTER_2      0x05FE0
+#define VID_E_DOWN_CLUSTER_3      0x06580
+#define VID_E_DOWN_CLUSTER_4      0x06B20
+
+#define VID_F_DOWN_CLUSTER_1      0x070C0
+#define VID_F_DOWN_CLUSTER_2      0x07660
+#define VID_F_DOWN_CLUSTER_3      0x07C00
+#define VID_F_DOWN_CLUSTER_4      0x081A0
+
+#define VID_G_DOWN_CLUSTER_1      0x08740
+#define VID_G_DOWN_CLUSTER_2      0x08CE0
+#define VID_G_DOWN_CLUSTER_3      0x09280
+#define VID_G_DOWN_CLUSTER_4      0x09820
+
+#define VID_H_DOWN_CLUSTER_1      0x09DC0
+#define VID_H_DOWN_CLUSTER_2      0x0A360
+#define VID_H_DOWN_CLUSTER_3      0x0A900
+#define VID_H_DOWN_CLUSTER_4      0x0AEA0
+
+#define AUD_A_DOWN_CLUSTER_1      0x0B500
+#define AUD_A_DOWN_CLUSTER_2      0x0B580
+#define AUD_A_DOWN_CLUSTER_3      0x0B600
+
+#define AUD_B_DOWN_CLUSTER_1      0x0B680
+#define AUD_B_DOWN_CLUSTER_2      0x0B700
+#define AUD_B_DOWN_CLUSTER_3      0x0B780
+
+#define AUD_C_DOWN_CLUSTER_1      0x0B800
+#define AUD_C_DOWN_CLUSTER_2      0x0B880
+#define AUD_C_DOWN_CLUSTER_3      0x0B900
+
+#define AUD_D_DOWN_CLUSTER_1      0x0B980
+#define AUD_D_DOWN_CLUSTER_2      0x0BA00
+#define AUD_D_DOWN_CLUSTER_3      0x0BA80
+
+#define TX_SRAM_POOL_FREE         0x0BB00
+#define TX_SRAM_END               0x0C000
+
+#define BYTES_TO_DWORDS(bcount) ((bcount) >> 2)
+#define BYTES_TO_QWORDS(bcount) ((bcount) >> 3)
+#define BYTES_TO_OWORDS(bcount) ((bcount) >> 4)
+
+#define VID_IQ_SIZE_DW             BYTES_TO_DWORDS(VID_IQ_SIZE)
+#define VID_CDT_SIZE_QW            BYTES_TO_QWORDS(VID_CDT_SIZE)
+#define VID_CLUSTER_SIZE_OW        BYTES_TO_OWORDS(VID_CLUSTER_SIZE)
+
+#define AUDIO_IQ_SIZE_DW           BYTES_TO_DWORDS(AUDIO_IQ_SIZE)
+#define AUDIO_CDT_SIZE_QW          BYTES_TO_QWORDS(AUDIO_CDT_SIZE)
+#define AUDIO_CLUSTER_SIZE_QW      BYTES_TO_QWORDS(AUDIO_CLUSTER_SIZE)
+
+#define MBIF_IQ_SIZE_DW            BYTES_TO_DWORDS(MBIF_IQ_SIZE)
+#define MBIF_CDT_SIZE_QW           BYTES_TO_QWORDS(MBIF_CDT_SIZE)
+#define MBIF_CLUSTER_SIZE_OW       BYTES_TO_OWORDS(MBIF_CLUSTER_SIZE)
+
+#endif
diff --git a/drivers/media/video/cx25821/cx25821-video-upstream-ch2.c b/drivers/media/video/cx25821/cx25821-video-upstream-ch2.c
new file mode 100644
index 000000000000..2a724ddfa53f
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-video-upstream-ch2.c
@@ -0,0 +1,823 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <hiep.huynh@conexant.com>, <shu.lin@conexant.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include "cx25821-video.h"
+#include "cx25821-video-upstream-ch2.h"
+
+#include <linux/fs.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/syscalls.h>
+#include <linux/file.h>
+#include <linux/fcntl.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+
+MODULE_DESCRIPTION("v4l2 driver module for cx25821 based TV cards");
+MODULE_AUTHOR("Hiep Huynh <hiep.huynh@conexant.com>");
+MODULE_LICENSE("GPL");
+
+static int _intr_msk = FLD_VID_SRC_RISC1 | FLD_VID_SRC_UF | FLD_VID_SRC_SYNC |
+                        FLD_VID_SRC_OPC_ERR;
+
+static __le32 *cx25821_update_riscprogram_ch2(struct cx25821_dev *dev,
+                                              __le32 *rp, unsigned int offset,
+                                              unsigned int bpl, u32 sync_line,
+                                              unsigned int lines,
+                                              int fifo_enable, int field_type)
+{
+        unsigned int line, i;
+        int dist_betwn_starts = bpl * 2;
+
+        *(rp++) = cpu_to_le32(RISC_RESYNC | sync_line);
+
+        if (USE_RISC_NOOP_VIDEO) {
+                for (i = 0; i < NUM_NO_OPS; i++)
+                        *(rp++) = cpu_to_le32(RISC_NOOP);
+        }
+
+        /* scan lines */
+        for (line = 0; line < lines; line++) {
+                *(rp++) = cpu_to_le32(RISC_READ | RISC_SOL | RISC_EOL | bpl);
+                *(rp++) = cpu_to_le32(dev->_data_buf_phys_addr_ch2 + offset);
+                *(rp++) = cpu_to_le32(0);       /* bits 63-32 */
+
+                if ((lines <= NTSC_FIELD_HEIGHT)
+                    || (line < (NTSC_FIELD_HEIGHT - 1))
+                    || !(dev->_isNTSC_ch2)) {
+                        offset += dist_betwn_starts;
+                }
+        }
+
+        return rp;
+}
+
+static __le32 *cx25821_risc_field_upstream_ch2(struct cx25821_dev *dev,
+                                               __le32 *rp,
+                                               dma_addr_t databuf_phys_addr,
+                                               unsigned int offset,
+                                               u32 sync_line, unsigned int bpl,
+                                               unsigned int lines,
+                                               int fifo_enable, int field_type)
+{
+        unsigned int line, i;
+        struct sram_channel *sram_ch =
+           dev->channels[dev->_channel2_upstream_select].sram_channels;
+        int dist_betwn_starts = bpl * 2;
+
+        /* sync instruction */
+        if (sync_line != NO_SYNC_LINE)
+                *(rp++) = cpu_to_le32(RISC_RESYNC | sync_line);
+
+        if (USE_RISC_NOOP_VIDEO) {
+                for (i = 0; i < NUM_NO_OPS; i++)
+                        *(rp++) = cpu_to_le32(RISC_NOOP);
+        }
+
+        /* scan lines */
+        for (line = 0; line < lines; line++) {
+                *(rp++) = cpu_to_le32(RISC_READ | RISC_SOL | RISC_EOL | bpl);
+                *(rp++) = cpu_to_le32(databuf_phys_addr + offset);
+                *(rp++) = cpu_to_le32(0);       /* bits 63-32 */
+
+                if ((lines <= NTSC_FIELD_HEIGHT)
+                    || (line < (NTSC_FIELD_HEIGHT - 1))
+                    || !(dev->_isNTSC_ch2)) {
+                        offset += dist_betwn_starts;
+                }
+
+               /*
+                 check if we need to enable the FIFO after the first 4 lines
+                  For the upstream video channel, the risc engine will enable
+                  the FIFO.
+               */
+                if (fifo_enable && line == 3) {
+                        *(rp++) = RISC_WRITECR;
+                        *(rp++) = sram_ch->dma_ctl;
+                        *(rp++) = FLD_VID_FIFO_EN;
+                        *(rp++) = 0x00000001;
+                }
+        }
+
+        return rp;
+}
+
+int cx25821_risc_buffer_upstream_ch2(struct cx25821_dev *dev,
+                                     struct pci_dev *pci,
+                                     unsigned int top_offset, unsigned int bpl,
+                                     unsigned int lines)
+{
+        __le32 *rp;
+        int fifo_enable = 0;
+        int singlefield_lines = lines >> 1; /*get line count for single field */
+        int odd_num_lines = singlefield_lines;
+        int frame = 0;
+        int frame_size = 0;
+        int databuf_offset = 0;
+        int risc_program_size = 0;
+        int risc_flag = RISC_CNT_RESET;
+        unsigned int bottom_offset = bpl;
+        dma_addr_t risc_phys_jump_addr;
+
+        if (dev->_isNTSC_ch2) {
+                odd_num_lines = singlefield_lines + 1;
+                risc_program_size = FRAME1_VID_PROG_SIZE;
+                if (bpl == Y411_LINE_SZ)
+                        frame_size = FRAME_SIZE_NTSC_Y411;
+                else
+                        frame_size = FRAME_SIZE_NTSC_Y422;
+        } else {
+                risc_program_size = PAL_VID_PROG_SIZE;
+                if (bpl == Y411_LINE_SZ)
+                        frame_size = FRAME_SIZE_PAL_Y411;
+                else
+                        frame_size = FRAME_SIZE_PAL_Y422;
+        }
+
+        /* Virtual address of Risc buffer program */
+        rp = dev->_dma_virt_addr_ch2;
+
+        for (frame = 0; frame < NUM_FRAMES; frame++) {
+                databuf_offset = frame_size * frame;
+
+                if (UNSET != top_offset) {
+                        fifo_enable = (frame == 0) ? FIFO_ENABLE : FIFO_DISABLE;
+                        rp = cx25821_risc_field_upstream_ch2(dev, rp,
+                                dev->_data_buf_phys_addr_ch2 + databuf_offset,
+                                top_offset, 0, bpl, odd_num_lines, fifo_enable,
+                                ODD_FIELD);
+                }
+
+                fifo_enable = FIFO_DISABLE;
+
+               /* Even field */
+                rp = cx25821_risc_field_upstream_ch2(dev, rp,
+                                dev->_data_buf_phys_addr_ch2 + databuf_offset,
+                                bottom_offset, 0x200, bpl, singlefield_lines,
+                                fifo_enable, EVEN_FIELD);
+
+                if (frame == 0) {
+                        risc_flag = RISC_CNT_RESET;
+                        risc_phys_jump_addr = dev->_dma_phys_start_addr_ch2 +
+                                        risc_program_size;
+                } else {
+                        risc_flag = RISC_CNT_INC;
+                        risc_phys_jump_addr = dev->_dma_phys_start_addr_ch2;
+                }
+
+               /*
+                  Loop to 2ndFrameRISC or to Start of
+                  Risc program & generate IRQ
+               */
+                *(rp++) = cpu_to_le32(RISC_JUMP | RISC_IRQ1 | risc_flag);
+                *(rp++) = cpu_to_le32(risc_phys_jump_addr);
+                *(rp++) = cpu_to_le32(0);
+        }
+
+        return 0;
+}
+
+void cx25821_stop_upstream_video_ch2(struct cx25821_dev *dev)
+{
+        struct sram_channel *sram_ch =
+           dev->channels[VID_UPSTREAM_SRAM_CHANNEL_J].sram_channels;
+        u32 tmp = 0;
+
+        if (!dev->_is_running_ch2) {
+                pr_info("No video file is currently running so return!\n");
+                return;
+        }
+        /* Disable RISC interrupts */
+        tmp = cx_read(sram_ch->int_msk);
+        cx_write(sram_ch->int_msk, tmp & ~_intr_msk);
+
+        /* Turn OFF risc and fifo */
+        tmp = cx_read(sram_ch->dma_ctl);
+        cx_write(sram_ch->dma_ctl, tmp & ~(FLD_VID_FIFO_EN | FLD_VID_RISC_EN));
+
+        /* Clear data buffer memory */
+        if (dev->_data_buf_virt_addr_ch2)
+                memset(dev->_data_buf_virt_addr_ch2, 0,
+                       dev->_data_buf_size_ch2);
+
+        dev->_is_running_ch2 = 0;
+        dev->_is_first_frame_ch2 = 0;
+        dev->_frame_count_ch2 = 0;
+        dev->_file_status_ch2 = END_OF_FILE;
+
+        kfree(dev->_irq_queues_ch2);
+        dev->_irq_queues_ch2 = NULL;
+
+        kfree(dev->_filename_ch2);
+
+        tmp = cx_read(VID_CH_MODE_SEL);
+        cx_write(VID_CH_MODE_SEL, tmp & 0xFFFFFE00);
+}
+
+void cx25821_free_mem_upstream_ch2(struct cx25821_dev *dev)
+{
+        if (dev->_is_running_ch2)
+                cx25821_stop_upstream_video_ch2(dev);
+
+        if (dev->_dma_virt_addr_ch2) {
+                pci_free_consistent(dev->pci, dev->_risc_size_ch2,
+                                    dev->_dma_virt_addr_ch2,
+                                    dev->_dma_phys_addr_ch2);
+                dev->_dma_virt_addr_ch2 = NULL;
+        }
+
+        if (dev->_data_buf_virt_addr_ch2) {
+                pci_free_consistent(dev->pci, dev->_data_buf_size_ch2,
+                                    dev->_data_buf_virt_addr_ch2,
+                                    dev->_data_buf_phys_addr_ch2);
+                dev->_data_buf_virt_addr_ch2 = NULL;
+        }
+}
+
+int cx25821_get_frame_ch2(struct cx25821_dev *dev, struct sram_channel *sram_ch)
+{
+        struct file *myfile;
+        int frame_index_temp = dev->_frame_index_ch2;
+        int i = 0;
+        int line_size =
+            (dev->_pixel_format_ch2 ==
+             PIXEL_FRMT_411) ? Y411_LINE_SZ : Y422_LINE_SZ;
+        int frame_size = 0;
+        int frame_offset = 0;
+        ssize_t vfs_read_retval = 0;
+        char mybuf[line_size];
+        loff_t file_offset;
+        loff_t pos;
+        mm_segment_t old_fs;
+
+        if (dev->_file_status_ch2 == END_OF_FILE)
+                return 0;
+
+        if (dev->_isNTSC_ch2) {
+                frame_size =
+                    (line_size ==
+                     Y411_LINE_SZ) ? FRAME_SIZE_NTSC_Y411 :
+                    FRAME_SIZE_NTSC_Y422;
+        } else {
+                frame_size =
+                    (line_size ==
+                     Y411_LINE_SZ) ? FRAME_SIZE_PAL_Y411 : FRAME_SIZE_PAL_Y422;
+        }
+
+        frame_offset = (frame_index_temp > 0) ? frame_size : 0;
+        file_offset = dev->_frame_count_ch2 * frame_size;
+
+        myfile = filp_open(dev->_filename_ch2, O_RDONLY | O_LARGEFILE, 0);
+        if (IS_ERR(myfile)) {
+                const int open_errno = -PTR_ERR(myfile);
+                pr_err("%s(): ERROR opening file(%s) with errno = %d!\n",
+                       __func__, dev->_filename_ch2, open_errno);
+                return PTR_ERR(myfile);
+        } else {
+                if (!(myfile->f_op)) {
+                        pr_err("%s(): File has no file operations registered!\n",
+                               __func__);
+                        filp_close(myfile, NULL);
+                        return -EIO;
+                }
+
+                if (!myfile->f_op->read) {
+                        pr_err("%s(): File has no READ operations registered!\n",
+                               __func__);
+                        filp_close(myfile, NULL);
+                        return -EIO;
+                }
+
+                pos = myfile->f_pos;
+                old_fs = get_fs();
+                set_fs(KERNEL_DS);
+
+                for (i = 0; i < dev->_lines_count_ch2; i++) {
+                        pos = file_offset;
+
+                        vfs_read_retval =
+                            vfs_read(myfile, mybuf, line_size, &pos);
+
+                        if (vfs_read_retval > 0 && vfs_read_retval == line_size
+                            && dev->_data_buf_virt_addr_ch2 != NULL) {
+                                memcpy((void *)(dev->_data_buf_virt_addr_ch2 +
+                                                frame_offset / 4), mybuf,
+                                       vfs_read_retval);
+                        }
+
+                        file_offset += vfs_read_retval;
+                        frame_offset += vfs_read_retval;
+
+                        if (vfs_read_retval < line_size) {
+                                pr_info("Done: exit %s() since no more bytes to read from Video file\n",
+                                        __func__);
+                                break;
+                        }
+                }
+
+                if (i > 0)
+                        dev->_frame_count_ch2++;
+
+                dev->_file_status_ch2 =
+                    (vfs_read_retval == line_size) ? IN_PROGRESS : END_OF_FILE;
+
+                set_fs(old_fs);
+                filp_close(myfile, NULL);
+        }
+
+        return 0;
+}
+
+static void cx25821_vidups_handler_ch2(struct work_struct *work)
+{
+        struct cx25821_dev *dev =
+            container_of(work, struct cx25821_dev, _irq_work_entry_ch2);
+
+        if (!dev) {
+                pr_err("ERROR %s(): since container_of(work_struct) FAILED!\n",
+                       __func__);
+                return;
+        }
+
+        cx25821_get_frame_ch2(dev,
+                             dev->channels[dev->
+                               _channel2_upstream_select].sram_channels);
+}
+
+int cx25821_openfile_ch2(struct cx25821_dev *dev, struct sram_channel *sram_ch)
+{
+        struct file *myfile;
+        int i = 0, j = 0;
+        int line_size =
+            (dev->_pixel_format_ch2 ==
+             PIXEL_FRMT_411) ? Y411_LINE_SZ : Y422_LINE_SZ;
+        ssize_t vfs_read_retval = 0;
+        char mybuf[line_size];
+        loff_t pos;
+        loff_t offset = (unsigned long)0;
+        mm_segment_t old_fs;
+
+        myfile = filp_open(dev->_filename_ch2, O_RDONLY | O_LARGEFILE, 0);
+
+        if (IS_ERR(myfile)) {
+                const int open_errno = -PTR_ERR(myfile);
+                pr_err("%s(): ERROR opening file(%s) with errno = %d!\n",
+                       __func__, dev->_filename_ch2, open_errno);
+                return PTR_ERR(myfile);
+        } else {
+                if (!(myfile->f_op)) {
+                        pr_err("%s(): File has no file operations registered!\n",
+                               __func__);
+                        filp_close(myfile, NULL);
+                        return -EIO;
+                }
+
+                if (!myfile->f_op->read) {
+                        pr_err("%s(): File has no READ operations registered!  Returning\n",
+                               __func__);
+                        filp_close(myfile, NULL);
+                        return -EIO;
+                }
+
+                pos = myfile->f_pos;
+                old_fs = get_fs();
+                set_fs(KERNEL_DS);
+
+                for (j = 0; j < NUM_FRAMES; j++) {
+                        for (i = 0; i < dev->_lines_count_ch2; i++) {
+                                pos = offset;
+
+                                vfs_read_retval =
+                                    vfs_read(myfile, mybuf, line_size, &pos);
+
+                                if (vfs_read_retval > 0
+                                    && vfs_read_retval == line_size
+                                    && dev->_data_buf_virt_addr_ch2 != NULL) {
+                                        memcpy((void *)(dev->
+                                                        _data_buf_virt_addr_ch2
+                                                        + offset / 4), mybuf,
+                                               vfs_read_retval);
+                                }
+
+                                offset += vfs_read_retval;
+
+                                if (vfs_read_retval < line_size) {
+                                        pr_info("Done: exit %s() since no more bytes to read from Video file\n",
+                                                __func__);
+                                        break;
+                                }
+                        }
+
+                        if (i > 0)
+                                dev->_frame_count_ch2++;
+
+                        if (vfs_read_retval < line_size)
+                                break;
+                }
+
+                dev->_file_status_ch2 =
+                    (vfs_read_retval == line_size) ? IN_PROGRESS : END_OF_FILE;
+
+                set_fs(old_fs);
+                myfile->f_pos = 0;
+                filp_close(myfile, NULL);
+        }
+
+        return 0;
+}
+
+static int cx25821_upstream_buffer_prepare_ch2(struct cx25821_dev *dev,
+                                               struct sram_channel *sram_ch,
+                                               int bpl)
+{
+        int ret = 0;
+        dma_addr_t dma_addr;
+        dma_addr_t data_dma_addr;
+
+        if (dev->_dma_virt_addr_ch2 != NULL) {
+                pci_free_consistent(dev->pci, dev->upstream_riscbuf_size_ch2,
+                                    dev->_dma_virt_addr_ch2,
+                                    dev->_dma_phys_addr_ch2);
+        }
+
+        dev->_dma_virt_addr_ch2 =
+            pci_alloc_consistent(dev->pci, dev->upstream_riscbuf_size_ch2,
+                                 &dma_addr);
+        dev->_dma_virt_start_addr_ch2 = dev->_dma_virt_addr_ch2;
+        dev->_dma_phys_start_addr_ch2 = dma_addr;
+        dev->_dma_phys_addr_ch2 = dma_addr;
+        dev->_risc_size_ch2 = dev->upstream_riscbuf_size_ch2;
+
+        if (!dev->_dma_virt_addr_ch2) {
+                pr_err("FAILED to allocate memory for Risc buffer! Returning\n");
+                return -ENOMEM;
+        }
+
+        /* Iniitize at this address until n bytes to 0 */
+        memset(dev->_dma_virt_addr_ch2, 0, dev->_risc_size_ch2);
+
+        if (dev->_data_buf_virt_addr_ch2 != NULL) {
+                pci_free_consistent(dev->pci, dev->upstream_databuf_size_ch2,
+                                    dev->_data_buf_virt_addr_ch2,
+                                    dev->_data_buf_phys_addr_ch2);
+        }
+        /* For Video Data buffer allocation */
+        dev->_data_buf_virt_addr_ch2 =
+            pci_alloc_consistent(dev->pci, dev->upstream_databuf_size_ch2,
+                                 &data_dma_addr);
+        dev->_data_buf_phys_addr_ch2 = data_dma_addr;
+        dev->_data_buf_size_ch2 = dev->upstream_databuf_size_ch2;
+
+        if (!dev->_data_buf_virt_addr_ch2) {
+                pr_err("FAILED to allocate memory for data buffer! Returning\n");
+                return -ENOMEM;
+        }
+
+        /* Initialize at this address until n bytes to 0 */
+        memset(dev->_data_buf_virt_addr_ch2, 0, dev->_data_buf_size_ch2);
+
+        ret = cx25821_openfile_ch2(dev, sram_ch);
+        if (ret < 0)
+                return ret;
+
+        /* Creating RISC programs */
+        ret = cx25821_risc_buffer_upstream_ch2(dev, dev->pci, 0, bpl,
+                                                dev->_lines_count_ch2);
+        if (ret < 0) {
+                pr_info("Failed creating Video Upstream Risc programs!\n");
+                goto error;
+        }
+
+        return 0;
+
+error:
+        return ret;
+}
+
+int cx25821_video_upstream_irq_ch2(struct cx25821_dev *dev, int chan_num,
+                                   u32 status)
+{
+        u32 int_msk_tmp;
+        struct sram_channel *channel = dev->channels[chan_num].sram_channels;
+        int singlefield_lines = NTSC_FIELD_HEIGHT;
+        int line_size_in_bytes = Y422_LINE_SZ;
+        int odd_risc_prog_size = 0;
+        dma_addr_t risc_phys_jump_addr;
+        __le32 *rp;
+
+        if (status & FLD_VID_SRC_RISC1) {
+                /* We should only process one program per call */
+                u32 prog_cnt = cx_read(channel->gpcnt);
+
+                /*
+                 *  Since we've identified our IRQ, clear our bits from the
+                 *  interrupt mask and interrupt status registers
+                 */
+                int_msk_tmp = cx_read(channel->int_msk);
+                cx_write(channel->int_msk, int_msk_tmp & ~_intr_msk);
+                cx_write(channel->int_stat, _intr_msk);
+
+                spin_lock(&dev->slock);
+
+                dev->_frame_index_ch2 = prog_cnt;
+
+                queue_work(dev->_irq_queues_ch2, &dev->_irq_work_entry_ch2);
+
+                if (dev->_is_first_frame_ch2) {
+                        dev->_is_first_frame_ch2 = 0;
+
+                        if (dev->_isNTSC_ch2) {
+                                singlefield_lines += 1;
+                                odd_risc_prog_size = ODD_FLD_NTSC_PROG_SIZE;
+                        } else {
+                                singlefield_lines = PAL_FIELD_HEIGHT;
+                                odd_risc_prog_size = ODD_FLD_PAL_PROG_SIZE;
+                        }
+
+                        if (dev->_dma_virt_start_addr_ch2 != NULL) {
+                                if (dev->_pixel_format_ch2 == PIXEL_FRMT_411)
+                                        line_size_in_bytes = Y411_LINE_SZ;
+                                else
+                                        line_size_in_bytes = Y422_LINE_SZ;
+                                risc_phys_jump_addr =
+                                    dev->_dma_phys_start_addr_ch2 +
+                                    odd_risc_prog_size;
+
+                                rp = cx25821_update_riscprogram_ch2(dev,
+                                                dev->_dma_virt_start_addr_ch2,
+                                                TOP_OFFSET, line_size_in_bytes,
+                                                0x0, singlefield_lines,
+                                                FIFO_DISABLE, ODD_FIELD);
+
+                               /* Jump to Even Risc program of 1st Frame */
+                                *(rp++) = cpu_to_le32(RISC_JUMP);
+                                *(rp++) = cpu_to_le32(risc_phys_jump_addr);
+                                *(rp++) = cpu_to_le32(0);
+                        }
+                }
+
+                spin_unlock(&dev->slock);
+        }
+
+        if (dev->_file_status_ch2 == END_OF_FILE) {
+                pr_info("EOF Channel 2 Framecount = %d\n",
+                        dev->_frame_count_ch2);
+                return -1;
+        }
+        /* ElSE, set the interrupt mask register, re-enable irq. */
+        int_msk_tmp = cx_read(channel->int_msk);
+        cx_write(channel->int_msk, int_msk_tmp |= _intr_msk);
+
+        return 0;
+}
+
+static irqreturn_t cx25821_upstream_irq_ch2(int irq, void *dev_id)
+{
+        struct cx25821_dev *dev = dev_id;
+        u32 msk_stat, vid_status;
+        int handled = 0;
+        int channel_num = 0;
+        struct sram_channel *sram_ch;
+
+        if (!dev)
+                return -1;
+
+        channel_num = VID_UPSTREAM_SRAM_CHANNEL_J;
+        sram_ch = dev->channels[channel_num].sram_channels;
+
+        msk_stat = cx_read(sram_ch->int_mstat);
+        vid_status = cx_read(sram_ch->int_stat);
+
+        /* Only deal with our interrupt */
+        if (vid_status) {
+                handled =
+                    cx25821_video_upstream_irq_ch2(dev, channel_num,
+                                                   vid_status);
+        }
+
+        if (handled < 0)
+                cx25821_stop_upstream_video_ch2(dev);
+        else
+                handled += handled;
+
+        return IRQ_RETVAL(handled);
+}
+
+static void cx25821_set_pixelengine_ch2(struct cx25821_dev *dev,
+                                        struct sram_channel *ch, int pix_format)
+{
+        int width = WIDTH_D1;
+        int height = dev->_lines_count_ch2;
+        int num_lines, odd_num_lines;
+        u32 value;
+        int vip_mode = PIXEL_ENGINE_VIP1;
+
+        value = ((pix_format & 0x3) << 12) | (vip_mode & 0x7);
+        value &= 0xFFFFFFEF;
+        value |= dev->_isNTSC_ch2 ? 0 : 0x10;
+        cx_write(ch->vid_fmt_ctl, value);
+
+        /*
+         *  set number of active pixels in each line. Default is 720
+         * pixels in both NTSC and PAL format
+         */
+        cx_write(ch->vid_active_ctl1, width);
+
+        num_lines = (height / 2) & 0x3FF;
+        odd_num_lines = num_lines;
+
+        if (dev->_isNTSC_ch2)
+                odd_num_lines += 1;
+
+        value = (num_lines << 16) | odd_num_lines;
+
+        /* set number of active lines in field 0 (top) and field 1 (bottom) */
+        cx_write(ch->vid_active_ctl2, value);
+
+        cx_write(ch->vid_cdt_size, VID_CDT_SIZE >> 3);
+}
+
+int cx25821_start_video_dma_upstream_ch2(struct cx25821_dev *dev,
+                                         struct sram_channel *sram_ch)
+{
+        u32 tmp = 0;
+        int err = 0;
+
+        /*
+         *  656/VIP SRC Upstream Channel I & J and 7 - Host Bus Interface
+         * for channel A-C
+         */
+        tmp = cx_read(VID_CH_MODE_SEL);
+        cx_write(VID_CH_MODE_SEL, tmp | 0x1B0001FF);
+
+        /*
+         *  Set the physical start address of the RISC program in the initial
+         *  program counter(IPC) member of the cmds.
+         */
+        cx_write(sram_ch->cmds_start + 0, dev->_dma_phys_addr_ch2);
+        cx_write(sram_ch->cmds_start + 4, 0); /* Risc IPC High 64 bits 63-32 */
+
+        /* reset counter */
+        cx_write(sram_ch->gpcnt_ctl, 3);
+
+        /* Clear our bits from the interrupt status register. */
+        cx_write(sram_ch->int_stat, _intr_msk);
+
+        /* Set the interrupt mask register, enable irq. */
+        cx_set(PCI_INT_MSK, cx_read(PCI_INT_MSK) | (1 << sram_ch->irq_bit));
+        tmp = cx_read(sram_ch->int_msk);
+        cx_write(sram_ch->int_msk, tmp |= _intr_msk);
+
+        err =
+            request_irq(dev->pci->irq, cx25821_upstream_irq_ch2,
+                        IRQF_SHARED, dev->name, dev);
+        if (err < 0) {
+                pr_err("%s: can't get upstream IRQ %d\n",
+                       dev->name, dev->pci->irq);
+                goto fail_irq;
+        }
+        /* Start the DMA  engine */
+        tmp = cx_read(sram_ch->dma_ctl);
+        cx_set(sram_ch->dma_ctl, tmp | FLD_VID_RISC_EN);
+
+        dev->_is_running_ch2 = 1;
+        dev->_is_first_frame_ch2 = 1;
+
+        return 0;
+
+fail_irq:
+        cx25821_dev_unregister(dev);
+        return err;
+}
+
+int cx25821_vidupstream_init_ch2(struct cx25821_dev *dev, int channel_select,
+                                 int pixel_format)
+{
+        struct sram_channel *sram_ch;
+        u32 tmp;
+        int retval = 0;
+        int err = 0;
+        int data_frame_size = 0;
+        int risc_buffer_size = 0;
+        int str_length = 0;
+
+        if (dev->_is_running_ch2) {
+                pr_info("Video Channel is still running so return!\n");
+                return 0;
+        }
+
+        dev->_channel2_upstream_select = channel_select;
+        sram_ch = dev->channels[channel_select].sram_channels;
+
+        INIT_WORK(&dev->_irq_work_entry_ch2, cx25821_vidups_handler_ch2);
+        dev->_irq_queues_ch2 =
+            create_singlethread_workqueue("cx25821_workqueue2");
+
+        if (!dev->_irq_queues_ch2) {
+                pr_err("create_singlethread_workqueue() for Video FAILED!\n");
+                return -ENOMEM;
+        }
+        /*
+         * 656/VIP SRC Upstream Channel I & J and 7 -
+         * Host Bus Interface for channel A-C
+         */
+        tmp = cx_read(VID_CH_MODE_SEL);
+        cx_write(VID_CH_MODE_SEL, tmp | 0x1B0001FF);
+
+        dev->_is_running_ch2 = 0;
+        dev->_frame_count_ch2 = 0;
+        dev->_file_status_ch2 = RESET_STATUS;
+        dev->_lines_count_ch2 = dev->_isNTSC_ch2 ? 480 : 576;
+        dev->_pixel_format_ch2 = pixel_format;
+        dev->_line_size_ch2 =
+            (dev->_pixel_format_ch2 ==
+             PIXEL_FRMT_422) ? (WIDTH_D1 * 2) : (WIDTH_D1 * 3) / 2;
+        data_frame_size = dev->_isNTSC_ch2 ? NTSC_DATA_BUF_SZ : PAL_DATA_BUF_SZ;
+        risc_buffer_size =
+            dev->_isNTSC_ch2 ? NTSC_RISC_BUF_SIZE : PAL_RISC_BUF_SIZE;
+
+        if (dev->input_filename_ch2) {
+                str_length = strlen(dev->input_filename_ch2);
+                dev->_filename_ch2 = kmalloc(str_length + 1, GFP_KERNEL);
+
+                if (!dev->_filename_ch2)
+                        goto error;
+
+                memcpy(dev->_filename_ch2, dev->input_filename_ch2,
+                       str_length + 1);
+        } else {
+                str_length = strlen(dev->_defaultname_ch2);
+                dev->_filename_ch2 = kmalloc(str_length + 1, GFP_KERNEL);
+
+                if (!dev->_filename_ch2)
+                        goto error;
+
+                memcpy(dev->_filename_ch2, dev->_defaultname_ch2,
+                       str_length + 1);
+        }
+
+        /* Default if filename is empty string */
+        if (strcmp(dev->input_filename_ch2, "") == 0) {
+                if (dev->_isNTSC_ch2) {
+                        dev->_filename_ch2 =
+                            (dev->_pixel_format_ch2 ==
+                             PIXEL_FRMT_411) ? "/root/vid411.yuv" :
+                            "/root/vidtest.yuv";
+                } else {
+                        dev->_filename_ch2 =
+                            (dev->_pixel_format_ch2 ==
+                             PIXEL_FRMT_411) ? "/root/pal411.yuv" :
+                            "/root/pal422.yuv";
+                }
+        }
+
+        retval = cx25821_sram_channel_setup_upstream(dev, sram_ch,
+                                                dev->_line_size_ch2, 0);
+
+        /* setup fifo + format */
+        cx25821_set_pixelengine_ch2(dev, sram_ch, dev->_pixel_format_ch2);
+
+        dev->upstream_riscbuf_size_ch2 = risc_buffer_size * 2;
+        dev->upstream_databuf_size_ch2 = data_frame_size * 2;
+
+        /* Allocating buffers and prepare RISC program */
+        retval = cx25821_upstream_buffer_prepare_ch2(dev, sram_ch,
+                                                dev->_line_size_ch2);
+        if (retval < 0) {
+                pr_err("%s: Failed to set up Video upstream buffers!\n",
+                       dev->name);
+                goto error;
+        }
+
+        cx25821_start_video_dma_upstream_ch2(dev, sram_ch);
+
+        return 0;
+
+error:
+        cx25821_dev_unregister(dev);
+
+        return err;
+}
diff --git a/drivers/media/video/cx25821/cx25821-video-upstream-ch2.h b/drivers/media/video/cx25821/cx25821-video-upstream-ch2.h
new file mode 100644
index 000000000000..d42dab59b663
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-video-upstream-ch2.h
@@ -0,0 +1,138 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <hiep.huynh@conexant.com>, <shu.lin@conexant.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/mutex.h>
+#include <linux/workqueue.h>
+
+#define OPEN_FILE_1           0
+#define NUM_PROGS             8
+#define NUM_FRAMES            2
+#define ODD_FIELD             0
+#define EVEN_FIELD            1
+#define TOP_OFFSET            0
+#define FIFO_DISABLE          0
+#define FIFO_ENABLE           1
+#define TEST_FRAMES           5
+#define END_OF_FILE           0
+#define IN_PROGRESS           1
+#define RESET_STATUS          -1
+#define NUM_NO_OPS            5
+
+/* PAL and NTSC line sizes and number of lines. */
+#define WIDTH_D1              720
+#define NTSC_LINES_PER_FRAME  480
+#define PAL_LINES_PER_FRAME   576
+#define PAL_LINE_SZ           1440
+#define Y422_LINE_SZ          1440
+#define Y411_LINE_SZ          1080
+#define NTSC_FIELD_HEIGHT     240
+#define NTSC_ODD_FLD_LINES    241
+#define PAL_FIELD_HEIGHT      288
+
+#define FRAME_SIZE_NTSC_Y422    (NTSC_LINES_PER_FRAME * Y422_LINE_SZ)
+#define FRAME_SIZE_NTSC_Y411    (NTSC_LINES_PER_FRAME * Y411_LINE_SZ)
+#define FRAME_SIZE_PAL_Y422     (PAL_LINES_PER_FRAME * Y422_LINE_SZ)
+#define FRAME_SIZE_PAL_Y411     (PAL_LINES_PER_FRAME * Y411_LINE_SZ)
+
+#define NTSC_DATA_BUF_SZ        (Y422_LINE_SZ * NTSC_LINES_PER_FRAME)
+#define PAL_DATA_BUF_SZ         (Y422_LINE_SZ * PAL_LINES_PER_FRAME)
+
+#define RISC_WRITECR_INSTRUCTION_SIZE   16
+#define RISC_SYNC_INSTRUCTION_SIZE      4
+#define JUMP_INSTRUCTION_SIZE           12
+#define MAXSIZE_NO_OPS                  36
+#define DWORD_SIZE                      4
+
+#define USE_RISC_NOOP_VIDEO   1
+
+#ifdef USE_RISC_NOOP_VIDEO
+#define PAL_US_VID_PROG_SIZE                                            \
+        (PAL_FIELD_HEIGHT * 3 * DWORD_SIZE +                            \
+         RISC_WRITECR_INSTRUCTION_SIZE + RISC_SYNC_INSTRUCTION_SIZE +   \
+         NUM_NO_OPS * DWORD_SIZE)
+
+#define PAL_RISC_BUF_SIZE         (2 * PAL_US_VID_PROG_SIZE)
+
+#define PAL_VID_PROG_SIZE                                               \
+        ((PAL_FIELD_HEIGHT * 2) * 3 * DWORD_SIZE +                      \
+         2 * RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE + \
+         JUMP_INSTRUCTION_SIZE + 2 * NUM_NO_OPS * DWORD_SIZE)
+
+#define ODD_FLD_PAL_PROG_SIZE                                           \
+        (PAL_FIELD_HEIGHT * 3 * DWORD_SIZE +                            \
+         RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE +   \
+         NUM_NO_OPS * DWORD_SIZE)
+
+#define NTSC_US_VID_PROG_SIZE                                           \
+        ((NTSC_ODD_FLD_LINES + 1) * 3 * DWORD_SIZE +                    \
+         RISC_WRITECR_INSTRUCTION_SIZE + JUMP_INSTRUCTION_SIZE +        \
+         NUM_NO_OPS * DWORD_SIZE)
+
+#define NTSC_RISC_BUF_SIZE                                              \
+        (2 * (RISC_SYNC_INSTRUCTION_SIZE + NTSC_US_VID_PROG_SIZE))
+
+#define FRAME1_VID_PROG_SIZE                                            \
+        ((NTSC_ODD_FLD_LINES + NTSC_FIELD_HEIGHT) *                     \
+         3 * DWORD_SIZE + 2 * RISC_SYNC_INSTRUCTION_SIZE +              \
+         RISC_WRITECR_INSTRUCTION_SIZE + JUMP_INSTRUCTION_SIZE +        \
+         2 * NUM_NO_OPS * DWORD_SIZE)
+
+#define ODD_FLD_NTSC_PROG_SIZE                                          \
+        (NTSC_ODD_FLD_LINES * 3 * DWORD_SIZE +                          \
+         RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE +   \
+         NUM_NO_OPS * DWORD_SIZE)
+#endif
+
+#ifndef USE_RISC_NOOP_VIDEO
+#define PAL_US_VID_PROG_SIZE                                            \
+        ((PAL_FIELD_HEIGHT + 1) * 3 * DWORD_SIZE +                      \
+         RISC_WRITECR_INSTRUCTION_SIZE)
+
+#define PAL_RISC_BUF_SIZE                                               \
+        (2 * (RISC_SYNC_INSTRUCTION_SIZE + PAL_US_VID_PROG_SIZE))
+
+#define PAL_VID_PROG_SIZE                                               \
+        ((PAL_FIELD_HEIGHT * 2) * 3 * DWORD_SIZE +                      \
+         2 * RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE + \
+         JUMP_INSTRUCTION_SIZE)
+
+#define ODD_FLD_PAL_PROG_SIZE                                           \
+        (PAL_FIELD_HEIGHT * 3 * DWORD_SIZE +                            \
+         RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE)
+
+#define ODD_FLD_NTSC_PROG_SIZE                                          \
+        (NTSC_ODD_FLD_LINES * 3 * DWORD_SIZE +                          \
+         RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE)
+
+#define NTSC_US_VID_PROG_SIZE                                           \
+        ((NTSC_ODD_FLD_LINES + 1) * 3 * DWORD_SIZE +                    \
+         RISC_WRITECR_INSTRUCTION_SIZE + JUMP_INSTRUCTION_SIZE)
+
+#define NTSC_RISC_BUF_SIZE                                              \
+        (2 * (RISC_SYNC_INSTRUCTION_SIZE + NTSC_US_VID_PROG_SIZE))
+
+#define FRAME1_VID_PROG_SIZE                                            \
+        ((NTSC_ODD_FLD_LINES + NTSC_FIELD_HEIGHT) *                     \
+         3 * DWORD_SIZE + 2 * RISC_SYNC_INSTRUCTION_SIZE +              \
+         RISC_WRITECR_INSTRUCTION_SIZE + JUMP_INSTRUCTION_SIZE)
+
+#endif
diff --git a/drivers/media/video/cx25821/cx25821-video-upstream.c b/drivers/media/video/cx25821/cx25821-video-upstream.c
new file mode 100644
index 000000000000..c0b80068f468
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-video-upstream.c
@@ -0,0 +1,885 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <hiep.huynh@conexant.com>, <shu.lin@conexant.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include "cx25821-video.h"
+#include "cx25821-video-upstream.h"
+
+#include <linux/fs.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/syscalls.h>
+#include <linux/file.h>
+#include <linux/fcntl.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+
+MODULE_DESCRIPTION("v4l2 driver module for cx25821 based TV cards");
+MODULE_AUTHOR("Hiep Huynh <hiep.huynh@conexant.com>");
+MODULE_LICENSE("GPL");
+
+static int _intr_msk = FLD_VID_SRC_RISC1 | FLD_VID_SRC_UF | FLD_VID_SRC_SYNC |
+                        FLD_VID_SRC_OPC_ERR;
+
+int cx25821_sram_channel_setup_upstream(struct cx25821_dev *dev,
+                                        struct sram_channel *ch,
+                                        unsigned int bpl, u32 risc)
+{
+        unsigned int i, lines;
+        u32 cdt;
+
+        if (ch->cmds_start == 0) {
+                cx_write(ch->ptr1_reg, 0);
+                cx_write(ch->ptr2_reg, 0);
+                cx_write(ch->cnt2_reg, 0);
+                cx_write(ch->cnt1_reg, 0);
+                return 0;
+        }
+
+        bpl = (bpl + 7) & ~7;   /* alignment */
+        cdt = ch->cdt;
+        lines = ch->fifo_size / bpl;
+
+        if (lines > 4)
+                lines = 4;
+
+        BUG_ON(lines < 2);
+
+        /* write CDT */
+        for (i = 0; i < lines; i++) {
+                cx_write(cdt + 16 * i, ch->fifo_start + bpl * i);
+                cx_write(cdt + 16 * i + 4, 0);
+                cx_write(cdt + 16 * i + 8, 0);
+                cx_write(cdt + 16 * i + 12, 0);
+        }
+
+        /* write CMDS */
+        cx_write(ch->cmds_start + 0, risc);
+
+        cx_write(ch->cmds_start + 4, 0);
+        cx_write(ch->cmds_start + 8, cdt);
+        cx_write(ch->cmds_start + 12, (lines * 16) >> 3);
+        cx_write(ch->cmds_start + 16, ch->ctrl_start);
+
+        cx_write(ch->cmds_start + 20, VID_IQ_SIZE_DW);
+
+        for (i = 24; i < 80; i += 4)
+                cx_write(ch->cmds_start + i, 0);
+
+        /* fill registers */
+        cx_write(ch->ptr1_reg, ch->fifo_start);
+        cx_write(ch->ptr2_reg, cdt);
+        cx_write(ch->cnt2_reg, (lines * 16) >> 3);
+        cx_write(ch->cnt1_reg, (bpl >> 3) - 1);
+
+        return 0;
+}
+
+static __le32 *cx25821_update_riscprogram(struct cx25821_dev *dev,
+                                          __le32 *rp, unsigned int offset,
+                                          unsigned int bpl, u32 sync_line,
+                                          unsigned int lines, int fifo_enable,
+                                          int field_type)
+{
+        unsigned int line, i;
+        int dist_betwn_starts = bpl * 2;
+
+        *(rp++) = cpu_to_le32(RISC_RESYNC | sync_line);
+
+        if (USE_RISC_NOOP_VIDEO) {
+                for (i = 0; i < NUM_NO_OPS; i++)
+                        *(rp++) = cpu_to_le32(RISC_NOOP);
+        }
+
+        /* scan lines */
+        for (line = 0; line < lines; line++) {
+                *(rp++) = cpu_to_le32(RISC_READ | RISC_SOL | RISC_EOL | bpl);
+                *(rp++) = cpu_to_le32(dev->_data_buf_phys_addr + offset);
+                *(rp++) = cpu_to_le32(0);       /* bits 63-32 */
+
+                if ((lines <= NTSC_FIELD_HEIGHT)
+                    || (line < (NTSC_FIELD_HEIGHT - 1)) || !(dev->_isNTSC)) {
+                        offset += dist_betwn_starts;
+                }
+        }
+
+        return rp;
+}
+
+static __le32 *cx25821_risc_field_upstream(struct cx25821_dev *dev, __le32 * rp,
+                                           dma_addr_t databuf_phys_addr,
+                                           unsigned int offset, u32 sync_line,
+                                           unsigned int bpl, unsigned int lines,
+                                           int fifo_enable, int field_type)
+{
+        unsigned int line, i;
+        struct sram_channel *sram_ch =
+           dev->channels[dev->_channel_upstream_select].sram_channels;
+        int dist_betwn_starts = bpl * 2;
+
+        /* sync instruction */
+        if (sync_line != NO_SYNC_LINE)
+                *(rp++) = cpu_to_le32(RISC_RESYNC | sync_line);
+
+        if (USE_RISC_NOOP_VIDEO) {
+                for (i = 0; i < NUM_NO_OPS; i++)
+                        *(rp++) = cpu_to_le32(RISC_NOOP);
+        }
+
+        /* scan lines */
+        for (line = 0; line < lines; line++) {
+                *(rp++) = cpu_to_le32(RISC_READ | RISC_SOL | RISC_EOL | bpl);
+                *(rp++) = cpu_to_le32(databuf_phys_addr + offset);
+                *(rp++) = cpu_to_le32(0);       /* bits 63-32 */
+
+                if ((lines <= NTSC_FIELD_HEIGHT)
+                    || (line < (NTSC_FIELD_HEIGHT - 1)) || !(dev->_isNTSC))
+                        /* to skip the other field line */
+                        offset += dist_betwn_starts;
+
+                /* check if we need to enable the FIFO after the first 4 lines
+                 * For the upstream video channel, the risc engine will enable
+                 * the FIFO. */
+                if (fifo_enable && line == 3) {
+                        *(rp++) = RISC_WRITECR;
+                        *(rp++) = sram_ch->dma_ctl;
+                        *(rp++) = FLD_VID_FIFO_EN;
+                        *(rp++) = 0x00000001;
+                }
+        }
+
+        return rp;
+}
+
+int cx25821_risc_buffer_upstream(struct cx25821_dev *dev,
+                                 struct pci_dev *pci,
+                                 unsigned int top_offset,
+                                 unsigned int bpl, unsigned int lines)
+{
+        __le32 *rp;
+        int fifo_enable = 0;
+        /* get line count for single field */
+        int singlefield_lines = lines >> 1;
+        int odd_num_lines = singlefield_lines;
+        int frame = 0;
+        int frame_size = 0;
+        int databuf_offset = 0;
+        int risc_program_size = 0;
+        int risc_flag = RISC_CNT_RESET;
+        unsigned int bottom_offset = bpl;
+        dma_addr_t risc_phys_jump_addr;
+
+        if (dev->_isNTSC) {
+                odd_num_lines = singlefield_lines + 1;
+                risc_program_size = FRAME1_VID_PROG_SIZE;
+                frame_size =
+                    (bpl ==
+                     Y411_LINE_SZ) ? FRAME_SIZE_NTSC_Y411 :
+                    FRAME_SIZE_NTSC_Y422;
+        } else {
+                risc_program_size = PAL_VID_PROG_SIZE;
+                frame_size =
+                    (bpl ==
+                     Y411_LINE_SZ) ? FRAME_SIZE_PAL_Y411 : FRAME_SIZE_PAL_Y422;
+        }
+
+        /* Virtual address of Risc buffer program */
+        rp = dev->_dma_virt_addr;
+
+        for (frame = 0; frame < NUM_FRAMES; frame++) {
+                databuf_offset = frame_size * frame;
+
+                if (UNSET != top_offset) {
+                        fifo_enable = (frame == 0) ? FIFO_ENABLE : FIFO_DISABLE;
+                        rp = cx25821_risc_field_upstream(dev, rp,
+                                                         dev->
+                                                         _data_buf_phys_addr +
+                                                         databuf_offset,
+                                                         top_offset, 0, bpl,
+                                                         odd_num_lines,
+                                                         fifo_enable,
+                                                         ODD_FIELD);
+                }
+
+                fifo_enable = FIFO_DISABLE;
+
+                /* Even Field */
+                rp = cx25821_risc_field_upstream(dev, rp,
+                                                 dev->_data_buf_phys_addr +
+                                                 databuf_offset, bottom_offset,
+                                                 0x200, bpl, singlefield_lines,
+                                                 fifo_enable, EVEN_FIELD);
+
+                if (frame == 0) {
+                        risc_flag = RISC_CNT_RESET;
+                        risc_phys_jump_addr =
+                            dev->_dma_phys_start_addr + risc_program_size;
+                } else {
+                        risc_phys_jump_addr = dev->_dma_phys_start_addr;
+                        risc_flag = RISC_CNT_INC;
+                }
+
+                /* Loop to 2ndFrameRISC or to Start of Risc
+                 * program & generate IRQ
+                 */
+                *(rp++) = cpu_to_le32(RISC_JUMP | RISC_IRQ1 | risc_flag);
+                *(rp++) = cpu_to_le32(risc_phys_jump_addr);
+                *(rp++) = cpu_to_le32(0);
+        }
+
+        return 0;
+}
+
+void cx25821_stop_upstream_video_ch1(struct cx25821_dev *dev)
+{
+        struct sram_channel *sram_ch =
+           dev->channels[VID_UPSTREAM_SRAM_CHANNEL_I].sram_channels;
+        u32 tmp = 0;
+
+        if (!dev->_is_running) {
+                pr_info("No video file is currently running so return!\n");
+                return;
+        }
+        /* Disable RISC interrupts */
+        tmp = cx_read(sram_ch->int_msk);
+        cx_write(sram_ch->int_msk, tmp & ~_intr_msk);
+
+        /* Turn OFF risc and fifo enable */
+        tmp = cx_read(sram_ch->dma_ctl);
+        cx_write(sram_ch->dma_ctl, tmp & ~(FLD_VID_FIFO_EN | FLD_VID_RISC_EN));
+
+        /* Clear data buffer memory */
+        if (dev->_data_buf_virt_addr)
+                memset(dev->_data_buf_virt_addr, 0, dev->_data_buf_size);
+
+        dev->_is_running = 0;
+        dev->_is_first_frame = 0;
+        dev->_frame_count = 0;
+        dev->_file_status = END_OF_FILE;
+
+        kfree(dev->_irq_queues);
+        dev->_irq_queues = NULL;
+
+        kfree(dev->_filename);
+
+        tmp = cx_read(VID_CH_MODE_SEL);
+        cx_write(VID_CH_MODE_SEL, tmp & 0xFFFFFE00);
+}
+
+void cx25821_free_mem_upstream_ch1(struct cx25821_dev *dev)
+{
+        if (dev->_is_running)
+                cx25821_stop_upstream_video_ch1(dev);
+
+        if (dev->_dma_virt_addr) {
+                pci_free_consistent(dev->pci, dev->_risc_size,
+                                    dev->_dma_virt_addr, dev->_dma_phys_addr);
+                dev->_dma_virt_addr = NULL;
+        }
+
+        if (dev->_data_buf_virt_addr) {
+                pci_free_consistent(dev->pci, dev->_data_buf_size,
+                                    dev->_data_buf_virt_addr,
+                                    dev->_data_buf_phys_addr);
+                dev->_data_buf_virt_addr = NULL;
+        }
+}
+
+int cx25821_get_frame(struct cx25821_dev *dev, struct sram_channel *sram_ch)
+{
+        struct file *myfile;
+        int frame_index_temp = dev->_frame_index;
+        int i = 0;
+        int line_size =
+            (dev->_pixel_format ==
+             PIXEL_FRMT_411) ? Y411_LINE_SZ : Y422_LINE_SZ;
+        int frame_size = 0;
+        int frame_offset = 0;
+        ssize_t vfs_read_retval = 0;
+        char mybuf[line_size];
+        loff_t file_offset;
+        loff_t pos;
+        mm_segment_t old_fs;
+
+        if (dev->_file_status == END_OF_FILE)
+                return 0;
+
+        if (dev->_isNTSC) {
+                frame_size =
+                    (line_size ==
+                     Y411_LINE_SZ) ? FRAME_SIZE_NTSC_Y411 :
+                    FRAME_SIZE_NTSC_Y422;
+        } else {
+                frame_size =
+                    (line_size ==
+                     Y411_LINE_SZ) ? FRAME_SIZE_PAL_Y411 : FRAME_SIZE_PAL_Y422;
+        }
+
+        frame_offset = (frame_index_temp > 0) ? frame_size : 0;
+        file_offset = dev->_frame_count * frame_size;
+
+        myfile = filp_open(dev->_filename, O_RDONLY | O_LARGEFILE, 0);
+
+        if (IS_ERR(myfile)) {
+                const int open_errno = -PTR_ERR(myfile);
+                pr_err("%s(): ERROR opening file(%s) with errno = %d!\n",
+                       __func__, dev->_filename, open_errno);
+                return PTR_ERR(myfile);
+        } else {
+                if (!(myfile->f_op)) {
+                        pr_err("%s(): File has no file operations registered!\n",
+                               __func__);
+                        filp_close(myfile, NULL);
+                        return -EIO;
+                }
+
+                if (!myfile->f_op->read) {
+                        pr_err("%s(): File has no READ operations registered!\n",
+                               __func__);
+                        filp_close(myfile, NULL);
+                        return -EIO;
+                }
+
+                pos = myfile->f_pos;
+                old_fs = get_fs();
+                set_fs(KERNEL_DS);
+
+                for (i = 0; i < dev->_lines_count; i++) {
+                        pos = file_offset;
+
+                        vfs_read_retval =
+                            vfs_read(myfile, mybuf, line_size, &pos);
+
+                        if (vfs_read_retval > 0 && vfs_read_retval == line_size
+                            && dev->_data_buf_virt_addr != NULL) {
+                                memcpy((void *)(dev->_data_buf_virt_addr +
+                                                frame_offset / 4), mybuf,
+                                       vfs_read_retval);
+                        }
+
+                        file_offset += vfs_read_retval;
+                        frame_offset += vfs_read_retval;
+
+                        if (vfs_read_retval < line_size) {
+                                pr_info("Done: exit %s() since no more bytes to read from Video file\n",
+                                        __func__);
+                                break;
+                        }
+                }
+
+                if (i > 0)
+                        dev->_frame_count++;
+
+                dev->_file_status =
+                    (vfs_read_retval == line_size) ? IN_PROGRESS : END_OF_FILE;
+
+                set_fs(old_fs);
+                filp_close(myfile, NULL);
+        }
+
+        return 0;
+}
+
+static void cx25821_vidups_handler(struct work_struct *work)
+{
+        struct cx25821_dev *dev =
+            container_of(work, struct cx25821_dev, _irq_work_entry);
+
+        if (!dev) {
+                pr_err("ERROR %s(): since container_of(work_struct) FAILED!\n",
+                       __func__);
+                return;
+        }
+
+        cx25821_get_frame(dev,
+                         dev->channels[dev->_channel_upstream_select].
+                                               sram_channels);
+}
+
+int cx25821_openfile(struct cx25821_dev *dev, struct sram_channel *sram_ch)
+{
+        struct file *myfile;
+        int i = 0, j = 0;
+        int line_size =
+            (dev->_pixel_format ==
+             PIXEL_FRMT_411) ? Y411_LINE_SZ : Y422_LINE_SZ;
+        ssize_t vfs_read_retval = 0;
+        char mybuf[line_size];
+        loff_t pos;
+        loff_t offset = (unsigned long)0;
+        mm_segment_t old_fs;
+
+        myfile = filp_open(dev->_filename, O_RDONLY | O_LARGEFILE, 0);
+
+        if (IS_ERR(myfile)) {
+                const int open_errno = -PTR_ERR(myfile);
+                pr_err("%s(): ERROR opening file(%s) with errno = %d!\n",
+                       __func__, dev->_filename, open_errno);
+                return PTR_ERR(myfile);
+        } else {
+                if (!(myfile->f_op)) {
+                        pr_err("%s(): File has no file operations registered!\n",
+                               __func__);
+                        filp_close(myfile, NULL);
+                        return -EIO;
+                }
+
+                if (!myfile->f_op->read) {
+                        pr_err("%s(): File has no READ operations registered!  Returning\n",
+                               __func__);
+                        filp_close(myfile, NULL);
+                        return -EIO;
+                }
+
+                pos = myfile->f_pos;
+                old_fs = get_fs();
+                set_fs(KERNEL_DS);
+
+                for (j = 0; j < NUM_FRAMES; j++) {
+                        for (i = 0; i < dev->_lines_count; i++) {
+                                pos = offset;
+
+                                vfs_read_retval =
+                                    vfs_read(myfile, mybuf, line_size, &pos);
+
+                                if (vfs_read_retval > 0
+                                    && vfs_read_retval == line_size
+                                    && dev->_data_buf_virt_addr != NULL) {
+                                        memcpy((void *)(dev->
+                                                        _data_buf_virt_addr +
+                                                        offset / 4), mybuf,
+                                               vfs_read_retval);
+                                }
+
+                                offset += vfs_read_retval;
+
+                                if (vfs_read_retval < line_size) {
+                                        pr_info("Done: exit %s() since no more bytes to read from Video file\n",
+                                                __func__);
+                                        break;
+                                }
+                        }
+
+                        if (i > 0)
+                                dev->_frame_count++;
+
+                        if (vfs_read_retval < line_size)
+                                break;
+                }
+
+                dev->_file_status =
+                    (vfs_read_retval == line_size) ? IN_PROGRESS : END_OF_FILE;
+
+                set_fs(old_fs);
+                myfile->f_pos = 0;
+                filp_close(myfile, NULL);
+        }
+
+        return 0;
+}
+
+int cx25821_upstream_buffer_prepare(struct cx25821_dev *dev,
+                                    struct sram_channel *sram_ch, int bpl)
+{
+        int ret = 0;
+        dma_addr_t dma_addr;
+        dma_addr_t data_dma_addr;
+
+        if (dev->_dma_virt_addr != NULL) {
+                pci_free_consistent(dev->pci, dev->upstream_riscbuf_size,
+                                    dev->_dma_virt_addr, dev->_dma_phys_addr);
+        }
+
+        dev->_dma_virt_addr =
+            pci_alloc_consistent(dev->pci, dev->upstream_riscbuf_size,
+                                 &dma_addr);
+        dev->_dma_virt_start_addr = dev->_dma_virt_addr;
+        dev->_dma_phys_start_addr = dma_addr;
+        dev->_dma_phys_addr = dma_addr;
+        dev->_risc_size = dev->upstream_riscbuf_size;
+
+        if (!dev->_dma_virt_addr) {
+                pr_err("FAILED to allocate memory for Risc buffer! Returning\n");
+                return -ENOMEM;
+        }
+
+        /* Clear memory at address */
+        memset(dev->_dma_virt_addr, 0, dev->_risc_size);
+
+        if (dev->_data_buf_virt_addr != NULL) {
+                pci_free_consistent(dev->pci, dev->upstream_databuf_size,
+                                    dev->_data_buf_virt_addr,
+                                    dev->_data_buf_phys_addr);
+        }
+        /* For Video Data buffer allocation */
+        dev->_data_buf_virt_addr =
+            pci_alloc_consistent(dev->pci, dev->upstream_databuf_size,
+                                 &data_dma_addr);
+        dev->_data_buf_phys_addr = data_dma_addr;
+        dev->_data_buf_size = dev->upstream_databuf_size;
+
+        if (!dev->_data_buf_virt_addr) {
+                pr_err("FAILED to allocate memory for data buffer! Returning\n");
+                return -ENOMEM;
+        }
+
+        /* Clear memory at address */
+        memset(dev->_data_buf_virt_addr, 0, dev->_data_buf_size);
+
+        ret = cx25821_openfile(dev, sram_ch);
+        if (ret < 0)
+                return ret;
+
+        /* Create RISC programs */
+        ret =
+            cx25821_risc_buffer_upstream(dev, dev->pci, 0, bpl,
+                                         dev->_lines_count);
+        if (ret < 0) {
+                pr_info("Failed creating Video Upstream Risc programs!\n");
+                goto error;
+        }
+
+        return 0;
+
+error:
+        return ret;
+}
+
+int cx25821_video_upstream_irq(struct cx25821_dev *dev, int chan_num,
+                               u32 status)
+{
+        u32 int_msk_tmp;
+        struct sram_channel *channel = dev->channels[chan_num].sram_channels;
+        int singlefield_lines = NTSC_FIELD_HEIGHT;
+        int line_size_in_bytes = Y422_LINE_SZ;
+        int odd_risc_prog_size = 0;
+        dma_addr_t risc_phys_jump_addr;
+        __le32 *rp;
+
+        if (status & FLD_VID_SRC_RISC1) {
+                /* We should only process one program per call */
+                u32 prog_cnt = cx_read(channel->gpcnt);
+
+                /* Since we've identified our IRQ, clear our bits from the
+                 * interrupt mask and interrupt status registers */
+                int_msk_tmp = cx_read(channel->int_msk);
+                cx_write(channel->int_msk, int_msk_tmp & ~_intr_msk);
+                cx_write(channel->int_stat, _intr_msk);
+
+                spin_lock(&dev->slock);
+
+                dev->_frame_index = prog_cnt;
+
+                queue_work(dev->_irq_queues, &dev->_irq_work_entry);
+
+                if (dev->_is_first_frame) {
+                        dev->_is_first_frame = 0;
+
+                        if (dev->_isNTSC) {
+                                singlefield_lines += 1;
+                                odd_risc_prog_size = ODD_FLD_NTSC_PROG_SIZE;
+                        } else {
+                                singlefield_lines = PAL_FIELD_HEIGHT;
+                                odd_risc_prog_size = ODD_FLD_PAL_PROG_SIZE;
+                        }
+
+                        if (dev->_dma_virt_start_addr != NULL) {
+                                line_size_in_bytes =
+                                    (dev->_pixel_format ==
+                                     PIXEL_FRMT_411) ? Y411_LINE_SZ :
+                                    Y422_LINE_SZ;
+                                risc_phys_jump_addr =
+                                    dev->_dma_phys_start_addr +
+                                    odd_risc_prog_size;
+
+                                rp = cx25821_update_riscprogram(dev,
+                                        dev->_dma_virt_start_addr, TOP_OFFSET,
+                                        line_size_in_bytes, 0x0,
+                                        singlefield_lines, FIFO_DISABLE,
+                                        ODD_FIELD);
+
+                                /* Jump to Even Risc program of 1st Frame */
+                                *(rp++) = cpu_to_le32(RISC_JUMP);
+                                *(rp++) = cpu_to_le32(risc_phys_jump_addr);
+                                *(rp++) = cpu_to_le32(0);
+                        }
+                }
+
+                spin_unlock(&dev->slock);
+        } else {
+                if (status & FLD_VID_SRC_UF)
+                        pr_err("%s(): Video Received Underflow Error Interrupt!\n",
+                               __func__);
+
+                if (status & FLD_VID_SRC_SYNC)
+                        pr_err("%s(): Video Received Sync Error Interrupt!\n",
+                               __func__);
+
+                if (status & FLD_VID_SRC_OPC_ERR)
+                        pr_err("%s(): Video Received OpCode Error Interrupt!\n",
+                               __func__);
+        }
+
+        if (dev->_file_status == END_OF_FILE) {
+                pr_err("EOF Channel 1 Framecount = %d\n", dev->_frame_count);
+                return -1;
+        }
+        /* ElSE, set the interrupt mask register, re-enable irq. */
+        int_msk_tmp = cx_read(channel->int_msk);
+        cx_write(channel->int_msk, int_msk_tmp |= _intr_msk);
+
+        return 0;
+}
+
+static irqreturn_t cx25821_upstream_irq(int irq, void *dev_id)
+{
+        struct cx25821_dev *dev = dev_id;
+        u32 msk_stat, vid_status;
+        int handled = 0;
+        int channel_num = 0;
+        struct sram_channel *sram_ch;
+
+        if (!dev)
+                return -1;
+
+        channel_num = VID_UPSTREAM_SRAM_CHANNEL_I;
+
+        sram_ch = dev->channels[channel_num].sram_channels;
+
+        msk_stat = cx_read(sram_ch->int_mstat);
+        vid_status = cx_read(sram_ch->int_stat);
+
+        /* Only deal with our interrupt */
+        if (vid_status) {
+                handled =
+                    cx25821_video_upstream_irq(dev, channel_num, vid_status);
+        }
+
+        if (handled < 0)
+                cx25821_stop_upstream_video_ch1(dev);
+        else
+                handled += handled;
+
+        return IRQ_RETVAL(handled);
+}
+
+void cx25821_set_pixelengine(struct cx25821_dev *dev, struct sram_channel *ch,
+                             int pix_format)
+{
+        int width = WIDTH_D1;
+        int height = dev->_lines_count;
+        int num_lines, odd_num_lines;
+        u32 value;
+        int vip_mode = OUTPUT_FRMT_656;
+
+        value = ((pix_format & 0x3) << 12) | (vip_mode & 0x7);
+        value &= 0xFFFFFFEF;
+        value |= dev->_isNTSC ? 0 : 0x10;
+        cx_write(ch->vid_fmt_ctl, value);
+
+        /* set number of active pixels in each line.
+         * Default is 720 pixels in both NTSC and PAL format */
+        cx_write(ch->vid_active_ctl1, width);
+
+        num_lines = (height / 2) & 0x3FF;
+        odd_num_lines = num_lines;
+
+        if (dev->_isNTSC)
+                odd_num_lines += 1;
+
+        value = (num_lines << 16) | odd_num_lines;
+
+        /* set number of active lines in field 0 (top) and field 1 (bottom) */
+        cx_write(ch->vid_active_ctl2, value);
+
+        cx_write(ch->vid_cdt_size, VID_CDT_SIZE >> 3);
+}
+
+int cx25821_start_video_dma_upstream(struct cx25821_dev *dev,
+                                     struct sram_channel *sram_ch)
+{
+        u32 tmp = 0;
+        int err = 0;
+
+        /* 656/VIP SRC Upstream Channel I & J and 7 - Host Bus Interface for
+         * channel A-C
+         */
+        tmp = cx_read(VID_CH_MODE_SEL);
+        cx_write(VID_CH_MODE_SEL, tmp | 0x1B0001FF);
+
+        /* Set the physical start address of the RISC program in the initial
+         * program counter(IPC) member of the cmds.
+         */
+        cx_write(sram_ch->cmds_start + 0, dev->_dma_phys_addr);
+        /* Risc IPC High 64 bits 63-32 */
+        cx_write(sram_ch->cmds_start + 4, 0);
+
+        /* reset counter */
+        cx_write(sram_ch->gpcnt_ctl, 3);
+
+        /* Clear our bits from the interrupt status register. */
+        cx_write(sram_ch->int_stat, _intr_msk);
+
+        /* Set the interrupt mask register, enable irq. */
+        cx_set(PCI_INT_MSK, cx_read(PCI_INT_MSK) | (1 << sram_ch->irq_bit));
+        tmp = cx_read(sram_ch->int_msk);
+        cx_write(sram_ch->int_msk, tmp |= _intr_msk);
+
+        err =
+            request_irq(dev->pci->irq, cx25821_upstream_irq,
+                        IRQF_SHARED, dev->name, dev);
+        if (err < 0) {
+                pr_err("%s: can't get upstream IRQ %d\n",
+                       dev->name, dev->pci->irq);
+                goto fail_irq;
+        }
+
+        /* Start the DMA  engine */
+        tmp = cx_read(sram_ch->dma_ctl);
+        cx_set(sram_ch->dma_ctl, tmp | FLD_VID_RISC_EN);
+
+        dev->_is_running = 1;
+        dev->_is_first_frame = 1;
+
+        return 0;
+
+fail_irq:
+        cx25821_dev_unregister(dev);
+        return err;
+}
+
+int cx25821_vidupstream_init_ch1(struct cx25821_dev *dev, int channel_select,
+                                 int pixel_format)
+{
+        struct sram_channel *sram_ch;
+        u32 tmp;
+        int retval = 0;
+        int err = 0;
+        int data_frame_size = 0;
+        int risc_buffer_size = 0;
+        int str_length = 0;
+
+        if (dev->_is_running) {
+                pr_info("Video Channel is still running so return!\n");
+                return 0;
+        }
+
+        dev->_channel_upstream_select = channel_select;
+        sram_ch = dev->channels[channel_select].sram_channels;
+
+        INIT_WORK(&dev->_irq_work_entry, cx25821_vidups_handler);
+        dev->_irq_queues = create_singlethread_workqueue("cx25821_workqueue");
+
+        if (!dev->_irq_queues) {
+                pr_err("create_singlethread_workqueue() for Video FAILED!\n");
+                return -ENOMEM;
+        }
+        /* 656/VIP SRC Upstream Channel I & J and 7 - Host Bus Interface for
+         * channel A-C
+         */
+        tmp = cx_read(VID_CH_MODE_SEL);
+        cx_write(VID_CH_MODE_SEL, tmp | 0x1B0001FF);
+
+        dev->_is_running = 0;
+        dev->_frame_count = 0;
+        dev->_file_status = RESET_STATUS;
+        dev->_lines_count = dev->_isNTSC ? 480 : 576;
+        dev->_pixel_format = pixel_format;
+        dev->_line_size =
+            (dev->_pixel_format ==
+             PIXEL_FRMT_422) ? (WIDTH_D1 * 2) : (WIDTH_D1 * 3) / 2;
+        data_frame_size = dev->_isNTSC ? NTSC_DATA_BUF_SZ : PAL_DATA_BUF_SZ;
+        risc_buffer_size =
+            dev->_isNTSC ? NTSC_RISC_BUF_SIZE : PAL_RISC_BUF_SIZE;
+
+        if (dev->input_filename) {
+                str_length = strlen(dev->input_filename);
+                dev->_filename = kmalloc(str_length + 1, GFP_KERNEL);
+
+                if (!dev->_filename)
+                        goto error;
+
+                memcpy(dev->_filename, dev->input_filename, str_length + 1);
+        } else {
+                str_length = strlen(dev->_defaultname);
+                dev->_filename = kmalloc(str_length + 1, GFP_KERNEL);
+
+                if (!dev->_filename)
+                        goto error;
+
+                memcpy(dev->_filename, dev->_defaultname, str_length + 1);
+        }
+
+        /* Default if filename is empty string */
+        if (strcmp(dev->input_filename, "") == 0) {
+                if (dev->_isNTSC) {
+                        dev->_filename =
+                            (dev->_pixel_format ==
+                             PIXEL_FRMT_411) ? "/root/vid411.yuv" :
+                            "/root/vidtest.yuv";
+                } else {
+                        dev->_filename =
+                            (dev->_pixel_format ==
+                             PIXEL_FRMT_411) ? "/root/pal411.yuv" :
+                            "/root/pal422.yuv";
+                }
+        }
+
+        dev->_is_running = 0;
+        dev->_frame_count = 0;
+        dev->_file_status = RESET_STATUS;
+        dev->_lines_count = dev->_isNTSC ? 480 : 576;
+        dev->_pixel_format = pixel_format;
+        dev->_line_size =
+            (dev->_pixel_format ==
+             PIXEL_FRMT_422) ? (WIDTH_D1 * 2) : (WIDTH_D1 * 3) / 2;
+
+        retval =
+            cx25821_sram_channel_setup_upstream(dev, sram_ch, dev->_line_size,
+                                                0);
+
+        /* setup fifo + format */
+        cx25821_set_pixelengine(dev, sram_ch, dev->_pixel_format);
+
+        dev->upstream_riscbuf_size = risc_buffer_size * 2;
+        dev->upstream_databuf_size = data_frame_size * 2;
+
+        /* Allocating buffers and prepare RISC program */
+        retval = cx25821_upstream_buffer_prepare(dev, sram_ch, dev->_line_size);
+        if (retval < 0) {
+                pr_err("%s: Failed to set up Video upstream buffers!\n",
+                       dev->name);
+                goto error;
+        }
+
+        cx25821_start_video_dma_upstream(dev, sram_ch);
+
+        return 0;
+
+error:
+        cx25821_dev_unregister(dev);
+
+        return err;
+}
diff --git a/drivers/media/video/cx25821/cx25821-video-upstream.h b/drivers/media/video/cx25821/cx25821-video-upstream.h
new file mode 100644
index 000000000000..268ec8aa6a61
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-video-upstream.h
@@ -0,0 +1,139 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <hiep.huynh@conexant.com>, <shu.lin@conexant.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/mutex.h>
+#include <linux/workqueue.h>
+
+#define OUTPUT_FRMT_656       0
+#define OPEN_FILE_1           0
+#define NUM_PROGS             8
+#define NUM_FRAMES            2
+#define ODD_FIELD             0
+#define EVEN_FIELD            1
+#define TOP_OFFSET            0
+#define FIFO_DISABLE          0
+#define FIFO_ENABLE           1
+#define TEST_FRAMES           5
+#define END_OF_FILE           0
+#define IN_PROGRESS           1
+#define RESET_STATUS          -1
+#define NUM_NO_OPS            5
+
+/* PAL and NTSC line sizes and number of lines. */
+#define WIDTH_D1              720
+#define NTSC_LINES_PER_FRAME  480
+#define PAL_LINES_PER_FRAME   576
+#define PAL_LINE_SZ           1440
+#define Y422_LINE_SZ          1440
+#define Y411_LINE_SZ          1080
+#define NTSC_FIELD_HEIGHT     240
+#define NTSC_ODD_FLD_LINES    241
+#define PAL_FIELD_HEIGHT      288
+
+#define FRAME_SIZE_NTSC_Y422    (NTSC_LINES_PER_FRAME * Y422_LINE_SZ)
+#define FRAME_SIZE_NTSC_Y411    (NTSC_LINES_PER_FRAME * Y411_LINE_SZ)
+#define FRAME_SIZE_PAL_Y422     (PAL_LINES_PER_FRAME * Y422_LINE_SZ)
+#define FRAME_SIZE_PAL_Y411     (PAL_LINES_PER_FRAME * Y411_LINE_SZ)
+
+#define NTSC_DATA_BUF_SZ        (Y422_LINE_SZ * NTSC_LINES_PER_FRAME)
+#define PAL_DATA_BUF_SZ         (Y422_LINE_SZ * PAL_LINES_PER_FRAME)
+
+#define RISC_WRITECR_INSTRUCTION_SIZE   16
+#define RISC_SYNC_INSTRUCTION_SIZE      4
+#define JUMP_INSTRUCTION_SIZE           12
+#define MAXSIZE_NO_OPS                  36
+#define DWORD_SIZE                      4
+
+#define USE_RISC_NOOP_VIDEO   1
+
+#ifdef USE_RISC_NOOP_VIDEO
+#define PAL_US_VID_PROG_SIZE                                            \
+        (PAL_FIELD_HEIGHT * 3 * DWORD_SIZE +                            \
+         RISC_WRITECR_INSTRUCTION_SIZE + RISC_SYNC_INSTRUCTION_SIZE +   \
+         NUM_NO_OPS * DWORD_SIZE)
+
+#define PAL_RISC_BUF_SIZE           (2 * PAL_US_VID_PROG_SIZE)
+
+#define PAL_VID_PROG_SIZE                                               \
+        ((PAL_FIELD_HEIGHT * 2) * 3 * DWORD_SIZE +                      \
+         2 * RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE + \
+         JUMP_INSTRUCTION_SIZE + 2 * NUM_NO_OPS * DWORD_SIZE)
+
+#define ODD_FLD_PAL_PROG_SIZE                                           \
+        (PAL_FIELD_HEIGHT * 3 * DWORD_SIZE +                            \
+         RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE +   \
+         NUM_NO_OPS * DWORD_SIZE)
+
+#define ODD_FLD_NTSC_PROG_SIZE                                          \
+        (NTSC_ODD_FLD_LINES * 3 * DWORD_SIZE +                          \
+         RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE +   \
+         NUM_NO_OPS * DWORD_SIZE)
+
+#define NTSC_US_VID_PROG_SIZE                                           \
+        ((NTSC_ODD_FLD_LINES + 1) * 3 * DWORD_SIZE +                    \
+         RISC_WRITECR_INSTRUCTION_SIZE + JUMP_INSTRUCTION_SIZE +        \
+         NUM_NO_OPS * DWORD_SIZE)
+
+#define NTSC_RISC_BUF_SIZE                                              \
+        (2 * (RISC_SYNC_INSTRUCTION_SIZE + NTSC_US_VID_PROG_SIZE))
+
+#define FRAME1_VID_PROG_SIZE                                            \
+        ((NTSC_ODD_FLD_LINES + NTSC_FIELD_HEIGHT) * 3 * DWORD_SIZE +    \
+         2 * RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE + \
+         JUMP_INSTRUCTION_SIZE + 2 * NUM_NO_OPS * DWORD_SIZE)
+
+#endif
+
+#ifndef USE_RISC_NOOP_VIDEO
+#define PAL_US_VID_PROG_SIZE                                            \
+        (PAL_FIELD_HEIGHT * 3 * DWORD_SIZE +                            \
+         RISC_WRITECR_INSTRUCTION_SIZE + RISC_SYNC_INSTRUCTION_SIZE +   \
+         JUMP_INSTRUCTION_SIZE)
+
+#define PAL_RISC_BUF_SIZE               (2 * PAL_US_VID_PROG_SIZE)
+
+#define PAL_VID_PROG_SIZE                                               \
+        ((PAL_FIELD_HEIGHT * 2) * 3 * DWORD_SIZE +                      \
+         2 * RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE + \
+         JUMP_INSTRUCTION_SIZE)
+
+#define ODD_FLD_PAL_PROG_SIZE                                           \
+        (PAL_FIELD_HEIGHT * 3 * DWORD_SIZE +                            \
+         RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE)
+
+#define ODD_FLD_NTSC_PROG_SIZE                                          \
+        (NTSC_ODD_FLD_LINES * 3 * DWORD_SIZE +                          \
+         RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE)
+
+#define NTSC_US_VID_PROG_SIZE                                           \
+        ((NTSC_ODD_FLD_LINES + 1) * 3 * DWORD_SIZE +                    \
+         RISC_WRITECR_INSTRUCTION_SIZE + JUMP_INSTRUCTION_SIZE)
+
+#define NTSC_RISC_BUF_SIZE                                              \
+        (2 * (RISC_SYNC_INSTRUCTION_SIZE + NTSC_US_VID_PROG_SIZE))
+
+#define FRAME1_VID_PROG_SIZE                                            \
+        ((NTSC_ODD_FLD_LINES + NTSC_FIELD_HEIGHT) * 3 * DWORD_SIZE +    \
+         2 * RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE + \
+         JUMP_INSTRUCTION_SIZE)
+
+#endif
diff --git a/drivers/media/video/cx25821/cx25821-video.c b/drivers/media/video/cx25821/cx25821-video.c
new file mode 100644
index 000000000000..4d6907cda75b
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-video.c
@@ -0,0 +1,2012 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <shu.lin@conexant.com>, <hiep.huynh@conexant.com>
+ *  Based on Steven Toth <stoth@linuxtv.org> cx23885 driver
+ *  Parts adapted/taken from Eduardo Moscoso Rubino
+ *  Copyright (C) 2009 Eduardo Moscoso Rubino <moscoso@TopoLogica.com>
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include "cx25821-video.h"
+
+MODULE_DESCRIPTION("v4l2 driver module for cx25821 based TV cards");
+MODULE_AUTHOR("Hiep Huynh <hiep.huynh@conexant.com>");
+MODULE_LICENSE("GPL");
+
+static unsigned int video_nr[] = {[0 ... (CX25821_MAXBOARDS - 1)] = UNSET };
+static unsigned int radio_nr[] = {[0 ... (CX25821_MAXBOARDS - 1)] = UNSET };
+
+module_param_array(video_nr, int, NULL, 0444);
+module_param_array(radio_nr, int, NULL, 0444);
+
+MODULE_PARM_DESC(video_nr, "video device numbers");
+MODULE_PARM_DESC(radio_nr, "radio device numbers");
+
+static unsigned int video_debug = VIDEO_DEBUG;
+module_param(video_debug, int, 0644);
+MODULE_PARM_DESC(video_debug, "enable debug messages [video]");
+
+static unsigned int irq_debug;
+module_param(irq_debug, int, 0644);
+MODULE_PARM_DESC(irq_debug, "enable debug messages [IRQ handler]");
+
+unsigned int vid_limit = 16;
+module_param(vid_limit, int, 0644);
+MODULE_PARM_DESC(vid_limit, "capture memory limit in megabytes");
+
+static void cx25821_init_controls(struct cx25821_dev *dev, int chan_num);
+
+static const struct v4l2_file_operations video_fops;
+static const struct v4l2_ioctl_ops video_ioctl_ops;
+
+#define FORMAT_FLAGS_PACKED       0x01
+
+struct cx25821_fmt formats[] = {
+        {
+                .name = "8 bpp, gray",
+                .fourcc = V4L2_PIX_FMT_GREY,
+                .depth = 8,
+                .flags = FORMAT_FLAGS_PACKED,
+         }, {
+                .name = "4:1:1, packed, Y41P",
+                .fourcc = V4L2_PIX_FMT_Y41P,
+                .depth = 12,
+                .flags = FORMAT_FLAGS_PACKED,
+        }, {
+                .name = "4:2:2, packed, YUYV",
+                .fourcc = V4L2_PIX_FMT_YUYV,
+                .depth = 16,
+                .flags = FORMAT_FLAGS_PACKED,
+        }, {
+                .name = "4:2:2, packed, UYVY",
+                .fourcc = V4L2_PIX_FMT_UYVY,
+                .depth = 16,
+                .flags = FORMAT_FLAGS_PACKED,
+        }, {
+                .name = "4:2:0, YUV",
+                .fourcc = V4L2_PIX_FMT_YUV420,
+                .depth = 12,
+                .flags = FORMAT_FLAGS_PACKED,
+        },
+};
+
+int cx25821_get_format_size(void)
+{
+        return ARRAY_SIZE(formats);
+}
+
+struct cx25821_fmt *cx25821_format_by_fourcc(unsigned int fourcc)
+{
+        unsigned int i;
+
+        if (fourcc == V4L2_PIX_FMT_Y41P || fourcc == V4L2_PIX_FMT_YUV411P)
+                return formats + 1;
+
+        for (i = 0; i < ARRAY_SIZE(formats); i++)
+                if (formats[i].fourcc == fourcc)
+                        return formats + i;
+
+        pr_err("%s(0x%08x) NOT FOUND\n", __func__, fourcc);
+        return NULL;
+}
+
+void cx25821_dump_video_queue(struct cx25821_dev *dev,
+                              struct cx25821_dmaqueue *q)
+{
+        struct cx25821_buffer *buf;
+        struct list_head *item;
+        dprintk(1, "%s()\n", __func__);
+
+        if (!list_empty(&q->active)) {
+                list_for_each(item, &q->active)
+                    buf = list_entry(item, struct cx25821_buffer, vb.queue);
+        }
+
+        if (!list_empty(&q->queued)) {
+                list_for_each(item, &q->queued)
+                    buf = list_entry(item, struct cx25821_buffer, vb.queue);
+        }
+
+}
+
+void cx25821_video_wakeup(struct cx25821_dev *dev, struct cx25821_dmaqueue *q,
+                          u32 count)
+{
+        struct cx25821_buffer *buf;
+        int bc;
+
+        for (bc = 0;; bc++) {
+                if (list_empty(&q->active)) {
+                        dprintk(1, "bc=%d (=0: active empty)\n", bc);
+                        break;
+                }
+
+                buf =
+                    list_entry(q->active.next, struct cx25821_buffer, vb.queue);
+
+                /* count comes from the hw and it is 16bit wide --
+                 * this trick handles wrap-arounds correctly for
+                 * up to 32767 buffers in flight... */
+                if ((s16) (count - buf->count) < 0)
+                        break;
+
+                do_gettimeofday(&buf->vb.ts);
+                buf->vb.state = VIDEOBUF_DONE;
+                list_del(&buf->vb.queue);
+                wake_up(&buf->vb.done);
+        }
+
+        if (list_empty(&q->active))
+                del_timer(&q->timeout);
+        else
+                mod_timer(&q->timeout, jiffies + BUFFER_TIMEOUT);
+        if (bc != 1)
+                pr_err("%s: %d buffers handled (should be 1)\n", __func__, bc);
+}
+
+#ifdef TUNER_FLAG
+int cx25821_set_tvnorm(struct cx25821_dev *dev, v4l2_std_id norm)
+{
+        dprintk(1, "%s(norm = 0x%08x) name: [%s]\n",
+                __func__, (unsigned int)norm, v4l2_norm_to_name(norm));
+
+        dev->tvnorm = norm;
+
+        /* Tell the internal A/V decoder */
+        cx25821_call_all(dev, core, s_std, norm);
+
+        return 0;
+}
+#endif
+
+struct video_device *cx25821_vdev_init(struct cx25821_dev *dev,
+                                       struct pci_dev *pci,
+                                       struct video_device *template,
+                                       char *type)
+{
+        struct video_device *vfd;
+        dprintk(1, "%s()\n", __func__);
+
+        vfd = video_device_alloc();
+        if (NULL == vfd)
+                return NULL;
+        *vfd = *template;
+        vfd->v4l2_dev = &dev->v4l2_dev;
+        vfd->release = video_device_release;
+        snprintf(vfd->name, sizeof(vfd->name), "%s %s (%s)", dev->name, type,
+                 cx25821_boards[dev->board].name);
+        video_set_drvdata(vfd, dev);
+        return vfd;
+}
+
+/*
+static int cx25821_ctrl_query(struct v4l2_queryctrl *qctrl)
+{
+        int i;
+
+        if (qctrl->id < V4L2_CID_BASE || qctrl->id >= V4L2_CID_LASTP1)
+                return -EINVAL;
+        for (i = 0; i < CX25821_CTLS; i++)
+                if (cx25821_ctls[i].v.id == qctrl->id)
+                        break;
+        if (i == CX25821_CTLS) {
+                *qctrl = no_ctl;
+                return 0;
+        }
+        *qctrl = cx25821_ctls[i].v;
+        return 0;
+}
+*/
+
+/* resource management */
+int cx25821_res_get(struct cx25821_dev *dev, struct cx25821_fh *fh,
+                    unsigned int bit)
+{
+        dprintk(1, "%s()\n", __func__);
+        if (fh->resources & bit)
+                /* have it already allocated */
+                return 1;
+
+        /* is it free? */
+        mutex_lock(&dev->lock);
+        if (dev->channels[fh->channel_id].resources & bit) {
+                /* no, someone else uses it */
+                mutex_unlock(&dev->lock);
+                return 0;
+        }
+        /* it's free, grab it */
+        fh->resources |= bit;
+        dev->channels[fh->channel_id].resources |= bit;
+        dprintk(1, "res: get %d\n", bit);
+        mutex_unlock(&dev->lock);
+        return 1;
+}
+
+int cx25821_res_check(struct cx25821_fh *fh, unsigned int bit)
+{
+        return fh->resources & bit;
+}
+
+int cx25821_res_locked(struct cx25821_fh *fh, unsigned int bit)
+{
+        return fh->dev->channels[fh->channel_id].resources & bit;
+}
+
+void cx25821_res_free(struct cx25821_dev *dev, struct cx25821_fh *fh,
+                      unsigned int bits)
+{
+        BUG_ON((fh->resources & bits) != bits);
+        dprintk(1, "%s()\n", __func__);
+
+        mutex_lock(&dev->lock);
+        fh->resources &= ~bits;
+        dev->channels[fh->channel_id].resources &= ~bits;
+        dprintk(1, "res: put %d\n", bits);
+        mutex_unlock(&dev->lock);
+}
+
+int cx25821_video_mux(struct cx25821_dev *dev, unsigned int input)
+{
+        struct v4l2_routing route;
+        memset(&route, 0, sizeof(route));
+
+        dprintk(1, "%s(): video_mux: %d [vmux=%d, gpio=0x%x,0x%x,0x%x,0x%x]\n",
+                __func__, input, INPUT(input)->vmux, INPUT(input)->gpio0,
+                INPUT(input)->gpio1, INPUT(input)->gpio2, INPUT(input)->gpio3);
+        dev->input = input;
+
+        route.input = INPUT(input)->vmux;
+
+        /* Tell the internal A/V decoder */
+        cx25821_call_all(dev, video, s_routing, INPUT(input)->vmux, 0, 0);
+
+        return 0;
+}
+
+int cx25821_start_video_dma(struct cx25821_dev *dev,
+                            struct cx25821_dmaqueue *q,
+                            struct cx25821_buffer *buf,
+                            struct sram_channel *channel)
+{
+        int tmp = 0;
+
+        /* setup fifo + format */
+        cx25821_sram_channel_setup(dev, channel, buf->bpl, buf->risc.dma);
+
+        /* reset counter */
+        cx_write(channel->gpcnt_ctl, 3);
+        q->count = 1;
+
+        /* enable irq */
+        cx_set(PCI_INT_MSK, cx_read(PCI_INT_MSK) | (1 << channel->i));
+        cx_set(channel->int_msk, 0x11);
+
+        /* start dma */
+        cx_write(channel->dma_ctl, 0x11);       /* FIFO and RISC enable */
+
+        /* make sure upstream setting if any is reversed */
+        tmp = cx_read(VID_CH_MODE_SEL);
+        cx_write(VID_CH_MODE_SEL, tmp & 0xFFFFFE00);
+
+        return 0;
+}
+
+int cx25821_restart_video_queue(struct cx25821_dev *dev,
+                                struct cx25821_dmaqueue *q,
+                                struct sram_channel *channel)
+{
+        struct cx25821_buffer *buf, *prev;
+        struct list_head *item;
+
+        if (!list_empty(&q->active)) {
+                buf =
+                    list_entry(q->active.next, struct cx25821_buffer, vb.queue);
+
+                cx25821_start_video_dma(dev, q, buf, channel);
+
+                list_for_each(item, &q->active) {
+                        buf = list_entry(item, struct cx25821_buffer, vb.queue);
+                        buf->count = q->count++;
+                }
+
+                mod_timer(&q->timeout, jiffies + BUFFER_TIMEOUT);
+                return 0;
+        }
+
+        prev = NULL;
+        for (;;) {
+                if (list_empty(&q->queued))
+                        return 0;
+
+                buf =
+                    list_entry(q->queued.next, struct cx25821_buffer, vb.queue);
+
+                if (NULL == prev) {
+                        list_move_tail(&buf->vb.queue, &q->active);
+                        cx25821_start_video_dma(dev, q, buf, channel);
+                        buf->vb.state = VIDEOBUF_ACTIVE;
+                        buf->count = q->count++;
+                        mod_timer(&q->timeout, jiffies + BUFFER_TIMEOUT);
+                } else if (prev->vb.width == buf->vb.width &&
+                           prev->vb.height == buf->vb.height &&
+                           prev->fmt == buf->fmt) {
+                        list_move_tail(&buf->vb.queue, &q->active);
+                        buf->vb.state = VIDEOBUF_ACTIVE;
+                        buf->count = q->count++;
+                        prev->risc.jmp[1] = cpu_to_le32(buf->risc.dma);
+                        prev->risc.jmp[2] = cpu_to_le32(0); /* Bits 63 - 32 */
+                } else {
+                        return 0;
+                }
+                prev = buf;
+        }
+}
+
+void cx25821_vid_timeout(unsigned long data)
+{
+        struct cx25821_data *timeout_data = (struct cx25821_data *)data;
+        struct cx25821_dev *dev = timeout_data->dev;
+        struct sram_channel *channel = timeout_data->channel;
+        struct cx25821_dmaqueue *q = &dev->channels[channel->i].vidq;
+        struct cx25821_buffer *buf;
+        unsigned long flags;
+
+        /* cx25821_sram_channel_dump(dev, channel); */
+        cx_clear(channel->dma_ctl, 0x11);
+
+        spin_lock_irqsave(&dev->slock, flags);
+        while (!list_empty(&q->active)) {
+                buf =
+                    list_entry(q->active.next, struct cx25821_buffer, vb.queue);
+                list_del(&buf->vb.queue);
+
+                buf->vb.state = VIDEOBUF_ERROR;
+                wake_up(&buf->vb.done);
+        }
+
+        cx25821_restart_video_queue(dev, q, channel);
+        spin_unlock_irqrestore(&dev->slock, flags);
+}
+
+int cx25821_video_irq(struct cx25821_dev *dev, int chan_num, u32 status)
+{
+        u32 count = 0;
+        int handled = 0;
+        u32 mask;
+        struct sram_channel *channel = dev->channels[chan_num].sram_channels;
+
+        mask = cx_read(channel->int_msk);
+        if (0 == (status & mask))
+                return handled;
+
+        cx_write(channel->int_stat, status);
+
+        /* risc op code error */
+        if (status & (1 << 16)) {
+                pr_warn("%s, %s: video risc op code error\n",
+                        dev->name, channel->name);
+                cx_clear(channel->dma_ctl, 0x11);
+                cx25821_sram_channel_dump(dev, channel);
+        }
+
+        /* risc1 y */
+        if (status & FLD_VID_DST_RISC1) {
+                spin_lock(&dev->slock);
+                count = cx_read(channel->gpcnt);
+                cx25821_video_wakeup(dev, &dev->channels[channel->i].vidq,
+                                count);
+                spin_unlock(&dev->slock);
+                handled++;
+        }
+
+        /* risc2 y */
+        if (status & 0x10) {
+                dprintk(2, "stopper video\n");
+                spin_lock(&dev->slock);
+                cx25821_restart_video_queue(dev,
+                                &dev->channels[channel->i].vidq, channel);
+                spin_unlock(&dev->slock);
+                handled++;
+        }
+        return handled;
+}
+
+void cx25821_videoioctl_unregister(struct cx25821_dev *dev)
+{
+        if (dev->ioctl_dev) {
+                if (video_is_registered(dev->ioctl_dev))
+                        video_unregister_device(dev->ioctl_dev);
+                else
+                        video_device_release(dev->ioctl_dev);
+
+                dev->ioctl_dev = NULL;
+        }
+}
+
+void cx25821_video_unregister(struct cx25821_dev *dev, int chan_num)
+{
+        cx_clear(PCI_INT_MSK, 1);
+
+        if (dev->channels[chan_num].video_dev) {
+                if (video_is_registered(dev->channels[chan_num].video_dev))
+                        video_unregister_device(
+                                        dev->channels[chan_num].video_dev);
+                else
+                        video_device_release(
+                                        dev->channels[chan_num].video_dev);
+
+                dev->channels[chan_num].video_dev = NULL;
+
+                btcx_riscmem_free(dev->pci,
+                                &dev->channels[chan_num].vidq.stopper);
+
+                pr_warn("device %d released!\n", chan_num);
+        }
+
+}
+
+int cx25821_video_register(struct cx25821_dev *dev)
+{
+        int err;
+        int i;
+
+        struct video_device cx25821_video_device = {
+                .name = "cx25821-video",
+                .fops = &video_fops,
+                .minor = -1,
+                .ioctl_ops = &video_ioctl_ops,
+                .tvnorms = CX25821_NORMS,
+                .current_norm = V4L2_STD_NTSC_M,
+        };
+
+        spin_lock_init(&dev->slock);
+
+        for (i = 0; i < MAX_VID_CHANNEL_NUM - 1; ++i) {
+                cx25821_init_controls(dev, i);
+
+                cx25821_risc_stopper(dev->pci, &dev->channels[i].vidq.stopper,
+                                dev->channels[i].sram_channels->dma_ctl,
+                                0x11, 0);
+
+                dev->channels[i].sram_channels = &cx25821_sram_channels[i];
+                dev->channels[i].video_dev = NULL;
+                dev->channels[i].resources = 0;
+
+                cx_write(dev->channels[i].sram_channels->int_stat, 0xffffffff);
+
+                INIT_LIST_HEAD(&dev->channels[i].vidq.active);
+                INIT_LIST_HEAD(&dev->channels[i].vidq.queued);
+
+                dev->channels[i].timeout_data.dev = dev;
+                dev->channels[i].timeout_data.channel =
+                        &cx25821_sram_channels[i];
+                dev->channels[i].vidq.timeout.function =
+                        cx25821_vid_timeout;
+                dev->channels[i].vidq.timeout.data =
+                        (unsigned long)&dev->channels[i].timeout_data;
+                init_timer(&dev->channels[i].vidq.timeout);
+
+                /* register v4l devices */
+                dev->channels[i].video_dev = cx25821_vdev_init(dev,
+                                dev->pci, &cx25821_video_device, "video");
+
+                err = video_register_device(dev->channels[i].video_dev,
+                                VFL_TYPE_GRABBER, video_nr[dev->nr]);
+
+                if (err < 0)
+                        goto fail_unreg;
+
+        }
+
+        /* set PCI interrupt */
+        cx_set(PCI_INT_MSK, 0xff);
+
+        /* initial device configuration */
+        mutex_lock(&dev->lock);
+#ifdef TUNER_FLAG
+        dev->tvnorm = cx25821_video_device.current_norm;
+        cx25821_set_tvnorm(dev, dev->tvnorm);
+#endif
+        mutex_unlock(&dev->lock);
+
+
+        return 0;
+
+fail_unreg:
+        cx25821_video_unregister(dev, i);
+        return err;
+}
+
+int cx25821_buffer_setup(struct videobuf_queue *q, unsigned int *count,
+                 unsigned int *size)
+{
+        struct cx25821_fh *fh = q->priv_data;
+
+        *size = fh->fmt->depth * fh->width * fh->height >> 3;
+
+        if (0 == *count)
+                *count = 32;
+
+        if (*size * *count > vid_limit * 1024 * 1024)
+                *count = (vid_limit * 1024 * 1024) / *size;
+
+        return 0;
+}
+
+int cx25821_buffer_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,
+                   enum v4l2_field field)
+{
+        struct cx25821_fh *fh = q->priv_data;
+        struct cx25821_dev *dev = fh->dev;
+        struct cx25821_buffer *buf =
+            container_of(vb, struct cx25821_buffer, vb);
+        int rc, init_buffer = 0;
+        u32 line0_offset, line1_offset;
+        struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
+        int bpl_local = LINE_SIZE_D1;
+        int channel_opened = fh->channel_id;
+
+        BUG_ON(NULL == fh->fmt);
+        if (fh->width < 48 || fh->width > 720 ||
+            fh->height < 32 || fh->height > 576)
+                return -EINVAL;
+
+        buf->vb.size = (fh->width * fh->height * fh->fmt->depth) >> 3;
+
+        if (0 != buf->vb.baddr && buf->vb.bsize < buf->vb.size)
+                return -EINVAL;
+
+        if (buf->fmt != fh->fmt ||
+            buf->vb.width != fh->width ||
+            buf->vb.height != fh->height || buf->vb.field != field) {
+                buf->fmt = fh->fmt;
+                buf->vb.width = fh->width;
+                buf->vb.height = fh->height;
+                buf->vb.field = field;
+                init_buffer = 1;
+        }
+
+        if (VIDEOBUF_NEEDS_INIT == buf->vb.state) {
+                init_buffer = 1;
+                rc = videobuf_iolock(q, &buf->vb, NULL);
+                if (0 != rc) {
+                        printk(KERN_DEBUG pr_fmt("videobuf_iolock failed!\n"));
+                        goto fail;
+                }
+        }
+
+        dprintk(1, "init_buffer=%d\n", init_buffer);
+
+        if (init_buffer) {
+
+                channel_opened = dev->channel_opened;
+                if (channel_opened < 0 || channel_opened > 7)
+                        channel_opened = 7;
+
+                if (dev->channels[channel_opened].pixel_formats ==
+                                PIXEL_FRMT_411)
+                        buf->bpl = (buf->fmt->depth * buf->vb.width) >> 3;
+                else
+                        buf->bpl = (buf->fmt->depth >> 3) * (buf->vb.width);
+
+                if (dev->channels[channel_opened].pixel_formats ==
+                                PIXEL_FRMT_411) {
+                        bpl_local = buf->bpl;
+                } else {
+                        bpl_local = buf->bpl;   /* Default */
+
+                        if (channel_opened >= 0 && channel_opened <= 7) {
+                                if (dev->channels[channel_opened]
+                                                .use_cif_resolution) {
+                                        if (dev->tvnorm & V4L2_STD_PAL_BG
+                                            || dev->tvnorm & V4L2_STD_PAL_DK)
+                                                bpl_local = 352 << 1;
+                                        else
+                                                bpl_local =
+                                                 dev->channels[channel_opened].
+                                                 cif_width <<
+                                                 1;
+                                }
+                        }
+                }
+
+                switch (buf->vb.field) {
+                case V4L2_FIELD_TOP:
+                        cx25821_risc_buffer(dev->pci, &buf->risc,
+                                            dma->sglist, 0, UNSET,
+                                            buf->bpl, 0, buf->vb.height);
+                        break;
+                case V4L2_FIELD_BOTTOM:
+                        cx25821_risc_buffer(dev->pci, &buf->risc,
+                                            dma->sglist, UNSET, 0,
+                                            buf->bpl, 0, buf->vb.height);
+                        break;
+                case V4L2_FIELD_INTERLACED:
+                        /* All other formats are top field first */
+                        line0_offset = 0;
+                        line1_offset = buf->bpl;
+                        dprintk(1, "top field first\n");
+
+                        cx25821_risc_buffer(dev->pci, &buf->risc,
+                                            dma->sglist, line0_offset,
+                                            bpl_local, bpl_local, bpl_local,
+                                            buf->vb.height >> 1);
+                        break;
+                case V4L2_FIELD_SEQ_TB:
+                        cx25821_risc_buffer(dev->pci, &buf->risc,
+                                            dma->sglist,
+                                            0, buf->bpl * (buf->vb.height >> 1),
+                                            buf->bpl, 0, buf->vb.height >> 1);
+                        break;
+                case V4L2_FIELD_SEQ_BT:
+                        cx25821_risc_buffer(dev->pci, &buf->risc,
+                                            dma->sglist,
+                                            buf->bpl * (buf->vb.height >> 1), 0,
+                                            buf->bpl, 0, buf->vb.height >> 1);
+                        break;
+                default:
+                        BUG();
+                }
+        }
+
+        dprintk(2, "[%p/%d] buffer_prep - %dx%d %dbpp \"%s\" - dma=0x%08lx\n",
+                buf, buf->vb.i, fh->width, fh->height, fh->fmt->depth,
+                fh->fmt->name, (unsigned long)buf->risc.dma);
+
+        buf->vb.state = VIDEOBUF_PREPARED;
+
+        return 0;
+
+fail:
+        cx25821_free_buffer(q, buf);
+        return rc;
+}
+
+void cx25821_buffer_release(struct videobuf_queue *q,
+                            struct videobuf_buffer *vb)
+{
+        struct cx25821_buffer *buf =
+            container_of(vb, struct cx25821_buffer, vb);
+
+        cx25821_free_buffer(q, buf);
+}
+
+struct videobuf_queue *get_queue(struct cx25821_fh *fh)
+{
+        switch (fh->type) {
+        case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+                return &fh->vidq;
+        default:
+                BUG();
+                return NULL;
+        }
+}
+
+int cx25821_get_resource(struct cx25821_fh *fh, int resource)
+{
+        switch (fh->type) {
+        case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+                return resource;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+int cx25821_video_mmap(struct file *file, struct vm_area_struct *vma)
+{
+        struct cx25821_fh *fh = file->private_data;
+
+        return videobuf_mmap_mapper(get_queue(fh), vma);
+}
+
+
+static void buffer_queue(struct videobuf_queue *vq, struct videobuf_buffer *vb)
+{
+        struct cx25821_buffer *buf =
+           container_of(vb, struct cx25821_buffer, vb);
+        struct cx25821_buffer *prev;
+        struct cx25821_fh *fh = vq->priv_data;
+        struct cx25821_dev *dev = fh->dev;
+        struct cx25821_dmaqueue *q = &dev->channels[fh->channel_id].vidq;
+
+        /* add jump to stopper */
+        buf->risc.jmp[0] = cpu_to_le32(RISC_JUMP | RISC_IRQ1 | RISC_CNT_INC);
+        buf->risc.jmp[1] = cpu_to_le32(q->stopper.dma);
+        buf->risc.jmp[2] = cpu_to_le32(0);      /* bits 63-32 */
+
+        dprintk(2, "jmp to stopper (0x%x)\n", buf->risc.jmp[1]);
+
+        if (!list_empty(&q->queued)) {
+                list_add_tail(&buf->vb.queue, &q->queued);
+                buf->vb.state = VIDEOBUF_QUEUED;
+                dprintk(2, "[%p/%d] buffer_queue - append to queued\n", buf,
+                                buf->vb.i);
+
+        } else if (list_empty(&q->active)) {
+                list_add_tail(&buf->vb.queue, &q->active);
+                cx25821_start_video_dma(dev, q, buf,
+                                dev->channels[fh->channel_id].sram_channels);
+                buf->vb.state = VIDEOBUF_ACTIVE;
+                buf->count = q->count++;
+                mod_timer(&q->timeout, jiffies + BUFFER_TIMEOUT);
+                dprintk(2, "[%p/%d] buffer_queue - first active, buf cnt = %d, q->count = %d\n",
+                                buf, buf->vb.i, buf->count, q->count);
+        } else {
+                prev = list_entry(q->active.prev, struct cx25821_buffer,
+                                vb.queue);
+                if (prev->vb.width == buf->vb.width
+                   && prev->vb.height == buf->vb.height
+                   && prev->fmt == buf->fmt) {
+                        list_add_tail(&buf->vb.queue, &q->active);
+                        buf->vb.state = VIDEOBUF_ACTIVE;
+                        buf->count = q->count++;
+                        prev->risc.jmp[1] = cpu_to_le32(buf->risc.dma);
+
+                        /* 64 bit bits 63-32 */
+                        prev->risc.jmp[2] = cpu_to_le32(0);
+                        dprintk(2, "[%p/%d] buffer_queue - append to active, buf->count=%d\n",
+                                        buf, buf->vb.i, buf->count);
+
+                } else {
+                        list_add_tail(&buf->vb.queue, &q->queued);
+                        buf->vb.state = VIDEOBUF_QUEUED;
+                        dprintk(2, "[%p/%d] buffer_queue - first queued\n", buf,
+                                        buf->vb.i);
+                }
+        }
+
+        if (list_empty(&q->active))
+                dprintk(2, "active queue empty!\n");
+}
+
+static struct videobuf_queue_ops cx25821_video_qops = {
+        .buf_setup = cx25821_buffer_setup,
+        .buf_prepare = cx25821_buffer_prepare,
+        .buf_queue = buffer_queue,
+        .buf_release = cx25821_buffer_release,
+};
+
+static int video_open(struct file *file)
+{
+        struct video_device *vdev = video_devdata(file);
+        struct cx25821_dev *h, *dev = video_drvdata(file);
+        struct cx25821_fh *fh;
+        struct list_head *list;
+        int minor = video_devdata(file)->minor;
+        enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        u32 pix_format;
+        int ch_id = 0;
+        int i;
+
+        dprintk(1, "open dev=%s type=%s\n", video_device_node_name(vdev),
+                        v4l2_type_names[type]);
+
+        /* allocate + initialize per filehandle data */
+        fh = kzalloc(sizeof(*fh), GFP_KERNEL);
+        if (NULL == fh)
+                return -ENOMEM;
+
+        mutex_lock(&cx25821_devlist_mutex);
+
+        list_for_each(list, &cx25821_devlist)
+        {
+                h = list_entry(list, struct cx25821_dev, devlist);
+
+                for (i = 0; i < MAX_VID_CHANNEL_NUM; i++) {
+                        if (h->channels[i].video_dev &&
+                           h->channels[i].video_dev->minor == minor) {
+                                dev = h;
+                                ch_id = i;
+                                type  = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+                        }
+                }
+        }
+
+        if (NULL == dev) {
+                mutex_unlock(&cx25821_devlist_mutex);
+                kfree(fh);
+                return -ENODEV;
+        }
+
+        file->private_data = fh;
+        fh->dev = dev;
+        fh->type = type;
+        fh->width = 720;
+        fh->channel_id = ch_id;
+
+        if (dev->tvnorm & V4L2_STD_PAL_BG || dev->tvnorm & V4L2_STD_PAL_DK)
+                fh->height = 576;
+        else
+                fh->height = 480;
+
+        dev->channel_opened = fh->channel_id;
+        if (dev->channels[ch_id].pixel_formats == PIXEL_FRMT_411)
+                pix_format = V4L2_PIX_FMT_Y41P;
+        else
+                pix_format = V4L2_PIX_FMT_YUYV;
+        fh->fmt = cx25821_format_by_fourcc(pix_format);
+
+        v4l2_prio_open(&dev->channels[ch_id].prio, &fh->prio);
+
+        videobuf_queue_sg_init(&fh->vidq, &cx25821_video_qops,
+                              &dev->pci->dev, &dev->slock,
+                              V4L2_BUF_TYPE_VIDEO_CAPTURE,
+                              V4L2_FIELD_INTERLACED,
+                              sizeof(struct cx25821_buffer), fh, NULL);
+
+        dprintk(1, "post videobuf_queue_init()\n");
+        mutex_unlock(&cx25821_devlist_mutex);
+
+        return 0;
+}
+
+static ssize_t video_read(struct file *file, char __user * data, size_t count,
+                         loff_t *ppos)
+{
+        struct cx25821_fh *fh = file->private_data;
+
+        switch (fh->type) {
+        case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+                if (cx25821_res_locked(fh, RESOURCE_VIDEO0))
+                        return -EBUSY;
+
+                return videobuf_read_one(&fh->vidq, data, count, ppos,
+                                        file->f_flags & O_NONBLOCK);
+
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+static unsigned int video_poll(struct file *file,
+                              struct poll_table_struct *wait)
+{
+        struct cx25821_fh *fh = file->private_data;
+        struct cx25821_buffer *buf;
+
+        if (cx25821_res_check(fh, RESOURCE_VIDEO0)) {
+                /* streaming capture */
+                if (list_empty(&fh->vidq.stream))
+                        return POLLERR;
+                buf = list_entry(fh->vidq.stream.next,
+                                struct cx25821_buffer, vb.stream);
+        } else {
+                /* read() capture */
+                buf = (struct cx25821_buffer *)fh->vidq.read_buf;
+                if (NULL == buf)
+                        return POLLERR;
+        }
+
+        poll_wait(file, &buf->vb.done, wait);
+        if (buf->vb.state == VIDEOBUF_DONE || buf->vb.state == VIDEOBUF_ERROR) {
+                if (buf->vb.state == VIDEOBUF_DONE) {
+                        struct cx25821_dev *dev = fh->dev;
+
+                        if (dev && dev->channels[fh->channel_id]
+                                        .use_cif_resolution) {
+                                u8 cam_id = *((char *)buf->vb.baddr + 3);
+                                memcpy((char *)buf->vb.baddr,
+                                      (char *)buf->vb.baddr + (fh->width * 2),
+                                      (fh->width * 2));
+                                *((char *)buf->vb.baddr + 3) = cam_id;
+                        }
+                }
+
+                return POLLIN | POLLRDNORM;
+        }
+
+        return 0;
+}
+
+static int video_release(struct file *file)
+{
+        struct cx25821_fh *fh = file->private_data;
+        struct cx25821_dev *dev = fh->dev;
+
+        /* stop the risc engine and fifo */
+        cx_write(channel0->dma_ctl, 0); /* FIFO and RISC disable */
+
+        /* stop video capture */
+        if (cx25821_res_check(fh, RESOURCE_VIDEO0)) {
+                videobuf_queue_cancel(&fh->vidq);
+                cx25821_res_free(dev, fh, RESOURCE_VIDEO0);
+        }
+
+        if (fh->vidq.read_buf) {
+                cx25821_buffer_release(&fh->vidq, fh->vidq.read_buf);
+                kfree(fh->vidq.read_buf);
+        }
+
+        videobuf_mmap_free(&fh->vidq);
+
+        v4l2_prio_close(&dev->channels[fh->channel_id].prio, fh->prio);
+        file->private_data = NULL;
+        kfree(fh);
+
+        return 0;
+}
+
+static int vidioc_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
+{
+        struct cx25821_fh *fh = priv;
+        struct cx25821_dev *dev = fh->dev;
+
+        if (unlikely(fh->type != V4L2_BUF_TYPE_VIDEO_CAPTURE))
+                return -EINVAL;
+
+        if (unlikely(i != fh->type))
+                return -EINVAL;
+
+        if (unlikely(!cx25821_res_get(dev, fh, cx25821_get_resource(fh,
+                                                RESOURCE_VIDEO0))))
+                return -EBUSY;
+
+        return videobuf_streamon(get_queue(fh));
+}
+
+static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
+{
+        struct cx25821_fh *fh = priv;
+        struct cx25821_dev *dev = fh->dev;
+        int err, res;
+
+        if (fh->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+                return -EINVAL;
+        if (i != fh->type)
+                return -EINVAL;
+
+        res = cx25821_get_resource(fh, RESOURCE_VIDEO0);
+        err = videobuf_streamoff(get_queue(fh));
+        if (err < 0)
+                return err;
+        cx25821_res_free(dev, fh, res);
+        return 0;
+}
+
+static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
+                                struct v4l2_format *f)
+{
+        struct cx25821_fh *fh = priv;
+        struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
+        struct v4l2_mbus_framefmt mbus_fmt;
+        int err;
+        int pix_format = PIXEL_FRMT_422;
+
+        if (fh) {
+                err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
+                                      fh->prio);
+                if (0 != err)
+                        return err;
+        }
+
+        dprintk(2, "%s()\n", __func__);
+        err = cx25821_vidioc_try_fmt_vid_cap(file, priv, f);
+
+        if (0 != err)
+                return err;
+
+        fh->fmt = cx25821_format_by_fourcc(f->fmt.pix.pixelformat);
+        fh->vidq.field = f->fmt.pix.field;
+
+        /* check if width and height is valid based on set standard */
+        if (cx25821_is_valid_width(f->fmt.pix.width, dev->tvnorm))
+                fh->width = f->fmt.pix.width;
+
+        if (cx25821_is_valid_height(f->fmt.pix.height, dev->tvnorm))
+                fh->height = f->fmt.pix.height;
+
+        if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_Y41P)
+                pix_format = PIXEL_FRMT_411;
+        else if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV)
+                pix_format = PIXEL_FRMT_422;
+        else
+                return -EINVAL;
+
+        cx25821_set_pixel_format(dev, SRAM_CH00, pix_format);
+
+        /* check if cif resolution */
+        if (fh->width == 320 || fh->width == 352)
+                dev->channels[fh->channel_id].use_cif_resolution = 1;
+        else
+                dev->channels[fh->channel_id].use_cif_resolution = 0;
+
+        dev->channels[fh->channel_id].cif_width = fh->width;
+        medusa_set_resolution(dev, fh->width, SRAM_CH00);
+
+        dprintk(2, "%s(): width=%d height=%d field=%d\n", __func__, fh->width,
+                fh->height, fh->vidq.field);
+        v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, V4L2_MBUS_FMT_FIXED);
+        cx25821_call_all(dev, video, s_mbus_fmt, &mbus_fmt);
+
+        return 0;
+}
+
+static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+        int ret_val = 0;
+        struct cx25821_fh *fh = priv;
+        struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
+
+        ret_val = videobuf_dqbuf(get_queue(fh), p, file->f_flags & O_NONBLOCK);
+
+        p->sequence = dev->channels[fh->channel_id].vidq.count;
+
+        return ret_val;
+}
+
+static int vidioc_log_status(struct file *file, void *priv)
+{
+        struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
+        struct cx25821_fh *fh = priv;
+        char name[32 + 2];
+
+        struct sram_channel *sram_ch = dev->channels[fh->channel_id]
+                                                                .sram_channels;
+        u32 tmp = 0;
+
+        snprintf(name, sizeof(name), "%s/2", dev->name);
+        pr_info("%s/2: ============  START LOG STATUS  ============\n",
+                dev->name);
+        cx25821_call_all(dev, core, log_status);
+        tmp = cx_read(sram_ch->dma_ctl);
+        pr_info("Video input 0 is %s\n",
+                (tmp & 0x11) ? "streaming" : "stopped");
+        pr_info("%s/2: =============  END LOG STATUS  =============\n",
+                dev->name);
+        return 0;
+}
+
+static int vidioc_s_ctrl(struct file *file, void *priv,
+                        struct v4l2_control *ctl)
+{
+        struct cx25821_fh *fh = priv;
+        struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
+        int err;
+
+        if (fh) {
+                err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
+                                      fh->prio);
+                if (0 != err)
+                        return err;
+        }
+
+        return cx25821_set_control(dev, ctl, fh->channel_id);
+}
+
+/* VIDEO IOCTLS */
+int cx25821_vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+                                 struct v4l2_format *f)
+{
+        struct cx25821_fh *fh = priv;
+
+        f->fmt.pix.width = fh->width;
+        f->fmt.pix.height = fh->height;
+        f->fmt.pix.field = fh->vidq.field;
+        f->fmt.pix.pixelformat = fh->fmt->fourcc;
+        f->fmt.pix.bytesperline = (f->fmt.pix.width * fh->fmt->depth) >> 3;
+        f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+
+        return 0;
+}
+
+int cx25821_vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+                                   struct v4l2_format *f)
+{
+        struct cx25821_fmt *fmt;
+        enum v4l2_field field;
+        unsigned int maxw, maxh;
+
+        fmt = cx25821_format_by_fourcc(f->fmt.pix.pixelformat);
+        if (NULL == fmt)
+                return -EINVAL;
+
+        field = f->fmt.pix.field;
+        maxw = 720;
+        maxh = 576;
+
+        if (V4L2_FIELD_ANY == field) {
+                if (f->fmt.pix.height > maxh / 2)
+                        field = V4L2_FIELD_INTERLACED;
+                else
+                        field = V4L2_FIELD_TOP;
+        }
+
+        switch (field) {
+        case V4L2_FIELD_TOP:
+        case V4L2_FIELD_BOTTOM:
+                maxh = maxh / 2;
+                break;
+        case V4L2_FIELD_INTERLACED:
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        f->fmt.pix.field = field;
+        if (f->fmt.pix.height < 32)
+                f->fmt.pix.height = 32;
+        if (f->fmt.pix.height > maxh)
+                f->fmt.pix.height = maxh;
+        if (f->fmt.pix.width < 48)
+                f->fmt.pix.width = 48;
+        if (f->fmt.pix.width > maxw)
+                f->fmt.pix.width = maxw;
+        f->fmt.pix.width &= ~0x03;
+        f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
+        f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+
+        return 0;
+}
+
+int cx25821_vidioc_querycap(struct file *file, void *priv,
+                            struct v4l2_capability *cap)
+{
+        struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
+
+        strcpy(cap->driver, "cx25821");
+        strlcpy(cap->card, cx25821_boards[dev->board].name, sizeof(cap->card));
+        sprintf(cap->bus_info, "PCIe:%s", pci_name(dev->pci));
+        cap->version = CX25821_VERSION_CODE;
+        cap->capabilities =
+            V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+        if (UNSET != dev->tuner_type)
+                cap->capabilities |= V4L2_CAP_TUNER;
+        return 0;
+}
+
+int cx25821_vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
+                            struct v4l2_fmtdesc *f)
+{
+        if (unlikely(f->index >= ARRAY_SIZE(formats)))
+                return -EINVAL;
+
+        strlcpy(f->description, formats[f->index].name, sizeof(f->description));
+        f->pixelformat = formats[f->index].fourcc;
+
+        return 0;
+}
+
+int cx25821_vidioc_reqbufs(struct file *file, void *priv,
+                           struct v4l2_requestbuffers *p)
+{
+        struct cx25821_fh *fh = priv;
+        return videobuf_reqbufs(get_queue(fh), p);
+}
+
+int cx25821_vidioc_querybuf(struct file *file, void *priv,
+                            struct v4l2_buffer *p)
+{
+        struct cx25821_fh *fh = priv;
+        return videobuf_querybuf(get_queue(fh), p);
+}
+
+int cx25821_vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+        struct cx25821_fh *fh = priv;
+        return videobuf_qbuf(get_queue(fh), p);
+}
+
+int cx25821_vidioc_g_priority(struct file *file, void *f, enum v4l2_priority *p)
+{
+        struct cx25821_dev *dev = ((struct cx25821_fh *)f)->dev;
+        struct cx25821_fh *fh = f;
+
+        *p = v4l2_prio_max(&dev->channels[fh->channel_id].prio);
+
+        return 0;
+}
+
+int cx25821_vidioc_s_priority(struct file *file, void *f,
+                              enum v4l2_priority prio)
+{
+        struct cx25821_fh *fh = f;
+        struct cx25821_dev *dev = ((struct cx25821_fh *)f)->dev;
+
+        return v4l2_prio_change(&dev->channels[fh->channel_id].prio, &fh->prio,
+                        prio);
+}
+
+#ifdef TUNER_FLAG
+int cx25821_vidioc_s_std(struct file *file, void *priv, v4l2_std_id * tvnorms)
+{
+        struct cx25821_fh *fh = priv;
+        struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
+        int err;
+
+        dprintk(1, "%s()\n", __func__);
+
+        if (fh) {
+                err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
+                                      fh->prio);
+                if (0 != err)
+                        return err;
+        }
+
+        if (dev->tvnorm == *tvnorms)
+                return 0;
+
+        mutex_lock(&dev->lock);
+        cx25821_set_tvnorm(dev, *tvnorms);
+        mutex_unlock(&dev->lock);
+
+        medusa_set_videostandard(dev);
+
+        return 0;
+}
+#endif
+
+int cx25821_enum_input(struct cx25821_dev *dev, struct v4l2_input *i)
+{
+        static const char * const iname[] = {
+                [CX25821_VMUX_COMPOSITE] = "Composite",
+                [CX25821_VMUX_SVIDEO] = "S-Video",
+                [CX25821_VMUX_DEBUG] = "for debug only",
+        };
+        unsigned int n;
+        dprintk(1, "%s()\n", __func__);
+
+        n = i->index;
+        if (n >= 2)
+                return -EINVAL;
+
+        if (0 == INPUT(n)->type)
+                return -EINVAL;
+
+        i->type = V4L2_INPUT_TYPE_CAMERA;
+        strcpy(i->name, iname[INPUT(n)->type]);
+
+        i->std = CX25821_NORMS;
+        return 0;
+}
+
+int cx25821_vidioc_enum_input(struct file *file, void *priv,
+                              struct v4l2_input *i)
+{
+        struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
+        dprintk(1, "%s()\n", __func__);
+        return cx25821_enum_input(dev, i);
+}
+
+int cx25821_vidioc_g_input(struct file *file, void *priv, unsigned int *i)
+{
+        struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
+
+        *i = dev->input;
+        dprintk(1, "%s(): returns %d\n", __func__, *i);
+        return 0;
+}
+
+int cx25821_vidioc_s_input(struct file *file, void *priv, unsigned int i)
+{
+        struct cx25821_fh *fh = priv;
+        struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
+        int err;
+
+        dprintk(1, "%s(%d)\n", __func__, i);
+
+        if (fh) {
+                err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
+                                      fh->prio);
+                if (0 != err)
+                        return err;
+        }
+
+        if (i >= CX25821_NR_INPUT) {
+                dprintk(1, "%s(): -EINVAL\n", __func__);
+                return -EINVAL;
+        }
+
+        mutex_lock(&dev->lock);
+        cx25821_video_mux(dev, i);
+        mutex_unlock(&dev->lock);
+        return 0;
+}
+
+#ifdef TUNER_FLAG
+int cx25821_vidioc_g_frequency(struct file *file, void *priv,
+                               struct v4l2_frequency *f)
+{
+        struct cx25821_fh *fh = priv;
+        struct cx25821_dev *dev = fh->dev;
+
+        f->frequency = dev->freq;
+
+        cx25821_call_all(dev, tuner, g_frequency, f);
+
+        return 0;
+}
+
+int cx25821_set_freq(struct cx25821_dev *dev, struct v4l2_frequency *f)
+{
+        mutex_lock(&dev->lock);
+        dev->freq = f->frequency;
+
+        cx25821_call_all(dev, tuner, s_frequency, f);
+
+        /* When changing channels it is required to reset TVAUDIO */
+        msleep(10);
+
+        mutex_unlock(&dev->lock);
+
+        return 0;
+}
+
+int cx25821_vidioc_s_frequency(struct file *file, void *priv,
+                               struct v4l2_frequency *f)
+{
+        struct cx25821_fh *fh = priv;
+        struct cx25821_dev *dev;
+        int err;
+
+        if (fh) {
+                dev = fh->dev;
+                err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
+                                      fh->prio);
+                if (0 != err)
+                        return err;
+        } else {
+                pr_err("Invalid fh pointer!\n");
+                return -EINVAL;
+        }
+
+        return cx25821_set_freq(dev, f);
+}
+#endif
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+int cx25821_vidioc_g_register(struct file *file, void *fh,
+                      struct v4l2_dbg_register *reg)
+{
+        struct cx25821_dev *dev = ((struct cx25821_fh *)fh)->dev;
+
+        if (!v4l2_chip_match_host(&reg->match))
+                return -EINVAL;
+
+        cx25821_call_all(dev, core, g_register, reg);
+
+        return 0;
+}
+
+int cx25821_vidioc_s_register(struct file *file, void *fh,
+                      struct v4l2_dbg_register *reg)
+{
+        struct cx25821_dev *dev = ((struct cx25821_fh *)fh)->dev;
+
+        if (!v4l2_chip_match_host(&reg->match))
+                return -EINVAL;
+
+        cx25821_call_all(dev, core, s_register, reg);
+
+        return 0;
+}
+
+#endif
+
+#ifdef TUNER_FLAG
+int cx25821_vidioc_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
+{
+        struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
+
+        if (unlikely(UNSET == dev->tuner_type))
+                return -EINVAL;
+        if (0 != t->index)
+                return -EINVAL;
+
+        strcpy(t->name, "Television");
+        t->type = V4L2_TUNER_ANALOG_TV;
+        t->capability = V4L2_TUNER_CAP_NORM;
+        t->rangehigh = 0xffffffffUL;
+
+        t->signal = 0xffff;     /* LOCKED */
+        return 0;
+}
+
+int cx25821_vidioc_s_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
+{
+        struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
+        struct cx25821_fh *fh = priv;
+        int err;
+
+        if (fh) {
+                err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
+                                      fh->prio);
+                if (0 != err)
+                        return err;
+        }
+
+        dprintk(1, "%s()\n", __func__);
+        if (UNSET == dev->tuner_type)
+                return -EINVAL;
+        if (0 != t->index)
+                return -EINVAL;
+
+        return 0;
+}
+
+#endif
+/*****************************************************************************/
+static const struct v4l2_queryctrl no_ctl = {
+        .name = "42",
+        .flags = V4L2_CTRL_FLAG_DISABLED,
+};
+
+static struct v4l2_queryctrl cx25821_ctls[] = {
+        /* --- video --- */
+        {
+         .id = V4L2_CID_BRIGHTNESS,
+         .name = "Brightness",
+         .minimum = 0,
+         .maximum = 10000,
+         .step = 1,
+         .default_value = 6200,
+         .type = V4L2_CTRL_TYPE_INTEGER,
+         }, {
+             .id = V4L2_CID_CONTRAST,
+             .name = "Contrast",
+             .minimum = 0,
+             .maximum = 10000,
+             .step = 1,
+             .default_value = 5000,
+             .type = V4L2_CTRL_TYPE_INTEGER,
+             }, {
+                 .id = V4L2_CID_SATURATION,
+                 .name = "Saturation",
+                 .minimum = 0,
+                 .maximum = 10000,
+                 .step = 1,
+                 .default_value = 5000,
+                 .type = V4L2_CTRL_TYPE_INTEGER,
+                 }, {
+                     .id = V4L2_CID_HUE,
+                     .name = "Hue",
+                     .minimum = 0,
+                     .maximum = 10000,
+                     .step = 1,
+                     .default_value = 5000,
+                     .type = V4L2_CTRL_TYPE_INTEGER,
+                     }
+};
+static const int CX25821_CTLS = ARRAY_SIZE(cx25821_ctls);
+
+static int cx25821_ctrl_query(struct v4l2_queryctrl *qctrl)
+{
+        int i;
+
+        if (qctrl->id < V4L2_CID_BASE || qctrl->id >= V4L2_CID_LASTP1)
+                return -EINVAL;
+        for (i = 0; i < CX25821_CTLS; i++)
+                if (cx25821_ctls[i].id == qctrl->id)
+                        break;
+        if (i == CX25821_CTLS) {
+                *qctrl = no_ctl;
+                return 0;
+        }
+        *qctrl = cx25821_ctls[i];
+        return 0;
+}
+
+int cx25821_vidioc_queryctrl(struct file *file, void *priv,
+                     struct v4l2_queryctrl *qctrl)
+{
+        return cx25821_ctrl_query(qctrl);
+}
+
+/* ------------------------------------------------------------------ */
+/* VIDEO CTRL IOCTLS                                                  */
+
+static const struct v4l2_queryctrl *ctrl_by_id(unsigned int id)
+{
+        unsigned int i;
+
+        for (i = 0; i < CX25821_CTLS; i++)
+                if (cx25821_ctls[i].id == id)
+                        return cx25821_ctls + i;
+        return NULL;
+}
+
+int cx25821_vidioc_g_ctrl(struct file *file, void *priv,
+                          struct v4l2_control *ctl)
+{
+        struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
+        struct cx25821_fh *fh = priv;
+
+        const struct v4l2_queryctrl *ctrl;
+
+        ctrl = ctrl_by_id(ctl->id);
+
+        if (NULL == ctrl)
+                return -EINVAL;
+        switch (ctl->id) {
+        case V4L2_CID_BRIGHTNESS:
+                ctl->value = dev->channels[fh->channel_id].ctl_bright;
+                break;
+        case V4L2_CID_HUE:
+                ctl->value = dev->channels[fh->channel_id].ctl_hue;
+                break;
+        case V4L2_CID_CONTRAST:
+                ctl->value = dev->channels[fh->channel_id].ctl_contrast;
+                break;
+        case V4L2_CID_SATURATION:
+                ctl->value = dev->channels[fh->channel_id].ctl_saturation;
+                break;
+        }
+        return 0;
+}
+
+int cx25821_set_control(struct cx25821_dev *dev,
+                        struct v4l2_control *ctl, int chan_num)
+{
+        int err;
+        const struct v4l2_queryctrl *ctrl;
+
+        err = -EINVAL;
+
+        ctrl = ctrl_by_id(ctl->id);
+
+        if (NULL == ctrl)
+                return err;
+
+        switch (ctrl->type) {
+        case V4L2_CTRL_TYPE_BOOLEAN:
+        case V4L2_CTRL_TYPE_MENU:
+        case V4L2_CTRL_TYPE_INTEGER:
+                if (ctl->value < ctrl->minimum)
+                        ctl->value = ctrl->minimum;
+                if (ctl->value > ctrl->maximum)
+                        ctl->value = ctrl->maximum;
+                break;
+        default:
+                /* nothing */ ;
+        }
+
+        switch (ctl->id) {
+        case V4L2_CID_BRIGHTNESS:
+                dev->channels[chan_num].ctl_bright = ctl->value;
+                medusa_set_brightness(dev, ctl->value, chan_num);
+                break;
+        case V4L2_CID_HUE:
+                dev->channels[chan_num].ctl_hue = ctl->value;
+                medusa_set_hue(dev, ctl->value, chan_num);
+                break;
+        case V4L2_CID_CONTRAST:
+                dev->channels[chan_num].ctl_contrast = ctl->value;
+                medusa_set_contrast(dev, ctl->value, chan_num);
+                break;
+        case V4L2_CID_SATURATION:
+                dev->channels[chan_num].ctl_saturation = ctl->value;
+                medusa_set_saturation(dev, ctl->value, chan_num);
+                break;
+        }
+
+        err = 0;
+
+        return err;
+}
+
+static void cx25821_init_controls(struct cx25821_dev *dev, int chan_num)
+{
+        struct v4l2_control ctrl;
+        int i;
+        for (i = 0; i < CX25821_CTLS; i++) {
+                ctrl.id = cx25821_ctls[i].id;
+                ctrl.value = cx25821_ctls[i].default_value;
+
+                cx25821_set_control(dev, &ctrl, chan_num);
+        }
+}
+
+int cx25821_vidioc_cropcap(struct file *file, void *priv,
+                           struct v4l2_cropcap *cropcap)
+{
+        struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
+
+        if (cropcap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+                return -EINVAL;
+        cropcap->bounds.top = cropcap->bounds.left = 0;
+        cropcap->bounds.width = 720;
+        cropcap->bounds.height = dev->tvnorm == V4L2_STD_PAL_BG ? 576 : 480;
+        cropcap->pixelaspect.numerator =
+                dev->tvnorm == V4L2_STD_PAL_BG ? 59 : 10;
+        cropcap->pixelaspect.denominator =
+                dev->tvnorm == V4L2_STD_PAL_BG ? 54 : 11;
+        cropcap->defrect = cropcap->bounds;
+        return 0;
+}
+
+int cx25821_vidioc_s_crop(struct file *file, void *priv, struct v4l2_crop *crop)
+{
+        struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
+        struct cx25821_fh *fh = priv;
+        int err;
+
+        if (fh) {
+                err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
+                                      fh->prio);
+                if (0 != err)
+                        return err;
+        }
+        /* cx25821_vidioc_s_crop not supported */
+        return -EINVAL;
+}
+
+int cx25821_vidioc_g_crop(struct file *file, void *priv, struct v4l2_crop *crop)
+{
+        /* cx25821_vidioc_g_crop not supported */
+        return -EINVAL;
+}
+
+int cx25821_vidioc_querystd(struct file *file, void *priv, v4l2_std_id * norm)
+{
+        /* medusa does not support video standard sensing of current input */
+        *norm = CX25821_NORMS;
+
+        return 0;
+}
+
+int cx25821_is_valid_width(u32 width, v4l2_std_id tvnorm)
+{
+        if (tvnorm == V4L2_STD_PAL_BG) {
+                if (width == 352 || width == 720)
+                        return 1;
+                else
+                        return 0;
+        }
+
+        if (tvnorm == V4L2_STD_NTSC_M) {
+                if (width == 320 || width == 352 || width == 720)
+                        return 1;
+                else
+                        return 0;
+        }
+        return 0;
+}
+
+int cx25821_is_valid_height(u32 height, v4l2_std_id tvnorm)
+{
+        if (tvnorm == V4L2_STD_PAL_BG) {
+                if (height == 576 || height == 288)
+                        return 1;
+                else
+                        return 0;
+        }
+
+        if (tvnorm == V4L2_STD_NTSC_M) {
+                if (height == 480 || height == 240)
+                        return 1;
+                else
+                        return 0;
+        }
+
+        return 0;
+}
+
+static long video_ioctl_upstream9(struct file *file, unsigned int cmd,
+                                 unsigned long arg)
+{
+        struct cx25821_fh *fh = file->private_data;
+        struct cx25821_dev *dev = fh->dev;
+        int command = 0;
+        struct upstream_user_struct *data_from_user;
+
+        data_from_user = (struct upstream_user_struct *)arg;
+
+        if (!data_from_user) {
+                pr_err("%s(): Upstream data is INVALID. Returning\n", __func__);
+                return 0;
+        }
+
+        command = data_from_user->command;
+
+        if (command != UPSTREAM_START_VIDEO && command != UPSTREAM_STOP_VIDEO)
+                return 0;
+
+        dev->input_filename = data_from_user->input_filename;
+        dev->input_audiofilename = data_from_user->input_filename;
+        dev->vid_stdname = data_from_user->vid_stdname;
+        dev->pixel_format = data_from_user->pixel_format;
+        dev->channel_select = data_from_user->channel_select;
+        dev->command = data_from_user->command;
+
+        switch (command) {
+        case UPSTREAM_START_VIDEO:
+                cx25821_start_upstream_video_ch1(dev, data_from_user);
+                break;
+
+        case UPSTREAM_STOP_VIDEO:
+                cx25821_stop_upstream_video_ch1(dev);
+                break;
+        }
+
+        return 0;
+}
+
+static long video_ioctl_upstream10(struct file *file, unsigned int cmd,
+                                  unsigned long arg)
+{
+        struct cx25821_fh *fh = file->private_data;
+        struct cx25821_dev *dev = fh->dev;
+        int command = 0;
+        struct upstream_user_struct *data_from_user;
+
+        data_from_user = (struct upstream_user_struct *)arg;
+
+        if (!data_from_user) {
+                pr_err("%s(): Upstream data is INVALID. Returning\n", __func__);
+                return 0;
+        }
+
+        command = data_from_user->command;
+
+        if (command != UPSTREAM_START_VIDEO && command != UPSTREAM_STOP_VIDEO)
+                return 0;
+
+        dev->input_filename_ch2 = data_from_user->input_filename;
+        dev->input_audiofilename = data_from_user->input_filename;
+        dev->vid_stdname_ch2 = data_from_user->vid_stdname;
+        dev->pixel_format_ch2 = data_from_user->pixel_format;
+        dev->channel_select_ch2 = data_from_user->channel_select;
+        dev->command_ch2 = data_from_user->command;
+
+        switch (command) {
+        case UPSTREAM_START_VIDEO:
+                cx25821_start_upstream_video_ch2(dev, data_from_user);
+                break;
+
+        case UPSTREAM_STOP_VIDEO:
+                cx25821_stop_upstream_video_ch2(dev);
+                break;
+        }
+
+        return 0;
+}
+
+static long video_ioctl_upstream11(struct file *file, unsigned int cmd,
+                                  unsigned long arg)
+{
+        struct cx25821_fh *fh = file->private_data;
+        struct cx25821_dev *dev = fh->dev;
+        int command = 0;
+        struct upstream_user_struct *data_from_user;
+
+        data_from_user = (struct upstream_user_struct *)arg;
+
+        if (!data_from_user) {
+                pr_err("%s(): Upstream data is INVALID. Returning\n", __func__);
+                return 0;
+        }
+
+        command = data_from_user->command;
+
+        if (command != UPSTREAM_START_AUDIO && command != UPSTREAM_STOP_AUDIO)
+                return 0;
+
+        dev->input_filename = data_from_user->input_filename;
+        dev->input_audiofilename = data_from_user->input_filename;
+        dev->vid_stdname = data_from_user->vid_stdname;
+        dev->pixel_format = data_from_user->pixel_format;
+        dev->channel_select = data_from_user->channel_select;
+        dev->command = data_from_user->command;
+
+        switch (command) {
+        case UPSTREAM_START_AUDIO:
+                cx25821_start_upstream_audio(dev, data_from_user);
+                break;
+
+        case UPSTREAM_STOP_AUDIO:
+                cx25821_stop_upstream_audio(dev);
+                break;
+        }
+
+        return 0;
+}
+
+static long video_ioctl_set(struct file *file, unsigned int cmd,
+                           unsigned long arg)
+{
+        struct cx25821_fh *fh = file->private_data;
+        struct cx25821_dev *dev = fh->dev;
+        struct downstream_user_struct *data_from_user;
+        int command;
+        int width = 720;
+        int selected_channel = 0, pix_format = 0, i = 0;
+        int cif_enable = 0, cif_width = 0;
+        u32 value = 0;
+
+        data_from_user = (struct downstream_user_struct *)arg;
+
+        if (!data_from_user) {
+                pr_err("%s(): User data is INVALID. Returning\n", __func__);
+                return 0;
+        }
+
+        command = data_from_user->command;
+
+        if (command != SET_VIDEO_STD && command != SET_PIXEL_FORMAT
+           && command != ENABLE_CIF_RESOLUTION && command != REG_READ
+           && command != REG_WRITE && command != MEDUSA_READ
+           && command != MEDUSA_WRITE) {
+                return 0;
+        }
+
+        switch (command) {
+        case SET_VIDEO_STD:
+                if (!strcmp(data_from_user->vid_stdname, "PAL"))
+                        dev->tvnorm = V4L2_STD_PAL_BG;
+                else
+                        dev->tvnorm = V4L2_STD_NTSC_M;
+                medusa_set_videostandard(dev);
+                break;
+
+        case SET_PIXEL_FORMAT:
+                selected_channel = data_from_user->decoder_select;
+                pix_format = data_from_user->pixel_format;
+
+                if (!(selected_channel <= 7 && selected_channel >= 0)) {
+                        selected_channel -= 4;
+                        selected_channel = selected_channel % 8;
+                }
+
+                if (selected_channel >= 0)
+                        cx25821_set_pixel_format(dev, selected_channel,
+                                                pix_format);
+
+                break;
+
+        case ENABLE_CIF_RESOLUTION:
+                selected_channel = data_from_user->decoder_select;
+                cif_enable = data_from_user->cif_resolution_enable;
+                cif_width = data_from_user->cif_width;
+
+                if (cif_enable) {
+                        if (dev->tvnorm & V4L2_STD_PAL_BG
+                            || dev->tvnorm & V4L2_STD_PAL_DK) {
+                                width = 352;
+                        } else {
+                                width = cif_width;
+                                if (cif_width != 320 && cif_width != 352)
+                                        width = 320;
+                        }
+                }
+
+                if (!(selected_channel <= 7 && selected_channel >= 0)) {
+                        selected_channel -= 4;
+                        selected_channel = selected_channel % 8;
+                }
+
+                if (selected_channel <= 7 && selected_channel >= 0) {
+                        dev->channels[selected_channel].
+                                use_cif_resolution = cif_enable;
+                        dev->channels[selected_channel].cif_width = width;
+                } else {
+                        for (i = 0; i < VID_CHANNEL_NUM; i++) {
+                                dev->channels[i].use_cif_resolution =
+                                        cif_enable;
+                                dev->channels[i].cif_width = width;
+                        }
+                }
+
+                medusa_set_resolution(dev, width, selected_channel);
+                break;
+        case REG_READ:
+                data_from_user->reg_data = cx_read(data_from_user->reg_address);
+                break;
+        case REG_WRITE:
+                cx_write(data_from_user->reg_address, data_from_user->reg_data);
+                break;
+        case MEDUSA_READ:
+                value = cx25821_i2c_read(&dev->i2c_bus[0],
+                                         (u16) data_from_user->reg_address,
+                                         &data_from_user->reg_data);
+                break;
+        case MEDUSA_WRITE:
+                cx25821_i2c_write(&dev->i2c_bus[0],
+                                  (u16) data_from_user->reg_address,
+                                  data_from_user->reg_data);
+                break;
+        }
+
+        return 0;
+}
+
+static long cx25821_video_ioctl(struct file *file,
+                                unsigned int cmd, unsigned long arg)
+{
+        int  ret = 0;
+
+        struct cx25821_fh  *fh  = file->private_data;
+
+        /* check to see if it's the video upstream */
+        if (fh->channel_id == SRAM_CH09) {
+                ret = video_ioctl_upstream9(file, cmd, arg);
+                return ret;
+        } else if (fh->channel_id == SRAM_CH10) {
+                ret = video_ioctl_upstream10(file, cmd, arg);
+                return ret;
+        } else if (fh->channel_id == SRAM_CH11) {
+                ret = video_ioctl_upstream11(file, cmd, arg);
+                ret = video_ioctl_set(file, cmd, arg);
+                return ret;
+        }
+
+        return video_ioctl2(file, cmd, arg);
+}
+
+/* exported stuff */
+static const struct v4l2_file_operations video_fops = {
+        .owner = THIS_MODULE,
+        .open = video_open,
+        .release = video_release,
+        .read = video_read,
+        .poll = video_poll,
+        .mmap = cx25821_video_mmap,
+        .ioctl = cx25821_video_ioctl,
+};
+
+static const struct v4l2_ioctl_ops video_ioctl_ops = {
+        .vidioc_querycap = cx25821_vidioc_querycap,
+        .vidioc_enum_fmt_vid_cap = cx25821_vidioc_enum_fmt_vid_cap,
+        .vidioc_g_fmt_vid_cap = cx25821_vidioc_g_fmt_vid_cap,
+        .vidioc_try_fmt_vid_cap = cx25821_vidioc_try_fmt_vid_cap,
+        .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,
+        .vidioc_reqbufs = cx25821_vidioc_reqbufs,
+        .vidioc_querybuf = cx25821_vidioc_querybuf,
+        .vidioc_qbuf = cx25821_vidioc_qbuf,
+        .vidioc_dqbuf = vidioc_dqbuf,
+#ifdef TUNER_FLAG
+        .vidioc_s_std = cx25821_vidioc_s_std,
+        .vidioc_querystd = cx25821_vidioc_querystd,
+#endif
+        .vidioc_cropcap = cx25821_vidioc_cropcap,
+        .vidioc_s_crop = cx25821_vidioc_s_crop,
+        .vidioc_g_crop = cx25821_vidioc_g_crop,
+        .vidioc_enum_input = cx25821_vidioc_enum_input,
+        .vidioc_g_input = cx25821_vidioc_g_input,
+        .vidioc_s_input = cx25821_vidioc_s_input,
+        .vidioc_g_ctrl = cx25821_vidioc_g_ctrl,
+        .vidioc_s_ctrl = vidioc_s_ctrl,
+        .vidioc_queryctrl = cx25821_vidioc_queryctrl,
+        .vidioc_streamon = vidioc_streamon,
+        .vidioc_streamoff = vidioc_streamoff,
+        .vidioc_log_status = vidioc_log_status,
+        .vidioc_g_priority = cx25821_vidioc_g_priority,
+        .vidioc_s_priority = cx25821_vidioc_s_priority,
+#ifdef TUNER_FLAG
+        .vidioc_g_tuner = cx25821_vidioc_g_tuner,
+        .vidioc_s_tuner = cx25821_vidioc_s_tuner,
+        .vidioc_g_frequency = cx25821_vidioc_g_frequency,
+        .vidioc_s_frequency = cx25821_vidioc_s_frequency,
+#endif
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+        .vidioc_g_register = cx25821_vidioc_g_register,
+        .vidioc_s_register = cx25821_vidioc_s_register,
+#endif
+};
+
+struct video_device cx25821_videoioctl_template = {
+        .name = "cx25821-videoioctl",
+        .fops = &video_fops,
+        .ioctl_ops = &video_ioctl_ops,
+        .tvnorms = CX25821_NORMS,
+        .current_norm = V4L2_STD_NTSC_M,
+};
diff --git a/drivers/media/video/cx25821/cx25821-video.h b/drivers/media/video/cx25821/cx25821-video.h
new file mode 100644
index 000000000000..d0d9538ca5b3
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821-video.h
@@ -0,0 +1,188 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <shu.lin@conexant.com>, <hiep.huynh@conexant.com>
+ *  Based on Steven Toth <stoth@linuxtv.org> cx23885 driver
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef CX25821_VIDEO_H_
+#define CX25821_VIDEO_H_
+
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kmod.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/kthread.h>
+#include <asm/div64.h>
+
+#include "cx25821.h"
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+
+#define TUNER_FLAG
+
+#define VIDEO_DEBUG 0
+
+#define dprintk(level, fmt, arg...)                                     \
+do {                                                                    \
+        if (VIDEO_DEBUG >= level)                                       \
+                printk(KERN_DEBUG "%s/0: " fmt, dev->name, ##arg);      \
+} while (0)
+
+/* For IOCTL to identify running upstream */
+#define UPSTREAM_START_VIDEO        700
+#define UPSTREAM_STOP_VIDEO         701
+#define UPSTREAM_START_AUDIO        702
+#define UPSTREAM_STOP_AUDIO         703
+#define UPSTREAM_DUMP_REGISTERS     702
+#define SET_VIDEO_STD               800
+#define SET_PIXEL_FORMAT            1000
+#define ENABLE_CIF_RESOLUTION       1001
+
+#define REG_READ                    900
+#define REG_WRITE                   901
+#define MEDUSA_READ                 910
+#define MEDUSA_WRITE                911
+
+extern struct sram_channel *channel0;
+extern struct sram_channel *channel1;
+extern struct sram_channel *channel2;
+extern struct sram_channel *channel3;
+extern struct sram_channel *channel4;
+extern struct sram_channel *channel5;
+extern struct sram_channel *channel6;
+extern struct sram_channel *channel7;
+extern struct sram_channel *channel9;
+extern struct sram_channel *channel10;
+extern struct sram_channel *channel11;
+extern struct video_device cx25821_videoioctl_template;
+/* extern const u32 *ctrl_classes[]; */
+
+extern unsigned int vid_limit;
+
+#define FORMAT_FLAGS_PACKED       0x01
+extern struct cx25821_fmt formats[];
+extern struct cx25821_fmt *cx25821_format_by_fourcc(unsigned int fourcc);
+extern struct cx25821_data timeout_data[MAX_VID_CHANNEL_NUM];
+
+extern void cx25821_dump_video_queue(struct cx25821_dev *dev,
+                                     struct cx25821_dmaqueue *q);
+extern void cx25821_video_wakeup(struct cx25821_dev *dev,
+                                 struct cx25821_dmaqueue *q, u32 count);
+
+#ifdef TUNER_FLAG
+extern int cx25821_set_tvnorm(struct cx25821_dev *dev, v4l2_std_id norm);
+#endif
+
+extern int cx25821_res_get(struct cx25821_dev *dev, struct cx25821_fh *fh,
+                           unsigned int bit);
+extern int cx25821_res_check(struct cx25821_fh *fh, unsigned int bit);
+extern int cx25821_res_locked(struct cx25821_fh *fh, unsigned int bit);
+extern void cx25821_res_free(struct cx25821_dev *dev, struct cx25821_fh *fh,
+                             unsigned int bits);
+extern int cx25821_video_mux(struct cx25821_dev *dev, unsigned int input);
+extern int cx25821_start_video_dma(struct cx25821_dev *dev,
+                                   struct cx25821_dmaqueue *q,
+                                   struct cx25821_buffer *buf,
+                                   struct sram_channel *channel);
+
+extern int cx25821_set_scale(struct cx25821_dev *dev, unsigned int width,
+                             unsigned int height, enum v4l2_field field);
+extern int cx25821_video_irq(struct cx25821_dev *dev, int chan_num, u32 status);
+extern void cx25821_video_unregister(struct cx25821_dev *dev, int chan_num);
+extern int cx25821_video_register(struct cx25821_dev *dev);
+extern int cx25821_get_format_size(void);
+
+extern int cx25821_buffer_setup(struct videobuf_queue *q, unsigned int *count,
+                                unsigned int *size);
+extern int cx25821_buffer_prepare(struct videobuf_queue *q,
+                                  struct videobuf_buffer *vb,
+                                  enum v4l2_field field);
+extern void cx25821_buffer_release(struct videobuf_queue *q,
+                                   struct videobuf_buffer *vb);
+extern struct videobuf_queue *get_queue(struct cx25821_fh *fh);
+extern int cx25821_get_resource(struct cx25821_fh *fh, int resource);
+extern int cx25821_video_mmap(struct file *file, struct vm_area_struct *vma);
+extern int cx25821_vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+                                          struct v4l2_format *f);
+extern int cx25821_vidioc_querycap(struct file *file, void *priv,
+                                   struct v4l2_capability *cap);
+extern int cx25821_vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
+                                           struct v4l2_fmtdesc *f);
+extern int cx25821_vidioc_reqbufs(struct file *file, void *priv,
+                                  struct v4l2_requestbuffers *p);
+extern int cx25821_vidioc_querybuf(struct file *file, void *priv,
+                                   struct v4l2_buffer *p);
+extern int cx25821_vidioc_qbuf(struct file *file, void *priv,
+                               struct v4l2_buffer *p);
+extern int cx25821_vidioc_s_std(struct file *file, void *priv,
+                                v4l2_std_id *tvnorms);
+extern int cx25821_enum_input(struct cx25821_dev *dev, struct v4l2_input *i);
+extern int cx25821_vidioc_enum_input(struct file *file, void *priv,
+                                     struct v4l2_input *i);
+extern int cx25821_vidioc_g_input(struct file *file, void *priv,
+                                  unsigned int *i);
+extern int cx25821_vidioc_s_input(struct file *file, void *priv,
+                                  unsigned int i);
+extern int cx25821_vidioc_g_ctrl(struct file *file, void *priv,
+                                 struct v4l2_control *ctl);
+extern int cx25821_vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+                                        struct v4l2_format *f);
+extern int cx25821_vidioc_g_frequency(struct file *file, void *priv,
+                                      struct v4l2_frequency *f);
+extern int cx25821_set_freq(struct cx25821_dev *dev, struct v4l2_frequency *f);
+extern int cx25821_vidioc_s_frequency(struct file *file, void *priv,
+                                      struct v4l2_frequency *f);
+extern int cx25821_vidioc_g_register(struct file *file, void *fh,
+                                     struct v4l2_dbg_register *reg);
+extern int cx25821_vidioc_s_register(struct file *file, void *fh,
+                                     struct v4l2_dbg_register *reg);
+extern int cx25821_vidioc_g_tuner(struct file *file, void *priv,
+                                  struct v4l2_tuner *t);
+extern int cx25821_vidioc_s_tuner(struct file *file, void *priv,
+                                  struct v4l2_tuner *t);
+
+extern int cx25821_is_valid_width(u32 width, v4l2_std_id tvnorm);
+extern int cx25821_is_valid_height(u32 height, v4l2_std_id tvnorm);
+
+extern int cx25821_vidioc_g_priority(struct file *file, void *f,
+                                     enum v4l2_priority *p);
+extern int cx25821_vidioc_s_priority(struct file *file, void *f,
+                                     enum v4l2_priority prio);
+
+extern int cx25821_vidioc_queryctrl(struct file *file, void *priv,
+                                    struct v4l2_queryctrl *qctrl);
+extern int cx25821_set_control(struct cx25821_dev *dev,
+                               struct v4l2_control *ctrl, int chan_num);
+
+extern int cx25821_vidioc_cropcap(struct file *file, void *fh,
+                                  struct v4l2_cropcap *cropcap);
+extern int cx25821_vidioc_s_crop(struct file *file, void *priv,
+                                 struct v4l2_crop *crop);
+extern int cx25821_vidioc_g_crop(struct file *file, void *priv,
+                                 struct v4l2_crop *crop);
+
+extern int cx25821_vidioc_querystd(struct file *file, void *priv,
+                                   v4l2_std_id *norm);
+#endif
diff --git a/drivers/media/video/cx25821/cx25821.h b/drivers/media/video/cx25821/cx25821.h
new file mode 100644
index 000000000000..2d2d00932823
--- /dev/null
+++ b/drivers/media/video/cx25821/cx25821.h
@@ -0,0 +1,617 @@
+/*
+ *  Driver for the Conexant CX25821 PCIe bridge
+ *
+ *  Copyright (C) 2009 Conexant Systems Inc.
+ *  Authors  <shu.lin@conexant.com>, <hiep.huynh@conexant.com>
+ *  Based on Steven Toth <stoth@linuxtv.org> cx23885 driver
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef CX25821_H_
+#define CX25821_H_
+
+#include <linux/pci.h>
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/sched.h>
+#include <linux/kdev_t.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/tuner.h>
+#include <media/tveeprom.h>
+#include <media/videobuf-dma-sg.h>
+#include <media/videobuf-dvb.h>
+
+#include "btcx-risc.h"
+#include "cx25821-reg.h"
+#include "cx25821-medusa-reg.h"
+#include "cx25821-sram.h"
+#include "cx25821-audio.h"
+#include "media/cx2341x.h"
+
+#include <linux/version.h>
+#include <linux/mutex.h>
+
+#define CX25821_VERSION_CODE KERNEL_VERSION(0, 0, 106)
+
+#define UNSET (-1U)
+#define NO_SYNC_LINE (-1U)
+
+#define CX25821_MAXBOARDS 2
+
+#define TRUE    1
+#define FALSE   0
+#define LINE_SIZE_D1    1440
+
+/* Number of decoders and encoders */
+#define MAX_DECODERS            8
+#define MAX_ENCODERS            2
+#define QUAD_DECODERS           4
+#define MAX_CAMERAS             16
+
+/* Max number of inputs by card */
+#define MAX_CX25821_INPUT 8
+#define INPUT(nr) (&cx25821_boards[dev->board].input[nr])
+#define RESOURCE_VIDEO0       1
+#define RESOURCE_VIDEO1       2
+#define RESOURCE_VIDEO2       4
+#define RESOURCE_VIDEO3       8
+#define RESOURCE_VIDEO4       16
+#define RESOURCE_VIDEO5       32
+#define RESOURCE_VIDEO6       64
+#define RESOURCE_VIDEO7       128
+#define RESOURCE_VIDEO8       256
+#define RESOURCE_VIDEO9       512
+#define RESOURCE_VIDEO10      1024
+#define RESOURCE_VIDEO11      2048
+#define RESOURCE_VIDEO_IOCTL  4096
+
+#define BUFFER_TIMEOUT     (HZ) /* 0.5 seconds */
+
+#define UNKNOWN_BOARD       0
+#define CX25821_BOARD        1
+
+/* Currently supported by the driver */
+#define CX25821_NORMS (\
+        V4L2_STD_NTSC_M |  V4L2_STD_NTSC_M_JP | V4L2_STD_NTSC_M_KR | \
+        V4L2_STD_PAL_BG |  V4L2_STD_PAL_DK    |  V4L2_STD_PAL_I    | \
+        V4L2_STD_PAL_M  |  V4L2_STD_PAL_N     |  V4L2_STD_PAL_H    | \
+        V4L2_STD_PAL_Nc)
+
+#define CX25821_BOARD_CONEXANT_ATHENA10 1
+#define MAX_VID_CHANNEL_NUM     12
+#define VID_CHANNEL_NUM 8
+#define CX25821_NR_INPUT 2
+
+struct cx25821_fmt {
+        char *name;
+        u32 fourcc;             /* v4l2 format id */
+        int depth;
+        int flags;
+        u32 cxformat;
+};
+
+struct cx25821_ctrl {
+        struct v4l2_queryctrl v;
+        u32 off;
+        u32 reg;
+        u32 mask;
+        u32 shift;
+};
+
+struct cx25821_tvnorm {
+        char *name;
+        v4l2_std_id id;
+        u32 cxiformat;
+        u32 cxoformat;
+};
+
+struct cx25821_fh {
+        struct cx25821_dev *dev;
+        enum v4l2_buf_type type;
+        int radio;
+        u32 resources;
+
+        enum v4l2_priority prio;
+
+        /* video overlay */
+        struct v4l2_window win;
+        struct v4l2_clip *clips;
+        unsigned int nclips;
+
+        /* video capture */
+        struct cx25821_fmt *fmt;
+        unsigned int width, height;
+        int channel_id;
+
+        /* vbi capture */
+        struct videobuf_queue vidq;
+        struct videobuf_queue vbiq;
+
+        /* H264 Encoder specifics ONLY */
+        struct videobuf_queue mpegq;
+        atomic_t v4l_reading;
+};
+
+enum cx25821_itype {
+        CX25821_VMUX_COMPOSITE = 1,
+        CX25821_VMUX_SVIDEO,
+        CX25821_VMUX_DEBUG,
+        CX25821_RADIO,
+};
+
+enum cx25821_src_sel_type {
+        CX25821_SRC_SEL_EXT_656_VIDEO = 0,
+        CX25821_SRC_SEL_PARALLEL_MPEG_VIDEO
+};
+
+/* buffer for one video frame */
+struct cx25821_buffer {
+        /* common v4l buffer stuff -- must be first */
+        struct videobuf_buffer vb;
+
+        /* cx25821 specific */
+        unsigned int bpl;
+        struct btcx_riscmem risc;
+        struct cx25821_fmt *fmt;
+        u32 count;
+};
+
+struct cx25821_input {
+        enum cx25821_itype type;
+        unsigned int vmux;
+        u32 gpio0, gpio1, gpio2, gpio3;
+};
+
+enum port {
+        CX25821_UNDEFINED = 0,
+        CX25821_RAW,
+        CX25821_264
+};
+
+struct cx25821_board {
+        char *name;
+        enum port porta;
+        enum port portb;
+        enum port portc;
+        unsigned int tuner_type;
+        unsigned int radio_type;
+        unsigned char tuner_addr;
+        unsigned char radio_addr;
+
+        u32 clk_freq;
+        struct cx25821_input input[CX25821_NR_INPUT];
+};
+
+struct cx25821_subid {
+        u16 subvendor;
+        u16 subdevice;
+        u32 card;
+};
+
+struct cx25821_i2c {
+        struct cx25821_dev *dev;
+
+        int nr;
+
+        /* i2c i/o */
+        struct i2c_adapter i2c_adap;
+        struct i2c_algo_bit_data i2c_algo;
+        struct i2c_client i2c_client;
+        u32 i2c_rc;
+
+        /* cx25821 registers used for raw addess */
+        u32 i2c_period;
+        u32 reg_ctrl;
+        u32 reg_stat;
+        u32 reg_addr;
+        u32 reg_rdata;
+        u32 reg_wdata;
+};
+
+struct cx25821_dmaqueue {
+        struct list_head active;
+        struct list_head queued;
+        struct timer_list timeout;
+        struct btcx_riscmem stopper;
+        u32 count;
+};
+
+struct cx25821_data {
+        struct cx25821_dev *dev;
+        struct sram_channel *channel;
+};
+
+struct cx25821_channel {
+        struct v4l2_prio_state prio;
+
+        int ctl_bright;
+        int ctl_contrast;
+        int ctl_hue;
+        int ctl_saturation;
+        struct cx25821_data timeout_data;
+
+        struct video_device *video_dev;
+        struct cx25821_dmaqueue vidq;
+
+        struct sram_channel *sram_channels;
+
+        struct mutex lock;
+        int resources;
+
+        int pixel_formats;
+        int use_cif_resolution;
+        int cif_width;
+};
+
+struct cx25821_dev {
+        struct list_head devlist;
+        atomic_t refcount;
+        struct v4l2_device v4l2_dev;
+
+        /* pci stuff */
+        struct pci_dev *pci;
+        unsigned char pci_rev, pci_lat;
+        int pci_bus, pci_slot;
+        u32 base_io_addr;
+        u32 __iomem *lmmio;
+        u8 __iomem *bmmio;
+        int pci_irqmask;
+        int hwrevision;
+
+        u32 clk_freq;
+
+        /* I2C adapters: Master 1 & 2 (External) & Master 3 (Internal only) */
+        struct cx25821_i2c i2c_bus[3];
+
+        int nr;
+        struct mutex lock;
+
+        struct cx25821_channel channels[MAX_VID_CHANNEL_NUM];
+
+        /* board details */
+        unsigned int board;
+        char name[32];
+
+        /* Analog video */
+        u32 resources;
+        unsigned int input;
+        u32 tvaudio;
+        v4l2_std_id tvnorm;
+        unsigned int tuner_type;
+        unsigned char tuner_addr;
+        unsigned int radio_type;
+        unsigned char radio_addr;
+        unsigned int has_radio;
+        unsigned int videc_type;
+        unsigned char videc_addr;
+        unsigned short _max_num_decoders;
+
+        /* Analog Audio Upstream */
+        int _audio_is_running;
+        int _audiopixel_format;
+        int _is_first_audio_frame;
+        int _audiofile_status;
+        int _audio_lines_count;
+        int _audioframe_count;
+        int _audio_upstream_channel;
+        int _last_index_irq;    /* The last interrupt index processed. */
+
+        __le32 *_risc_audio_jmp_addr;
+        __le32 *_risc_virt_start_addr;
+        __le32 *_risc_virt_addr;
+        dma_addr_t _risc_phys_addr;
+        dma_addr_t _risc_phys_start_addr;
+
+        unsigned int _audiorisc_size;
+        unsigned int _audiodata_buf_size;
+        __le32 *_audiodata_buf_virt_addr;
+        dma_addr_t _audiodata_buf_phys_addr;
+        char *_audiofilename;
+
+        /* V4l */
+        u32 freq;
+        struct video_device *vbi_dev;
+        struct video_device *radio_dev;
+        struct video_device *ioctl_dev;
+
+        spinlock_t slock;
+
+        /* Video Upstream */
+        int _line_size;
+        int _prog_cnt;
+        int _pixel_format;
+        int _is_first_frame;
+        int _is_running;
+        int _file_status;
+        int _lines_count;
+        int _frame_count;
+        int _channel_upstream_select;
+        unsigned int _risc_size;
+
+        __le32 *_dma_virt_start_addr;
+        __le32 *_dma_virt_addr;
+        dma_addr_t _dma_phys_addr;
+        dma_addr_t _dma_phys_start_addr;
+
+        unsigned int _data_buf_size;
+        __le32 *_data_buf_virt_addr;
+        dma_addr_t _data_buf_phys_addr;
+        char *_filename;
+        char *_defaultname;
+
+        int _line_size_ch2;
+        int _prog_cnt_ch2;
+        int _pixel_format_ch2;
+        int _is_first_frame_ch2;
+        int _is_running_ch2;
+        int _file_status_ch2;
+        int _lines_count_ch2;
+        int _frame_count_ch2;
+        int _channel2_upstream_select;
+        unsigned int _risc_size_ch2;
+
+        __le32 *_dma_virt_start_addr_ch2;
+        __le32 *_dma_virt_addr_ch2;
+        dma_addr_t _dma_phys_addr_ch2;
+        dma_addr_t _dma_phys_start_addr_ch2;
+
+        unsigned int _data_buf_size_ch2;
+        __le32 *_data_buf_virt_addr_ch2;
+        dma_addr_t _data_buf_phys_addr_ch2;
+        char *_filename_ch2;
+        char *_defaultname_ch2;
+
+        /* MPEG Encoder ONLY settings */
+        u32 cx23417_mailbox;
+        struct cx2341x_mpeg_params mpeg_params;
+        struct video_device *v4l_device;
+        atomic_t v4l_reader_count;
+        struct cx25821_tvnorm encodernorm;
+
+        u32 upstream_riscbuf_size;
+        u32 upstream_databuf_size;
+        u32 upstream_riscbuf_size_ch2;
+        u32 upstream_databuf_size_ch2;
+        u32 audio_upstream_riscbuf_size;
+        u32 audio_upstream_databuf_size;
+        int _isNTSC;
+        int _frame_index;
+        int _audioframe_index;
+        struct workqueue_struct *_irq_queues;
+        struct work_struct _irq_work_entry;
+        struct workqueue_struct *_irq_queues_ch2;
+        struct work_struct _irq_work_entry_ch2;
+        struct workqueue_struct *_irq_audio_queues;
+        struct work_struct _audio_work_entry;
+        char *input_filename;
+        char *input_filename_ch2;
+        int _frame_index_ch2;
+        int _isNTSC_ch2;
+        char *vid_stdname_ch2;
+        int pixel_format_ch2;
+        int channel_select_ch2;
+        int command_ch2;
+        char *input_audiofilename;
+        char *vid_stdname;
+        int pixel_format;
+        int channel_select;
+        int command;
+        int channel_opened;
+};
+
+struct upstream_user_struct {
+        char *input_filename;
+        char *vid_stdname;
+        int pixel_format;
+        int channel_select;
+        int command;
+};
+
+struct downstream_user_struct {
+        char *vid_stdname;
+        int pixel_format;
+        int cif_resolution_enable;
+        int cif_width;
+        int decoder_select;
+        int command;
+        int reg_address;
+        int reg_data;
+};
+
+extern struct upstream_user_struct *up_data;
+
+static inline struct cx25821_dev *get_cx25821(struct v4l2_device *v4l2_dev)
+{
+        return container_of(v4l2_dev, struct cx25821_dev, v4l2_dev);
+}
+
+#define cx25821_call_all(dev, o, f, args...) \
+        v4l2_device_call_all(&dev->v4l2_dev, 0, o, f, ##args)
+
+extern struct list_head cx25821_devlist;
+extern struct mutex cx25821_devlist_mutex;
+
+extern struct cx25821_board cx25821_boards[];
+extern struct cx25821_subid cx25821_subids[];
+
+#define SRAM_CH00  0            /* Video A */
+#define SRAM_CH01  1            /* Video B */
+#define SRAM_CH02  2            /* Video C */
+#define SRAM_CH03  3            /* Video D */
+#define SRAM_CH04  4            /* Video E */
+#define SRAM_CH05  5            /* Video F */
+#define SRAM_CH06  6            /* Video G */
+#define SRAM_CH07  7            /* Video H */
+
+#define SRAM_CH08  8            /* Audio A */
+#define SRAM_CH09  9            /* Video Upstream I */
+#define SRAM_CH10  10           /* Video Upstream J */
+#define SRAM_CH11  11           /* Audio Upstream AUD_CHANNEL_B */
+
+#define VID_UPSTREAM_SRAM_CHANNEL_I     SRAM_CH09
+#define VID_UPSTREAM_SRAM_CHANNEL_J     SRAM_CH10
+#define AUDIO_UPSTREAM_SRAM_CHANNEL_B   SRAM_CH11
+#define VIDEO_IOCTL_CH  11
+
+struct sram_channel {
+        char *name;
+        u32 i;
+        u32 cmds_start;
+        u32 ctrl_start;
+        u32 cdt;
+        u32 fifo_start;
+        u32 fifo_size;
+        u32 ptr1_reg;
+        u32 ptr2_reg;
+        u32 cnt1_reg;
+        u32 cnt2_reg;
+        u32 int_msk;
+        u32 int_stat;
+        u32 int_mstat;
+        u32 dma_ctl;
+        u32 gpcnt_ctl;
+        u32 gpcnt;
+        u32 aud_length;
+        u32 aud_cfg;
+        u32 fld_aud_fifo_en;
+        u32 fld_aud_risc_en;
+
+        /* For Upstream Video */
+        u32 vid_fmt_ctl;
+        u32 vid_active_ctl1;
+        u32 vid_active_ctl2;
+        u32 vid_cdt_size;
+
+        u32 vip_ctl;
+        u32 pix_frmt;
+        u32 jumponly;
+        u32 irq_bit;
+};
+extern struct sram_channel cx25821_sram_channels[];
+
+#define STATUS_SUCCESS         0
+#define STATUS_UNSUCCESSFUL    -1
+
+#define cx_read(reg)             readl(dev->lmmio + ((reg)>>2))
+#define cx_write(reg, value)     writel((value), dev->lmmio + ((reg)>>2))
+
+#define cx_andor(reg, mask, value) \
+        writel((readl(dev->lmmio+((reg)>>2)) & ~(mask)) |\
+        ((value) & (mask)), dev->lmmio+((reg)>>2))
+
+#define cx_set(reg, bit)          cx_andor((reg), (bit), (bit))
+#define cx_clear(reg, bit)        cx_andor((reg), (bit), 0)
+
+#define Set_GPIO_Bit(Bit)                       (1 << Bit)
+#define Clear_GPIO_Bit(Bit)                     (~(1 << Bit))
+
+#define CX25821_ERR(fmt, args...)                       \
+        pr_err("(%d): " fmt, dev->board, ##args)
+#define CX25821_WARN(fmt, args...)                      \
+        pr_warn("(%d): " fmt, dev->board, ##args)
+#define CX25821_INFO(fmt, args...)                      \
+        pr_info("(%d): " fmt, dev->board, ##args)
+
+extern int cx25821_i2c_register(struct cx25821_i2c *bus);
+extern void cx25821_card_setup(struct cx25821_dev *dev);
+extern int cx25821_ir_init(struct cx25821_dev *dev);
+extern int cx25821_i2c_read(struct cx25821_i2c *bus, u16 reg_addr, int *value);
+extern int cx25821_i2c_write(struct cx25821_i2c *bus, u16 reg_addr, int value);
+extern int cx25821_i2c_unregister(struct cx25821_i2c *bus);
+extern void cx25821_gpio_init(struct cx25821_dev *dev);
+extern void cx25821_set_gpiopin_direction(struct cx25821_dev *dev,
+                                          int pin_number, int pin_logic_value);
+
+extern int medusa_video_init(struct cx25821_dev *dev);
+extern int medusa_set_videostandard(struct cx25821_dev *dev);
+extern void medusa_set_resolution(struct cx25821_dev *dev, int width,
+                                  int decoder_select);
+extern int medusa_set_brightness(struct cx25821_dev *dev, int brightness,
+                                 int decoder);
+extern int medusa_set_contrast(struct cx25821_dev *dev, int contrast,
+                               int decoder);
+extern int medusa_set_hue(struct cx25821_dev *dev, int hue, int decoder);
+extern int medusa_set_saturation(struct cx25821_dev *dev, int saturation,
+                                 int decoder);
+
+extern int cx25821_sram_channel_setup(struct cx25821_dev *dev,
+                                      struct sram_channel *ch, unsigned int bpl,
+                                      u32 risc);
+
+extern int cx25821_risc_buffer(struct pci_dev *pci, struct btcx_riscmem *risc,
+                               struct scatterlist *sglist,
+                               unsigned int top_offset,
+                               unsigned int bottom_offset,
+                               unsigned int bpl,
+                               unsigned int padding, unsigned int lines);
+extern int cx25821_risc_databuffer_audio(struct pci_dev *pci,
+                                         struct btcx_riscmem *risc,
+                                         struct scatterlist *sglist,
+                                         unsigned int bpl,
+                                         unsigned int lines, unsigned int lpi);
+extern void cx25821_free_buffer(struct videobuf_queue *q,
+                                struct cx25821_buffer *buf);
+extern int cx25821_risc_stopper(struct pci_dev *pci, struct btcx_riscmem *risc,
+                                u32 reg, u32 mask, u32 value);
+extern void cx25821_sram_channel_dump(struct cx25821_dev *dev,
+                                      struct sram_channel *ch);
+extern void cx25821_sram_channel_dump_audio(struct cx25821_dev *dev,
+                                            struct sram_channel *ch);
+
+extern struct cx25821_dev *cx25821_dev_get(struct pci_dev *pci);
+extern void cx25821_print_irqbits(char *name, char *tag, char **strings,
+                                  int len, u32 bits, u32 mask);
+extern void cx25821_dev_unregister(struct cx25821_dev *dev);
+extern int cx25821_sram_channel_setup_audio(struct cx25821_dev *dev,
+                                            struct sram_channel *ch,
+                                            unsigned int bpl, u32 risc);
+
+extern int cx25821_vidupstream_init_ch1(struct cx25821_dev *dev,
+                                        int channel_select, int pixel_format);
+extern int cx25821_vidupstream_init_ch2(struct cx25821_dev *dev,
+                                        int channel_select, int pixel_format);
+extern int cx25821_audio_upstream_init(struct cx25821_dev *dev,
+                                       int channel_select);
+extern void cx25821_free_mem_upstream_ch1(struct cx25821_dev *dev);
+extern void cx25821_free_mem_upstream_ch2(struct cx25821_dev *dev);
+extern void cx25821_free_mem_upstream_audio(struct cx25821_dev *dev);
+extern void cx25821_start_upstream_video_ch1(struct cx25821_dev *dev,
+                                             struct upstream_user_struct
+                                             *up_data);
+extern void cx25821_start_upstream_video_ch2(struct cx25821_dev *dev,
+                                             struct upstream_user_struct
+                                             *up_data);
+extern void cx25821_start_upstream_audio(struct cx25821_dev *dev,
+                                         struct upstream_user_struct *up_data);
+extern void cx25821_stop_upstream_video_ch1(struct cx25821_dev *dev);
+extern void cx25821_stop_upstream_video_ch2(struct cx25821_dev *dev);
+extern void cx25821_stop_upstream_audio(struct cx25821_dev *dev);
+extern int cx25821_sram_channel_setup_upstream(struct cx25821_dev *dev,
+                                               struct sram_channel *ch,
+                                               unsigned int bpl, u32 risc);
+extern void cx25821_set_pixel_format(struct cx25821_dev *dev, int channel,
+                                     u32 format);
+extern void cx25821_videoioctl_unregister(struct cx25821_dev *dev);
+extern struct video_device *cx25821_vdev_init(struct cx25821_dev *dev,
+                                              struct pci_dev *pci,
+                                              struct video_device *template,
+                                              char *type);
+#endif
diff --git a/drivers/media/video/em28xx/em28xx-cards.c b/drivers/media/video/em28xx/em28xx-cards.c
index 4240f0b720fa..9b747c266afa 100644
--- a/drivers/media/video/em28xx/em28xx-cards.c
+++ b/drivers/media/video/em28xx/em28xx-cards.c
@@ -1923,6 +1923,8 @@ struct usb_device_id em28xx_id_table[] = {
                         .driver_info = EM2860_BOARD_TERRATEC_AV350 },
         { USB_DEVICE(0x0ccd, 0x0096),
                         .driver_info = EM2860_BOARD_TERRATEC_GRABBY },
+        { USB_DEVICE(0x0ccd, 0x10AF),
+                        .driver_info = EM2860_BOARD_TERRATEC_GRABBY },
         { USB_DEVICE(0x0fd9, 0x0033),
                         .driver_info = EM2860_BOARD_ELGATO_VIDEO_CAPTURE},
         { USB_DEVICE(0x185b, 0x2870),
diff --git a/drivers/media/video/imx074.c b/drivers/media/video/imx074.c
index 0382ea752e6f..8775e262bb6e 100644
--- a/drivers/media/video/imx074.c
+++ b/drivers/media/video/imx074.c
@@ -12,11 +12,11 @@
 
 #include <linux/delay.h>
 #include <linux/i2c.h>
+#include <linux/v4l2-mediabus.h>
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 
 #include <media/soc_camera.h>
-#include <media/soc_mediabus.h>
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-chip-ident.h>
 
@@ -267,6 +267,17 @@ static int imx074_g_chip_ident(struct v4l2_subdev *sd,
         return 0;
 }
 
+static int imx074_g_mbus_config(struct v4l2_subdev *sd,
+                                struct v4l2_mbus_config *cfg)
+{
+        cfg->type = V4L2_MBUS_CSI2;
+        cfg->flags = V4L2_MBUS_CSI2_2_LANE |
+                V4L2_MBUS_CSI2_CHANNEL_0 |
+                V4L2_MBUS_CSI2_CONTINUOUS_CLOCK;
+
+        return 0;
+}
+
 static struct v4l2_subdev_video_ops imx074_subdev_video_ops = {
         .s_stream       = imx074_s_stream,
         .s_mbus_fmt     = imx074_s_fmt,
@@ -275,6 +286,7 @@ static struct v4l2_subdev_video_ops imx074_subdev_video_ops = {
         .enum_mbus_fmt  = imx074_enum_fmt,
         .g_crop         = imx074_g_crop,
         .cropcap        = imx074_cropcap,
+        .g_mbus_config  = imx074_g_mbus_config,
 };
 
 static struct v4l2_subdev_core_ops imx074_subdev_core_ops = {
@@ -286,28 +298,7 @@ static struct v4l2_subdev_ops imx074_subdev_ops = {
         .video  = &imx074_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 imx074_query_bus_param(struct soc_camera_device *icd)
-{
-        return 0;
-}
-
-static int imx074_set_bus_param(struct soc_camera_device *icd,
-                                 unsigned long flags)
-{
-        return -EINVAL;
-}
-
-static struct soc_camera_ops imx074_ops = {
-        .query_bus_param        = imx074_query_bus_param,
-        .set_bus_param          = imx074_set_bus_param,
-};
-
-static int imx074_video_probe(struct soc_camera_device *icd,
-                              struct i2c_client *client)
+static int imx074_video_probe(struct i2c_client *client)
 {
         int ret;
         u16 id;
@@ -417,17 +408,10 @@ static int imx074_probe(struct i2c_client *client,
                         const struct i2c_device_id *did)
 {
         struct imx074 *priv;
-        struct soc_camera_device *icd = client->dev.platform_data;
         struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
-        struct soc_camera_link *icl;
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
         int ret;
 
-        if (!icd) {
-                dev_err(&client->dev, "IMX074: missing soc-camera data!\n");
-                return -EINVAL;
-        }
-
-        icl = to_soc_camera_link(icd);
         if (!icl) {
                 dev_err(&client->dev, "IMX074: missing platform data!\n");
                 return -EINVAL;
@@ -445,12 +429,10 @@ static int imx074_probe(struct i2c_client *client,
 
         v4l2_i2c_subdev_init(&priv->subdev, client, &imx074_subdev_ops);
 
-        icd->ops        = &imx074_ops;
         priv->fmt       = &imx074_colour_fmts[0];
 
-        ret = imx074_video_probe(icd, client);
+        ret = imx074_video_probe(client);
         if (ret < 0) {
-                icd->ops = NULL;
                 kfree(priv);
                 return ret;
         }
@@ -461,10 +443,8 @@ static int imx074_probe(struct i2c_client *client,
 static int imx074_remove(struct i2c_client *client)
 {
         struct imx074 *priv = to_imx074(client);
-        struct soc_camera_device *icd = client->dev.platform_data;
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
 
-        icd->ops = NULL;
         if (icl->free_bus)
                 icl->free_bus(icl);
         kfree(priv);
diff --git a/drivers/media/video/ivtv/ivtv-driver.c b/drivers/media/video/ivtv/ivtv-driver.c
index 0fb75524484d..41108a9a195e 100644
--- a/drivers/media/video/ivtv/ivtv-driver.c
+++ b/drivers/media/video/ivtv/ivtv-driver.c
@@ -1180,6 +1180,8 @@ static int __devinit ivtv_probe(struct pci_dev *pdev,
                 setup.addr = ADDR_UNSET;
                 setup.type = itv->options.tuner;
                 setup.mode_mask = T_ANALOG_TV;  /* matches TV tuners */
+                if (itv->options.radio > 0)
+                        setup.mode_mask |= T_RADIO;
                 setup.tuner_callback = (setup.type == TUNER_XC2028) ?
                         ivtv_reset_tuner_gpio : NULL;
                 ivtv_call_all(itv, tuner, s_type_addr, &setup);
diff --git a/drivers/media/video/marvell-ccic/mcam-core.c b/drivers/media/video/marvell-ccic/mcam-core.c
index 1141b976dff4..80ec64d2d6d8 100644
--- a/drivers/media/video/marvell-ccic/mcam-core.c
+++ b/drivers/media/video/marvell-ccic/mcam-core.c
@@ -883,7 +883,8 @@ static int mcam_read_setup(struct mcam_camera *cam)
  * Videobuf2 interface code.
  */
 
-static int mcam_vb_queue_setup(struct vb2_queue *vq, unsigned int *nbufs,
+static int mcam_vb_queue_setup(struct vb2_queue *vq,
+                const struct v4l2_format *fmt, unsigned int *nbufs,
                 unsigned int *num_planes, unsigned int sizes[],
                 void *alloc_ctxs[])
 {
diff --git a/drivers/media/video/mem2mem_testdev.c b/drivers/media/video/mem2mem_testdev.c
index 9594b52f8605..12897e8a3314 100644
--- a/drivers/media/video/mem2mem_testdev.c
+++ b/drivers/media/video/mem2mem_testdev.c
@@ -738,9 +738,10 @@ static const struct v4l2_ioctl_ops m2mtest_ioctl_ops = {
  * Queue operations
  */
 
-static int m2mtest_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
-                                unsigned int *nplanes, unsigned int sizes[],
-                                void *alloc_ctxs[])
+static int m2mtest_queue_setup(struct vb2_queue *vq,
+                                const struct v4l2_format *fmt,
+                                unsigned int *nbuffers, unsigned int *nplanes,
+                                unsigned int sizes[], void *alloc_ctxs[])
 {
         struct m2mtest_ctx *ctx = vb2_get_drv_priv(vq);
         struct m2mtest_q_data *q_data;
diff --git a/drivers/media/video/mt9m001.c b/drivers/media/video/mt9m001.c
index 4da9cca939c1..63ae5c61c9bf 100644
--- a/drivers/media/video/mt9m001.c
+++ b/drivers/media/video/mt9m001.c
@@ -13,9 +13,11 @@
 #include <linux/i2c.h>
 #include <linux/log2.h>
 
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-chip-ident.h>
-#include <media/soc_camera.h>
+#include <media/v4l2-ctrls.h>
 
 /*
  * mt9m001 i2c address 0x5d
@@ -84,15 +86,19 @@ static const struct mt9m001_datafmt mt9m001_monochrome_fmts[] = {
 
 struct mt9m001 {
         struct v4l2_subdev subdev;
+        struct v4l2_ctrl_handler hdl;
+        struct {
+                /* exposure/auto-exposure cluster */
+                struct v4l2_ctrl *autoexposure;
+                struct v4l2_ctrl *exposure;
+        };
         struct v4l2_rect rect;  /* Sensor window */
         const struct mt9m001_datafmt *fmt;
         const struct mt9m001_datafmt *fmts;
         int num_fmts;
         int model;      /* V4L2_IDENT_MT9M001* codes from v4l2-chip-ident.h */
-        unsigned int gain;
-        unsigned int exposure;
+        unsigned int total_h;
         unsigned short y_skip_top;      /* Lines to skip at the top */
-        unsigned char autoexposure;
 };
 
 static struct mt9m001 *to_mt9m001(const struct i2c_client *client)
@@ -165,54 +171,13 @@ static int mt9m001_s_stream(struct v4l2_subdev *sd, int enable)
         return 0;
 }
 
-static int mt9m001_set_bus_param(struct soc_camera_device *icd,
-                                 unsigned long flags)
-{
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-        unsigned long width_flag = flags & SOCAM_DATAWIDTH_MASK;
-
-        /* Only one width bit may be set */
-        if (!is_power_of_2(width_flag))
-                return -EINVAL;
-
-        if (icl->set_bus_param)
-                return icl->set_bus_param(icl, width_flag);
-
-        /*
-         * Without board specific bus width settings we only support the
-         * sensors native bus width
-         */
-        if (width_flag == SOCAM_DATAWIDTH_10)
-                return 0;
-
-        return -EINVAL;
-}
-
-static unsigned long mt9m001_query_bus_param(struct soc_camera_device *icd)
-{
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-        /* MT9M001 has all capture_format parameters fixed */
-        unsigned long flags = SOCAM_PCLK_SAMPLE_FALLING |
-                SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_HIGH |
-                SOCAM_DATA_ACTIVE_HIGH | SOCAM_MASTER;
-
-        if (icl->query_bus_param)
-                flags |= icl->query_bus_param(icl) & SOCAM_DATAWIDTH_MASK;
-        else
-                flags |= SOCAM_DATAWIDTH_10;
-
-        return soc_camera_apply_sensor_flags(icl, flags);
-}
-
 static int mt9m001_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
 {
         struct i2c_client *client = v4l2_get_subdevdata(sd);
         struct mt9m001 *mt9m001 = to_mt9m001(client);
         struct v4l2_rect rect = a->c;
-        struct soc_camera_device *icd = client->dev.platform_data;
         int ret;
         const u16 hblank = 9, vblank = 25;
-        unsigned int total_h;
 
         if (mt9m001->fmts == mt9m001_colour_fmts)
                 /*
@@ -231,7 +196,7 @@ static int mt9m001_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
         soc_camera_limit_side(&rect.top, &rect.height,
                      MT9M001_ROW_SKIP, MT9M001_MIN_HEIGHT, MT9M001_MAX_HEIGHT);
 
-        total_h = rect.height + mt9m001->y_skip_top + vblank;
+        mt9m001->total_h = rect.height + mt9m001->y_skip_top + vblank;
 
         /* Blanking and start values - default... */
         ret = reg_write(client, MT9M001_HORIZONTAL_BLANKING, hblank);
@@ -240,7 +205,7 @@ static int mt9m001_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
 
         /*
          * The caller provides a supported format, as verified per
-         * call to icd->try_fmt()
+         * call to .try_mbus_fmt()
          */
         if (!ret)
                 ret = reg_write(client, MT9M001_COLUMN_START, rect.left);
@@ -251,17 +216,8 @@ static int mt9m001_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
         if (!ret)
                 ret = reg_write(client, MT9M001_WINDOW_HEIGHT,
                                 rect.height + mt9m001->y_skip_top - 1);
-        if (!ret && mt9m001->autoexposure) {
-                ret = reg_write(client, MT9M001_SHUTTER_WIDTH, total_h);
-                if (!ret) {
-                        const struct v4l2_queryctrl *qctrl =
-                                soc_camera_find_qctrl(icd->ops,
-                                                      V4L2_CID_EXPOSURE);
-                        mt9m001->exposure = (524 + (total_h - 1) *
-                                 (qctrl->maximum - qctrl->minimum)) /
-                                1048 + qctrl->minimum;
-                }
-        }
+        if (!ret && v4l2_ctrl_g_ctrl(mt9m001->autoexposure) == V4L2_EXPOSURE_AUTO)
+                ret = reg_write(client, MT9M001_SHUTTER_WIDTH, mt9m001->total_h);
 
         if (!ret)
                 mt9m001->rect = rect;
@@ -421,107 +377,48 @@ static int mt9m001_s_register(struct v4l2_subdev *sd,
 }
 #endif
 
-static const struct v4l2_queryctrl mt9m001_controls[] = {
-        {
-                .id             = V4L2_CID_VFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Vertically",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        }, {
-                .id             = V4L2_CID_GAIN,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Gain",
-                .minimum        = 0,
-                .maximum        = 127,
-                .step           = 1,
-                .default_value  = 64,
-                .flags          = V4L2_CTRL_FLAG_SLIDER,
-        }, {
-                .id             = V4L2_CID_EXPOSURE,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Exposure",
-                .minimum        = 1,
-                .maximum        = 255,
-                .step           = 1,
-                .default_value  = 255,
-                .flags          = V4L2_CTRL_FLAG_SLIDER,
-        }, {
-                .id             = V4L2_CID_EXPOSURE_AUTO,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Automatic Exposure",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 1,
-        }
-};
-
-static struct soc_camera_ops mt9m001_ops = {
-        .set_bus_param          = mt9m001_set_bus_param,
-        .query_bus_param        = mt9m001_query_bus_param,
-        .controls               = mt9m001_controls,
-        .num_controls           = ARRAY_SIZE(mt9m001_controls),
-};
-
-static int mt9m001_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int mt9m001_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 {
-        struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct mt9m001 *mt9m001 = to_mt9m001(client);
-        int data;
+        struct mt9m001 *mt9m001 = container_of(ctrl->handler,
+                                               struct mt9m001, hdl);
+        s32 min, max;
 
         switch (ctrl->id) {
-        case V4L2_CID_VFLIP:
-                data = reg_read(client, MT9M001_READ_OPTIONS2);
-                if (data < 0)
-                        return -EIO;
-                ctrl->value = !!(data & 0x8000);
-                break;
         case V4L2_CID_EXPOSURE_AUTO:
-                ctrl->value = mt9m001->autoexposure;
-                break;
-        case V4L2_CID_GAIN:
-                ctrl->value = mt9m001->gain;
-                break;
-        case V4L2_CID_EXPOSURE:
-                ctrl->value = mt9m001->exposure;
+                min = mt9m001->exposure->minimum;
+                max = mt9m001->exposure->maximum;
+                mt9m001->exposure->val =
+                        (524 + (mt9m001->total_h - 1) * (max - min)) / 1048 + min;
                 break;
         }
         return 0;
 }
 
-static int mt9m001_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int mt9m001_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+        struct mt9m001 *mt9m001 = container_of(ctrl->handler,
+                                               struct mt9m001, hdl);
+        struct v4l2_subdev *sd = &mt9m001->subdev;
         struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct mt9m001 *mt9m001 = to_mt9m001(client);
-        struct soc_camera_device *icd = client->dev.platform_data;
-        const struct v4l2_queryctrl *qctrl;
+        struct v4l2_ctrl *exp = mt9m001->exposure;
         int data;
 
-        qctrl = soc_camera_find_qctrl(&mt9m001_ops, ctrl->id);
-
-        if (!qctrl)
-                return -EINVAL;
-
         switch (ctrl->id) {
         case V4L2_CID_VFLIP:
-                if (ctrl->value)
+                if (ctrl->val)
                         data = reg_set(client, MT9M001_READ_OPTIONS2, 0x8000);
                 else
                         data = reg_clear(client, MT9M001_READ_OPTIONS2, 0x8000);
                 if (data < 0)
                         return -EIO;
-                break;
+                return 0;
+
         case V4L2_CID_GAIN:
-                if (ctrl->value > qctrl->maximum || ctrl->value < qctrl->minimum)
-                        return -EINVAL;
                 /* See Datasheet Table 7, Gain settings. */
-                if (ctrl->value <= qctrl->default_value) {
+                if (ctrl->val <= ctrl->default_value) {
                         /* Pack it into 0..1 step 0.125, register values 0..8 */
-                        unsigned long range = qctrl->default_value - qctrl->minimum;
-                        data = ((ctrl->value - qctrl->minimum) * 8 + range / 2) / range;
+                        unsigned long range = ctrl->default_value - ctrl->minimum;
+                        data = ((ctrl->val - ctrl->minimum) * 8 + range / 2) / range;
 
                         dev_dbg(&client->dev, "Setting gain %d\n", data);
                         data = reg_write(client, MT9M001_GLOBAL_GAIN, data);
@@ -530,8 +427,8 @@ static int mt9m001_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
                 } else {
                         /* Pack it into 1.125..15 variable step, register values 9..67 */
                         /* We assume qctrl->maximum - qctrl->default_value - 1 > 0 */
-                        unsigned long range = qctrl->maximum - qctrl->default_value - 1;
-                        unsigned long gain = ((ctrl->value - qctrl->default_value - 1) *
+                        unsigned long range = ctrl->maximum - ctrl->default_value - 1;
+                        unsigned long gain = ((ctrl->val - ctrl->default_value - 1) *
                                                111 + range / 2) / range + 9;
 
                         if (gain <= 32)
@@ -547,66 +444,44 @@ static int mt9m001_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
                         if (data < 0)
                                 return -EIO;
                 }
+                return 0;
 
-                /* Success */
-                mt9m001->gain = ctrl->value;
-                break;
-        case V4L2_CID_EXPOSURE:
-                /* mt9m001 has maximum == default */
-                if (ctrl->value > qctrl->maximum || ctrl->value < qctrl->minimum)
-                        return -EINVAL;
-                else {
-                        unsigned long range = qctrl->maximum - qctrl->minimum;
-                        unsigned long shutter = ((ctrl->value - qctrl->minimum) * 1048 +
+        case V4L2_CID_EXPOSURE_AUTO:
+                if (ctrl->val == V4L2_EXPOSURE_MANUAL) {
+                        unsigned long range = exp->maximum - exp->minimum;
+                        unsigned long shutter = ((exp->val - exp->minimum) * 1048 +
                                                  range / 2) / range + 1;
 
                         dev_dbg(&client->dev,
                                 "Setting shutter width from %d to %lu\n",
-                                reg_read(client, MT9M001_SHUTTER_WIDTH),
-                                shutter);
+                                reg_read(client, MT9M001_SHUTTER_WIDTH), shutter);
                         if (reg_write(client, MT9M001_SHUTTER_WIDTH, shutter) < 0)
                                 return -EIO;
-                        mt9m001->exposure = ctrl->value;
-                        mt9m001->autoexposure = 0;
-                }
-                break;
-        case V4L2_CID_EXPOSURE_AUTO:
-                if (ctrl->value) {
+                } else {
                         const u16 vblank = 25;
-                        unsigned int total_h = mt9m001->rect.height +
+
+                        mt9m001->total_h = mt9m001->rect.height +
                                 mt9m001->y_skip_top + vblank;
-                        if (reg_write(client, MT9M001_SHUTTER_WIDTH,
-                                      total_h) < 0)
+                        if (reg_write(client, MT9M001_SHUTTER_WIDTH, mt9m001->total_h) < 0)
                                 return -EIO;
-                        qctrl = soc_camera_find_qctrl(icd->ops, V4L2_CID_EXPOSURE);
-                        mt9m001->exposure = (524 + (total_h - 1) *
-                                 (qctrl->maximum - qctrl->minimum)) /
-                                1048 + qctrl->minimum;
-                        mt9m001->autoexposure = 1;
-                } else
-                        mt9m001->autoexposure = 0;
-                break;
+                }
+                return 0;
         }
-        return 0;
+        return -EINVAL;
 }
 
 /*
  * Interface active, can use i2c. If it fails, it can indeed mean, that
  * this wasn't our capture interface, so, we wait for the right one
  */
-static int mt9m001_video_probe(struct soc_camera_device *icd,
+static int mt9m001_video_probe(struct soc_camera_link *icl,
                                struct i2c_client *client)
 {
         struct mt9m001 *mt9m001 = to_mt9m001(client);
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
         s32 data;
         unsigned long flags;
         int ret;
 
-        /* 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);
         dev_dbg(&client->dev, "write: %d\n", data);
@@ -661,18 +536,11 @@ static int mt9m001_video_probe(struct soc_camera_device *icd,
                 dev_err(&client->dev, "Failed to initialise the camera\n");
 
         /* mt9m001_init() has reset the chip, returning registers to defaults */
-        mt9m001->gain = 64;
-        mt9m001->exposure = 255;
-
-        return ret;
+        return v4l2_ctrl_handler_setup(&mt9m001->hdl);
 }
 
-static void mt9m001_video_remove(struct soc_camera_device *icd)
+static void mt9m001_video_remove(struct soc_camera_link *icl)
 {
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-
-        dev_dbg(icd->pdev, "Video removed: %p, %p\n",
-                icd->parent, icd->vdev);
         if (icl->free_bus)
                 icl->free_bus(icl);
 }
@@ -687,9 +555,12 @@ static int mt9m001_g_skip_top_lines(struct v4l2_subdev *sd, u32 *lines)
         return 0;
 }
 
+static const struct v4l2_ctrl_ops mt9m001_ctrl_ops = {
+        .g_volatile_ctrl = mt9m001_g_volatile_ctrl,
+        .s_ctrl = mt9m001_s_ctrl,
+};
+
 static struct v4l2_subdev_core_ops mt9m001_subdev_core_ops = {
-        .g_ctrl         = mt9m001_g_ctrl,
-        .s_ctrl         = mt9m001_s_ctrl,
         .g_chip_ident   = mt9m001_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register     = mt9m001_g_register,
@@ -710,6 +581,40 @@ static int mt9m001_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
         return 0;
 }
 
+static int mt9m001_g_mbus_config(struct v4l2_subdev *sd,
+                                struct v4l2_mbus_config *cfg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+        /* MT9M001 has all capture_format parameters fixed */
+        cfg->flags = V4L2_MBUS_PCLK_SAMPLE_FALLING |
+                V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+                V4L2_MBUS_DATA_ACTIVE_HIGH | V4L2_MBUS_MASTER;
+        cfg->type = V4L2_MBUS_PARALLEL;
+        cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+        return 0;
+}
+
+static int mt9m001_s_mbus_config(struct v4l2_subdev *sd,
+                                const struct v4l2_mbus_config *cfg)
+{
+        const struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+        struct mt9m001 *mt9m001 = to_mt9m001(client);
+        unsigned int bps = soc_mbus_get_fmtdesc(mt9m001->fmt->code)->bits_per_sample;
+
+        if (icl->set_bus_param)
+                return icl->set_bus_param(icl, 1 << (bps - 1));
+
+        /*
+         * Without board specific bus width settings we only support the
+         * sensors native bus width
+         */
+        return bps == 10 ? 0 : -EINVAL;
+}
+
 static struct v4l2_subdev_video_ops mt9m001_subdev_video_ops = {
         .s_stream       = mt9m001_s_stream,
         .s_mbus_fmt     = mt9m001_s_fmt,
@@ -719,6 +624,8 @@ static struct v4l2_subdev_video_ops mt9m001_subdev_video_ops = {
         .g_crop         = mt9m001_g_crop,
         .cropcap        = mt9m001_cropcap,
         .enum_mbus_fmt  = mt9m001_enum_fmt,
+        .g_mbus_config  = mt9m001_g_mbus_config,
+        .s_mbus_config  = mt9m001_s_mbus_config,
 };
 
 static struct v4l2_subdev_sensor_ops mt9m001_subdev_sensor_ops = {
@@ -735,17 +642,10 @@ static int mt9m001_probe(struct i2c_client *client,
                          const struct i2c_device_id *did)
 {
         struct mt9m001 *mt9m001;
-        struct soc_camera_device *icd = client->dev.platform_data;
         struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
-        struct soc_camera_link *icl;
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
         int ret;
 
-        if (!icd) {
-                dev_err(&client->dev, "MT9M001: missing soc-camera data!\n");
-                return -EINVAL;
-        }
-
-        icl = to_soc_camera_link(icd);
         if (!icl) {
                 dev_err(&client->dev, "MT9M001 driver needs platform data\n");
                 return -EINVAL;
@@ -762,25 +662,40 @@ static int mt9m001_probe(struct i2c_client *client,
                 return -ENOMEM;
 
         v4l2_i2c_subdev_init(&mt9m001->subdev, client, &mt9m001_subdev_ops);
+        v4l2_ctrl_handler_init(&mt9m001->hdl, 4);
+        v4l2_ctrl_new_std(&mt9m001->hdl, &mt9m001_ctrl_ops,
+                        V4L2_CID_VFLIP, 0, 1, 1, 0);
+        v4l2_ctrl_new_std(&mt9m001->hdl, &mt9m001_ctrl_ops,
+                        V4L2_CID_GAIN, 0, 127, 1, 64);
+        mt9m001->exposure = v4l2_ctrl_new_std(&mt9m001->hdl, &mt9m001_ctrl_ops,
+                        V4L2_CID_EXPOSURE, 1, 255, 1, 255);
+        /*
+         * Simulated autoexposure. If enabled, we calculate shutter width
+         * ourselves in the driver based on vertical blanking and frame width
+         */
+        mt9m001->autoexposure = v4l2_ctrl_new_std_menu(&mt9m001->hdl,
+                        &mt9m001_ctrl_ops, V4L2_CID_EXPOSURE_AUTO, 1, 0,
+                        V4L2_EXPOSURE_AUTO);
+        mt9m001->subdev.ctrl_handler = &mt9m001->hdl;
+        if (mt9m001->hdl.error) {
+                int err = mt9m001->hdl.error;
 
-        /* Second stage probe - when a capture adapter is there */
-        icd->ops                = &mt9m001_ops;
+                kfree(mt9m001);
+                return err;
+        }
+        v4l2_ctrl_auto_cluster(2, &mt9m001->autoexposure,
+                                        V4L2_EXPOSURE_MANUAL, true);
 
+        /* Second stage probe - when a capture adapter is there */
         mt9m001->y_skip_top     = 0;
         mt9m001->rect.left      = MT9M001_COLUMN_SKIP;
         mt9m001->rect.top       = MT9M001_ROW_SKIP;
         mt9m001->rect.width     = MT9M001_MAX_WIDTH;
         mt9m001->rect.height    = MT9M001_MAX_HEIGHT;
 
-        /*
-         * Simulated autoexposure. If enabled, we calculate shutter width
-         * ourselves in the driver based on vertical blanking and frame width
-         */
-        mt9m001->autoexposure = 1;
-
-        ret = mt9m001_video_probe(icd, client);
+        ret = mt9m001_video_probe(icl, client);
         if (ret) {
-                icd->ops = NULL;
+                v4l2_ctrl_handler_free(&mt9m001->hdl);
                 kfree(mt9m001);
         }
 
@@ -790,10 +705,11 @@ static int mt9m001_probe(struct i2c_client *client,
 static int mt9m001_remove(struct i2c_client *client)
 {
         struct mt9m001 *mt9m001 = to_mt9m001(client);
-        struct soc_camera_device *icd = client->dev.platform_data;
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
 
-        icd->ops = NULL;
-        mt9m001_video_remove(icd);
+        v4l2_device_unregister_subdev(&mt9m001->subdev);
+        v4l2_ctrl_handler_free(&mt9m001->hdl);
+        mt9m001_video_remove(icl);
         kfree(mt9m001);
 
         return 0;
diff --git a/drivers/media/video/mt9m111.c b/drivers/media/video/mt9m111.c
index 07af26e6bebd..f023cc092c2b 100644
--- a/drivers/media/video/mt9m111.c
+++ b/drivers/media/video/mt9m111.c
@@ -13,10 +13,12 @@
 #include <linux/log2.h>
 #include <linux/gpio.h>
 #include <linux/delay.h>
+#include <linux/v4l2-mediabus.h>
 
+#include <media/soc_camera.h>
 #include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
 #include <media/v4l2-chip-ident.h>
-#include <media/soc_camera.h>
 
 /*
  * MT9M111, MT9M112 and MT9M131:
@@ -177,6 +179,8 @@ enum mt9m111_context {
 
 struct mt9m111 {
         struct v4l2_subdev subdev;
+        struct v4l2_ctrl_handler hdl;
+        struct v4l2_ctrl *gain;
         int model;      /* V4L2_IDENT_MT9M111 or V4L2_IDENT_MT9M112 code
                          * from v4l2-chip-ident.h */
         enum mt9m111_context context;
@@ -185,13 +189,8 @@ struct mt9m111 {
         int power_count;
         const struct mt9m111_datafmt *fmt;
         int lastpage;   /* PageMap cache value */
-        unsigned int gain;
-        unsigned char autoexposure;
         unsigned char datawidth;
         unsigned int powered:1;
-        unsigned int hflip:1;
-        unsigned int vflip:1;
-        unsigned int autowhitebalance:1;
 };
 
 static struct mt9m111 *to_mt9m111(const struct i2c_client *client)
@@ -363,21 +362,6 @@ static int mt9m111_reset(struct mt9m111 *mt9m111)
         return ret;
 }
 
-static unsigned long mt9m111_query_bus_param(struct soc_camera_device *icd)
-{
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-        unsigned long flags = SOCAM_MASTER | SOCAM_PCLK_SAMPLE_RISING |
-                SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_HIGH |
-                SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATAWIDTH_8;
-
-        return soc_camera_apply_sensor_flags(icl, flags);
-}
-
-static int mt9m111_set_bus_param(struct soc_camera_device *icd, unsigned long f)
-{
-        return 0;
-}
-
 static int mt9m111_make_rect(struct mt9m111 *mt9m111,
                              struct v4l2_rect *rect)
 {
@@ -660,50 +644,6 @@ static int mt9m111_s_register(struct v4l2_subdev *sd,
 }
 #endif
 
-static const struct v4l2_queryctrl mt9m111_controls[] = {
-        {
-                .id             = V4L2_CID_VFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Verticaly",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        }, {
-                .id             = V4L2_CID_HFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Horizontaly",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        }, {    /* gain = 1/32*val (=>gain=1 if val==32) */
-                .id             = V4L2_CID_GAIN,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Gain",
-                .minimum        = 0,
-                .maximum        = 63 * 2 * 2,
-                .step           = 1,
-                .default_value  = 32,
-                .flags          = V4L2_CTRL_FLAG_SLIDER,
-        }, {
-                .id             = V4L2_CID_EXPOSURE_AUTO,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Auto Exposure",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 1,
-        }
-};
-
-static struct soc_camera_ops mt9m111_ops = {
-        .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 mt9m111 *mt9m111, int flip, int mask)
 {
         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
@@ -744,7 +684,6 @@ static int mt9m111_set_global_gain(struct mt9m111 *mt9m111, int gain)
         if (gain > 63 * 2 * 2)
                 return -EINVAL;
 
-        mt9m111->gain = gain;
         if ((gain >= 64 * 2) && (gain < 63 * 2 * 2))
                 val = (1 << 10) | (1 << 9) | (gain / 4);
         else if ((gain >= 64) && (gain < 64 * 2))
@@ -758,118 +697,47 @@ static int mt9m111_set_global_gain(struct mt9m111 *mt9m111, int gain)
 static int mt9m111_set_autoexposure(struct mt9m111 *mt9m111, int on)
 {
         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-        int ret;
 
         if (on)
-                ret = reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
-        else
-                ret = reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
-
-        if (!ret)
-                mt9m111->autoexposure = on;
-
-        return ret;
+                return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
+        return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
 }
 
 static int mt9m111_set_autowhitebalance(struct mt9m111 *mt9m111, int on)
 {
         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-        int ret;
 
         if (on)
-                ret = reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
-        else
-                ret = reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
-
-        if (!ret)
-                mt9m111->autowhitebalance = on;
-
-        return ret;
-}
-
-static int mt9m111_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-        struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
-        int data;
-
-        switch (ctrl->id) {
-        case V4L2_CID_VFLIP:
-                if (mt9m111->context == HIGHPOWER)
-                        data = reg_read(READ_MODE_B);
-                else
-                        data = reg_read(READ_MODE_A);
-
-                if (data < 0)
-                        return -EIO;
-                ctrl->value = !!(data & MT9M111_RMB_MIRROR_ROWS);
-                break;
-        case V4L2_CID_HFLIP:
-                if (mt9m111->context == HIGHPOWER)
-                        data = reg_read(READ_MODE_B);
-                else
-                        data = reg_read(READ_MODE_A);
-
-                if (data < 0)
-                        return -EIO;
-                ctrl->value = !!(data & MT9M111_RMB_MIRROR_COLS);
-                break;
-        case V4L2_CID_GAIN:
-                data = mt9m111_get_global_gain(mt9m111);
-                if (data < 0)
-                        return data;
-                ctrl->value = data;
-                break;
-        case V4L2_CID_EXPOSURE_AUTO:
-                ctrl->value = mt9m111->autoexposure;
-                break;
-        case V4L2_CID_AUTO_WHITE_BALANCE:
-                ctrl->value = mt9m111->autowhitebalance;
-                break;
-        }
-        return 0;
+                return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
+        return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
 }
 
-static int mt9m111_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int mt9m111_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-        struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
-        const struct v4l2_queryctrl *qctrl;
-        int ret;
-
-        qctrl = soc_camera_find_qctrl(&mt9m111_ops, ctrl->id);
-        if (!qctrl)
-                return -EINVAL;
+        struct mt9m111 *mt9m111 = container_of(ctrl->handler,
+                                               struct mt9m111, hdl);
 
         switch (ctrl->id) {
         case V4L2_CID_VFLIP:
-                mt9m111->vflip = ctrl->value;
-                ret = mt9m111_set_flip(mt9m111, ctrl->value,
+                return mt9m111_set_flip(mt9m111, ctrl->val,
                                         MT9M111_RMB_MIRROR_ROWS);
-                break;
         case V4L2_CID_HFLIP:
-                mt9m111->hflip = ctrl->value;
-                ret = mt9m111_set_flip(mt9m111, ctrl->value,
+                return mt9m111_set_flip(mt9m111, ctrl->val,
                                         MT9M111_RMB_MIRROR_COLS);
-                break;
         case V4L2_CID_GAIN:
-                ret = mt9m111_set_global_gain(mt9m111, ctrl->value);
-                break;
+                return mt9m111_set_global_gain(mt9m111, ctrl->val);
         case V4L2_CID_EXPOSURE_AUTO:
-                ret =  mt9m111_set_autoexposure(mt9m111, ctrl->value);
-                break;
+                return mt9m111_set_autoexposure(mt9m111, ctrl->val);
         case V4L2_CID_AUTO_WHITE_BALANCE:
-                ret =  mt9m111_set_autowhitebalance(mt9m111, ctrl->value);
-                break;
-        default:
-                ret = -EINVAL;
+                return mt9m111_set_autowhitebalance(mt9m111, ctrl->val);
         }
 
-        return ret;
+        return -EINVAL;
 }
 
 static int mt9m111_suspend(struct mt9m111 *mt9m111)
 {
-        mt9m111->gain = mt9m111_get_global_gain(mt9m111);
+        v4l2_ctrl_s_ctrl(mt9m111->gain, mt9m111_get_global_gain(mt9m111));
 
         return 0;
 }
@@ -879,11 +747,7 @@ static void mt9m111_restore_state(struct mt9m111 *mt9m111)
         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);
+        v4l2_ctrl_handler_setup(&mt9m111->hdl);
 }
 
 static int mt9m111_resume(struct mt9m111 *mt9m111)
@@ -911,8 +775,6 @@ static int mt9m111_init(struct mt9m111 *mt9m111)
                 ret = mt9m111_reset(mt9m111);
         if (!ret)
                 ret = mt9m111_set_context(mt9m111, mt9m111->context);
-        if (!ret)
-                ret = mt9m111_set_autoexposure(mt9m111, mt9m111->autoexposure);
         if (ret)
                 dev_err(&client->dev, "mt9m111 init failed: %d\n", ret);
         return ret;
@@ -922,22 +784,12 @@ static int mt9m111_init(struct mt9m111 *mt9m111)
  * Interface active, can use i2c. If it fails, it can indeed mean, that
  * this wasn't our capture interface, so, we wait for the right one
  */
-static int mt9m111_video_probe(struct soc_camera_device *icd,
-                               struct i2c_client *client)
+static int mt9m111_video_probe(struct i2c_client *client)
 {
         struct mt9m111 *mt9m111 = to_mt9m111(client);
         s32 data;
         int ret;
 
-        /* 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->lastpage = -1;
-
-        mt9m111->autoexposure = 1;
-        mt9m111->autowhitebalance = 1;
-
         data = reg_read(CHIP_VERSION);
 
         switch (data) {
@@ -951,17 +803,16 @@ static int mt9m111_video_probe(struct soc_camera_device *icd,
                 dev_info(&client->dev, "Detected a MT9M112 chip ID %x\n", data);
                 break;
         default:
-                ret = -ENODEV;
                 dev_err(&client->dev,
                         "No MT9M111/MT9M112/MT9M131 chip detected register read %x\n",
                         data);
-                goto ei2c;
+                return -ENODEV;
         }
 
         ret = mt9m111_init(mt9m111);
-
-ei2c:
-        return ret;
+        if (ret)
+                return ret;
+        return v4l2_ctrl_handler_setup(&mt9m111->hdl);
 }
 
 static int mt9m111_s_power(struct v4l2_subdev *sd, int on)
@@ -998,9 +849,11 @@ out:
         return ret;
 }
 
+static const struct v4l2_ctrl_ops mt9m111_ctrl_ops = {
+        .s_ctrl = mt9m111_s_ctrl,
+};
+
 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
@@ -1019,6 +872,21 @@ static int mt9m111_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
         return 0;
 }
 
+static int mt9m111_g_mbus_config(struct v4l2_subdev *sd,
+                                struct v4l2_mbus_config *cfg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+        cfg->flags = V4L2_MBUS_MASTER | V4L2_MBUS_PCLK_SAMPLE_RISING |
+                V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+                V4L2_MBUS_DATA_ACTIVE_HIGH;
+        cfg->type = V4L2_MBUS_PARALLEL;
+        cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+        return 0;
+}
+
 static struct v4l2_subdev_video_ops mt9m111_subdev_video_ops = {
         .s_mbus_fmt     = mt9m111_s_fmt,
         .g_mbus_fmt     = mt9m111_g_fmt,
@@ -1027,6 +895,7 @@ static struct v4l2_subdev_video_ops mt9m111_subdev_video_ops = {
         .g_crop         = mt9m111_g_crop,
         .cropcap        = mt9m111_cropcap,
         .enum_mbus_fmt  = mt9m111_enum_fmt,
+        .g_mbus_config  = mt9m111_g_mbus_config,
 };
 
 static struct v4l2_subdev_ops mt9m111_subdev_ops = {
@@ -1038,17 +907,10 @@ static int mt9m111_probe(struct i2c_client *client,
                          const struct i2c_device_id *did)
 {
         struct mt9m111 *mt9m111;
-        struct soc_camera_device *icd = client->dev.platform_data;
         struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
-        struct soc_camera_link *icl;
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
         int ret;
 
-        if (!icd) {
-                dev_err(&client->dev, "mt9m111: soc-camera data missing!\n");
-                return -EINVAL;
-        }
-
-        icl = to_soc_camera_link(icd);
         if (!icl) {
                 dev_err(&client->dev, "mt9m111: driver needs platform data\n");
                 return -EINVAL;
@@ -1065,19 +927,37 @@ static int mt9m111_probe(struct i2c_client *client,
                 return -ENOMEM;
 
         v4l2_i2c_subdev_init(&mt9m111->subdev, client, &mt9m111_subdev_ops);
+        v4l2_ctrl_handler_init(&mt9m111->hdl, 5);
+        v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
+                        V4L2_CID_VFLIP, 0, 1, 1, 0);
+        v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
+                        V4L2_CID_HFLIP, 0, 1, 1, 0);
+        v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
+                        V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
+        mt9m111->gain = v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
+                        V4L2_CID_GAIN, 0, 63 * 2 * 2, 1, 32);
+        v4l2_ctrl_new_std_menu(&mt9m111->hdl,
+                        &mt9m111_ctrl_ops, V4L2_CID_EXPOSURE_AUTO, 1, 0,
+                        V4L2_EXPOSURE_AUTO);
+        mt9m111->subdev.ctrl_handler = &mt9m111->hdl;
+        if (mt9m111->hdl.error) {
+                int err = mt9m111->hdl.error;
 
-        /* Second stage probe - when a capture adapter is there */
-        icd->ops                = &mt9m111_ops;
+                kfree(mt9m111);
+                return err;
+        }
 
+        /* Second stage probe - when a capture adapter is there */
         mt9m111->rect.left      = MT9M111_MIN_DARK_COLS;
         mt9m111->rect.top       = MT9M111_MIN_DARK_ROWS;
         mt9m111->rect.width     = MT9M111_MAX_WIDTH;
         mt9m111->rect.height    = MT9M111_MAX_HEIGHT;
         mt9m111->fmt            = &mt9m111_colour_fmts[0];
+        mt9m111->lastpage       = -1;
 
-        ret = mt9m111_video_probe(icd, client);
+        ret = mt9m111_video_probe(client);
         if (ret) {
-                icd->ops = NULL;
+                v4l2_ctrl_handler_free(&mt9m111->hdl);
                 kfree(mt9m111);
         }
 
@@ -1087,9 +967,9 @@ static int mt9m111_probe(struct i2c_client *client,
 static int mt9m111_remove(struct i2c_client *client)
 {
         struct mt9m111 *mt9m111 = to_mt9m111(client);
-        struct soc_camera_device *icd = client->dev.platform_data;
 
-        icd->ops = NULL;
+        v4l2_device_unregister_subdev(&mt9m111->subdev);
+        v4l2_ctrl_handler_free(&mt9m111->hdl);
         kfree(mt9m111);
 
         return 0;
diff --git a/drivers/media/video/mt9t031.c b/drivers/media/video/mt9t031.c
index 30547cc3f89b..7ee84cc578b9 100644
--- a/drivers/media/video/mt9t031.c
+++ b/drivers/media/video/mt9t031.c
@@ -13,11 +13,20 @@
 #include <linux/log2.h>
 #include <linux/pm.h>
 #include <linux/slab.h>
+#include <linux/v4l2-mediabus.h>
 #include <linux/videodev2.h>
 
 #include <media/soc_camera.h>
 #include <media/v4l2-chip-ident.h>
 #include <media/v4l2-subdev.h>
+#include <media/v4l2-ctrls.h>
+
+/*
+ * ATTENTION: this driver still cannot be used outside of the soc-camera
+ * framework because of its PM implementation, using the video_device node.
+ * If hardware becomes available for testing, alternative PM approaches shall
+ * be considered and tested.
+ */
 
 /*
  * mt9t031 i2c address 0x5d
@@ -57,21 +66,20 @@
 #define MT9T031_COLUMN_SKIP             32
 #define MT9T031_ROW_SKIP                20
 
-#define MT9T031_BUS_PARAM       (SOCAM_PCLK_SAMPLE_RISING |     \
-        SOCAM_PCLK_SAMPLE_FALLING | SOCAM_HSYNC_ACTIVE_HIGH |   \
-        SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_DATA_ACTIVE_HIGH |      \
-        SOCAM_MASTER | SOCAM_DATAWIDTH_10)
-
 struct mt9t031 {
         struct v4l2_subdev subdev;
+        struct v4l2_ctrl_handler hdl;
+        struct {
+                /* exposure/auto-exposure cluster */
+                struct v4l2_ctrl *autoexposure;
+                struct v4l2_ctrl *exposure;
+        };
         struct v4l2_rect rect;  /* Sensor window */
         int model;      /* V4L2_IDENT_MT9T031* codes from v4l2-chip-ident.h */
         u16 xskip;
         u16 yskip;
-        unsigned int gain;
+        unsigned int total_h;
         unsigned short y_skip_top;      /* Lines to skip at the top */
-        unsigned int exposure;
-        unsigned char autoexposure;
 };
 
 static struct mt9t031 *to_mt9t031(const struct i2c_client *client)
@@ -179,95 +187,6 @@ static int mt9t031_s_stream(struct v4l2_subdev *sd, int enable)
         return 0;
 }
 
-static int mt9t031_set_bus_param(struct soc_camera_device *icd,
-                                 unsigned long flags)
-{
-        struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
-
-        /* The caller should have queried our parameters, check anyway */
-        if (flags & ~MT9T031_BUS_PARAM)
-                return -EINVAL;
-
-        if (flags & SOCAM_PCLK_SAMPLE_FALLING)
-                reg_clear(client, MT9T031_PIXEL_CLOCK_CONTROL, 0x8000);
-        else
-                reg_set(client, MT9T031_PIXEL_CLOCK_CONTROL, 0x8000);
-
-        return 0;
-}
-
-static unsigned long mt9t031_query_bus_param(struct soc_camera_device *icd)
-{
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-
-        return soc_camera_apply_sensor_flags(icl, MT9T031_BUS_PARAM);
-}
-
-enum {
-        MT9T031_CTRL_VFLIP,
-        MT9T031_CTRL_HFLIP,
-        MT9T031_CTRL_GAIN,
-        MT9T031_CTRL_EXPOSURE,
-        MT9T031_CTRL_EXPOSURE_AUTO,
-};
-
-static const struct v4l2_queryctrl mt9t031_controls[] = {
-        [MT9T031_CTRL_VFLIP] = {
-                .id             = V4L2_CID_VFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Vertically",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        },
-        [MT9T031_CTRL_HFLIP] = {
-                .id             = V4L2_CID_HFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Horizontally",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        },
-        [MT9T031_CTRL_GAIN] = {
-                .id             = V4L2_CID_GAIN,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Gain",
-                .minimum        = 0,
-                .maximum        = 127,
-                .step           = 1,
-                .default_value  = 64,
-                .flags          = V4L2_CTRL_FLAG_SLIDER,
-        },
-        [MT9T031_CTRL_EXPOSURE] = {
-                .id             = V4L2_CID_EXPOSURE,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Exposure",
-                .minimum        = 1,
-                .maximum        = 255,
-                .step           = 1,
-                .default_value  = 255,
-                .flags          = V4L2_CTRL_FLAG_SLIDER,
-        },
-        [MT9T031_CTRL_EXPOSURE_AUTO] = {
-                .id             = V4L2_CID_EXPOSURE_AUTO,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Automatic Exposure",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 1,
-        }
-};
-
-static struct soc_camera_ops mt9t031_ops = {
-        .set_bus_param          = mt9t031_set_bus_param,
-        .query_bus_param        = mt9t031_query_bus_param,
-        .controls               = mt9t031_controls,
-        .num_controls           = ARRAY_SIZE(mt9t031_controls),
-};
-
 /* target must be _even_ */
 static u16 mt9t031_skip(s32 *source, s32 target, s32 max)
 {
@@ -353,7 +272,7 @@ static int mt9t031_set_params(struct i2c_client *client,
 
         /*
          * The caller provides a supported format, as guaranteed by
-         * icd->try_fmt_cap(), soc_camera_s_crop() and soc_camera_cropcap()
+         * .try_mbus_fmt(), soc_camera_s_crop() and soc_camera_cropcap()
          */
         if (ret >= 0)
                 ret = reg_write(client, MT9T031_COLUMN_START, rect->left);
@@ -364,17 +283,10 @@ static int mt9t031_set_params(struct i2c_client *client,
         if (ret >= 0)
                 ret = reg_write(client, MT9T031_WINDOW_HEIGHT,
                                 rect->height + mt9t031->y_skip_top - 1);
-        if (ret >= 0 && mt9t031->autoexposure) {
-                unsigned int total_h = rect->height + mt9t031->y_skip_top + vblank;
-                ret = set_shutter(client, total_h);
-                if (ret >= 0) {
-                        const u32 shutter_max = MT9T031_MAX_HEIGHT + vblank;
-                        const struct v4l2_queryctrl *qctrl =
-                                &mt9t031_controls[MT9T031_CTRL_EXPOSURE];
-                        mt9t031->exposure = (shutter_max / 2 + (total_h - 1) *
-                                 (qctrl->maximum - qctrl->minimum)) /
-                                shutter_max + qctrl->minimum;
-                }
+        if (ret >= 0 && v4l2_ctrl_g_ctrl(mt9t031->autoexposure) == V4L2_EXPOSURE_AUTO) {
+                mt9t031->total_h = rect->height + mt9t031->y_skip_top + vblank;
+
+                ret = set_shutter(client, mt9t031->total_h);
         }
 
         /* Re-enable register update, commit all changes */
@@ -543,71 +455,57 @@ static int mt9t031_s_register(struct v4l2_subdev *sd,
 }
 #endif
 
-static int mt9t031_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int mt9t031_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 {
-        struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct mt9t031 *mt9t031 = to_mt9t031(client);
-        int data;
+        struct mt9t031 *mt9t031 = container_of(ctrl->handler,
+                                               struct mt9t031, hdl);
+        const u32 shutter_max = MT9T031_MAX_HEIGHT + MT9T031_VERTICAL_BLANK;
+        s32 min, max;
 
         switch (ctrl->id) {
-        case V4L2_CID_VFLIP:
-                data = reg_read(client, MT9T031_READ_MODE_2);
-                if (data < 0)
-                        return -EIO;
-                ctrl->value = !!(data & 0x8000);
-                break;
-        case V4L2_CID_HFLIP:
-                data = reg_read(client, MT9T031_READ_MODE_2);
-                if (data < 0)
-                        return -EIO;
-                ctrl->value = !!(data & 0x4000);
-                break;
         case V4L2_CID_EXPOSURE_AUTO:
-                ctrl->value = mt9t031->autoexposure;
-                break;
-        case V4L2_CID_GAIN:
-                ctrl->value = mt9t031->gain;
-                break;
-        case V4L2_CID_EXPOSURE:
-                ctrl->value = mt9t031->exposure;
+                min = mt9t031->exposure->minimum;
+                max = mt9t031->exposure->maximum;
+                mt9t031->exposure->val =
+                        (shutter_max / 2 + (mt9t031->total_h - 1) * (max - min))
+                                / shutter_max + min;
                 break;
         }
         return 0;
 }
 
-static int mt9t031_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int mt9t031_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+        struct mt9t031 *mt9t031 = container_of(ctrl->handler,
+                                               struct mt9t031, hdl);
+        struct v4l2_subdev *sd = &mt9t031->subdev;
         struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct mt9t031 *mt9t031 = to_mt9t031(client);
-        const struct v4l2_queryctrl *qctrl;
+        struct v4l2_ctrl *exp = mt9t031->exposure;
         int data;
 
         switch (ctrl->id) {
         case V4L2_CID_VFLIP:
-                if (ctrl->value)
+                if (ctrl->val)
                         data = reg_set(client, MT9T031_READ_MODE_2, 0x8000);
                 else
                         data = reg_clear(client, MT9T031_READ_MODE_2, 0x8000);
                 if (data < 0)
                         return -EIO;
-                break;
+                return 0;
         case V4L2_CID_HFLIP:
-                if (ctrl->value)
+                if (ctrl->val)
                         data = reg_set(client, MT9T031_READ_MODE_2, 0x4000);
                 else
                         data = reg_clear(client, MT9T031_READ_MODE_2, 0x4000);
                 if (data < 0)
                         return -EIO;
-                break;
+                return 0;
         case V4L2_CID_GAIN:
-                qctrl = &mt9t031_controls[MT9T031_CTRL_GAIN];
-                if (ctrl->value > qctrl->maximum || ctrl->value < qctrl->minimum)
-                        return -EINVAL;
                 /* See Datasheet Table 7, Gain settings. */
-                if (ctrl->value <= qctrl->default_value) {
+                if (ctrl->val <= ctrl->default_value) {
                         /* Pack it into 0..1 step 0.125, register values 0..8 */
-                        unsigned long range = qctrl->default_value - qctrl->minimum;
-                        data = ((ctrl->value - qctrl->minimum) * 8 + range / 2) / range;
+                        unsigned long range = ctrl->default_value - ctrl->minimum;
+                        data = ((ctrl->val - ctrl->minimum) * 8 + range / 2) / range;
 
                         dev_dbg(&client->dev, "Setting gain %d\n", data);
                         data = reg_write(client, MT9T031_GLOBAL_GAIN, data);
@@ -616,9 +514,9 @@ static int mt9t031_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
                 } else {
                         /* Pack it into 1.125..128 variable step, register values 9..0x7860 */
                         /* We assume qctrl->maximum - qctrl->default_value - 1 > 0 */
-                        unsigned long range = qctrl->maximum - qctrl->default_value - 1;
+                        unsigned long range = ctrl->maximum - ctrl->default_value - 1;
                         /* calculated gain: map 65..127 to 9..1024 step 0.125 */
-                        unsigned long gain = ((ctrl->value - qctrl->default_value - 1) *
+                        unsigned long gain = ((ctrl->val - ctrl->default_value - 1) *
                                                1015 + range / 2) / range + 9;
 
                         if (gain <= 32)         /* calculated gain 9..32 -> 9..32 */
@@ -635,19 +533,13 @@ static int mt9t031_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
                         if (data < 0)
                                 return -EIO;
                 }
+                return 0;
 
-                /* Success */
-                mt9t031->gain = ctrl->value;
-                break;
-        case V4L2_CID_EXPOSURE:
-                qctrl = &mt9t031_controls[MT9T031_CTRL_EXPOSURE];
-                /* mt9t031 has maximum == default */
-                if (ctrl->value > qctrl->maximum || ctrl->value < qctrl->minimum)
-                        return -EINVAL;
-                else {
-                        const unsigned long range = qctrl->maximum - qctrl->minimum;
-                        const u32 shutter = ((ctrl->value - qctrl->minimum) * 1048 +
-                                             range / 2) / range + 1;
+        case V4L2_CID_EXPOSURE_AUTO:
+                if (ctrl->val == V4L2_EXPOSURE_MANUAL) {
+                        unsigned int range = exp->maximum - exp->minimum;
+                        unsigned int shutter = ((exp->val - exp->minimum) * 1048 +
+                                                 range / 2) / range + 1;
                         u32 old;
 
                         get_shutter(client, &old);
@@ -655,27 +547,15 @@ static int mt9t031_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
                                 old, shutter);
                         if (set_shutter(client, shutter) < 0)
                                 return -EIO;
-                        mt9t031->exposure = ctrl->value;
-                        mt9t031->autoexposure = 0;
-                }
-                break;
-        case V4L2_CID_EXPOSURE_AUTO:
-                if (ctrl->value) {
+                } else {
                         const u16 vblank = MT9T031_VERTICAL_BLANK;
-                        const u32 shutter_max = MT9T031_MAX_HEIGHT + vblank;
-                        unsigned int total_h = mt9t031->rect.height +
+                        mt9t031->total_h = mt9t031->rect.height +
                                 mt9t031->y_skip_top + vblank;
 
-                        if (set_shutter(client, total_h) < 0)
+                        if (set_shutter(client, mt9t031->total_h) < 0)
                                 return -EIO;
-                        qctrl = &mt9t031_controls[MT9T031_CTRL_EXPOSURE];
-                        mt9t031->exposure = (shutter_max / 2 + (total_h - 1) *
-                                 (qctrl->maximum - qctrl->minimum)) /
-                                shutter_max + qctrl->minimum;
-                        mt9t031->autoexposure = 1;
-                } else
-                        mt9t031->autoexposure = 0;
-                break;
+                }
+                return 0;
         default:
                 return -EINVAL;
         }
@@ -700,8 +580,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 = dev_get_drvdata(vdev->parent);
-        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+        struct v4l2_subdev *sd = soc_camera_vdev_to_subdev(vdev);
         struct i2c_client *client = v4l2_get_subdevdata(sd);
         struct mt9t031 *mt9t031 = to_mt9t031(client);
 
@@ -734,6 +613,19 @@ static struct device_type mt9t031_dev_type = {
         .pm     = &mt9t031_dev_pm_ops,
 };
 
+static int mt9t031_s_power(struct v4l2_subdev *sd, int on)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct video_device *vdev = soc_camera_i2c_to_vdev(client);
+
+        if (on)
+                vdev->dev.type = &mt9t031_dev_type;
+        else
+                vdev->dev.type = NULL;
+
+        return 0;
+}
+
 /*
  * Interface active, can use i2c. If it fails, it can indeed mean, that
  * this wasn't our capture interface, so, we wait for the right one
@@ -741,7 +633,6 @@ static struct device_type mt9t031_dev_type = {
 static int mt9t031_video_probe(struct i2c_client *client)
 {
         struct mt9t031 *mt9t031 = to_mt9t031(client);
-        struct video_device *vdev = soc_camera_i2c_to_vdev(client);
         s32 data;
         int ret;
 
@@ -768,11 +659,7 @@ static int mt9t031_video_probe(struct i2c_client *client)
         if (ret < 0)
                 dev_err(&client->dev, "Failed to initialise the camera\n");
         else
-                vdev->dev.type = &mt9t031_dev_type;
-
-        /* mt9t031_idle() has reset the chip to default. */
-        mt9t031->exposure = 255;
-        mt9t031->gain = 64;
+                v4l2_ctrl_handler_setup(&mt9t031->hdl);
 
         return ret;
 }
@@ -787,10 +674,14 @@ static int mt9t031_g_skip_top_lines(struct v4l2_subdev *sd, u32 *lines)
         return 0;
 }
 
+static const struct v4l2_ctrl_ops mt9t031_ctrl_ops = {
+        .g_volatile_ctrl = mt9t031_g_volatile_ctrl,
+        .s_ctrl = mt9t031_s_ctrl,
+};
+
 static struct v4l2_subdev_core_ops mt9t031_subdev_core_ops = {
-        .g_ctrl         = mt9t031_g_ctrl,
-        .s_ctrl         = mt9t031_s_ctrl,
         .g_chip_ident   = mt9t031_g_chip_ident,
+        .s_power        = mt9t031_s_power,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register     = mt9t031_g_register,
         .s_register     = mt9t031_s_register,
@@ -807,6 +698,34 @@ static int mt9t031_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
         return 0;
 }
 
+static int mt9t031_g_mbus_config(struct v4l2_subdev *sd,
+                                struct v4l2_mbus_config *cfg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+        cfg->flags = V4L2_MBUS_MASTER | V4L2_MBUS_PCLK_SAMPLE_RISING |
+                V4L2_MBUS_PCLK_SAMPLE_FALLING | V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+                V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_DATA_ACTIVE_HIGH;
+        cfg->type = V4L2_MBUS_PARALLEL;
+        cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+        return 0;
+}
+
+static int mt9t031_s_mbus_config(struct v4l2_subdev *sd,
+                                const struct v4l2_mbus_config *cfg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+        if (soc_camera_apply_board_flags(icl, cfg) &
+            V4L2_MBUS_PCLK_SAMPLE_FALLING)
+                return reg_clear(client, MT9T031_PIXEL_CLOCK_CONTROL, 0x8000);
+        else
+                return reg_set(client, MT9T031_PIXEL_CLOCK_CONTROL, 0x8000);
+}
+
 static struct v4l2_subdev_video_ops mt9t031_subdev_video_ops = {
         .s_stream       = mt9t031_s_stream,
         .s_mbus_fmt     = mt9t031_s_fmt,
@@ -816,6 +735,8 @@ static struct v4l2_subdev_video_ops mt9t031_subdev_video_ops = {
         .g_crop         = mt9t031_g_crop,
         .cropcap        = mt9t031_cropcap,
         .enum_mbus_fmt  = mt9t031_enum_fmt,
+        .g_mbus_config  = mt9t031_g_mbus_config,
+        .s_mbus_config  = mt9t031_s_mbus_config,
 };
 
 static struct v4l2_subdev_sensor_ops mt9t031_subdev_sensor_ops = {
@@ -832,18 +753,13 @@ static int mt9t031_probe(struct i2c_client *client,
                          const struct i2c_device_id *did)
 {
         struct mt9t031 *mt9t031;
-        struct soc_camera_device *icd = client->dev.platform_data;
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
         struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
         int ret;
 
-        if (icd) {
-                struct soc_camera_link *icl = to_soc_camera_link(icd);
-                if (!icl) {
-                        dev_err(&client->dev, "MT9T031 driver needs platform data\n");
-                        return -EINVAL;
-                }
-
-                icd->ops = &mt9t031_ops;
+        if (!icl) {
+                dev_err(&client->dev, "MT9T031 driver needs platform data\n");
+                return -EINVAL;
         }
 
         if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
@@ -857,6 +773,33 @@ static int mt9t031_probe(struct i2c_client *client,
                 return -ENOMEM;
 
         v4l2_i2c_subdev_init(&mt9t031->subdev, client, &mt9t031_subdev_ops);
+        v4l2_ctrl_handler_init(&mt9t031->hdl, 5);
+        v4l2_ctrl_new_std(&mt9t031->hdl, &mt9t031_ctrl_ops,
+                        V4L2_CID_VFLIP, 0, 1, 1, 0);
+        v4l2_ctrl_new_std(&mt9t031->hdl, &mt9t031_ctrl_ops,
+                        V4L2_CID_HFLIP, 0, 1, 1, 0);
+        v4l2_ctrl_new_std(&mt9t031->hdl, &mt9t031_ctrl_ops,
+                        V4L2_CID_GAIN, 0, 127, 1, 64);
+
+        /*
+         * Simulated autoexposure. If enabled, we calculate shutter width
+         * ourselves in the driver based on vertical blanking and frame width
+         */
+        mt9t031->autoexposure = v4l2_ctrl_new_std_menu(&mt9t031->hdl,
+                        &mt9t031_ctrl_ops, V4L2_CID_EXPOSURE_AUTO, 1, 0,
+                        V4L2_EXPOSURE_AUTO);
+        mt9t031->exposure = v4l2_ctrl_new_std(&mt9t031->hdl, &mt9t031_ctrl_ops,
+                        V4L2_CID_EXPOSURE, 1, 255, 1, 255);
+
+        mt9t031->subdev.ctrl_handler = &mt9t031->hdl;
+        if (mt9t031->hdl.error) {
+                int err = mt9t031->hdl.error;
+
+                kfree(mt9t031);
+                return err;
+        }
+        v4l2_ctrl_auto_cluster(2, &mt9t031->autoexposure,
+                                V4L2_EXPOSURE_MANUAL, true);
 
         mt9t031->y_skip_top     = 0;
         mt9t031->rect.left      = MT9T031_COLUMN_SKIP;
@@ -864,12 +807,6 @@ static int mt9t031_probe(struct i2c_client *client,
         mt9t031->rect.width     = MT9T031_MAX_WIDTH;
         mt9t031->rect.height    = MT9T031_MAX_HEIGHT;
 
-        /*
-         * Simulated autoexposure. If enabled, we calculate shutter width
-         * ourselves in the driver based on vertical blanking and frame width
-         */
-        mt9t031->autoexposure = 1;
-
         mt9t031->xskip = 1;
         mt9t031->yskip = 1;
 
@@ -880,8 +817,7 @@ static int mt9t031_probe(struct i2c_client *client,
         mt9t031_disable(client);
 
         if (ret) {
-                if (icd)
-                        icd->ops = NULL;
+                v4l2_ctrl_handler_free(&mt9t031->hdl);
                 kfree(mt9t031);
         }
 
@@ -891,10 +827,9 @@ static int mt9t031_probe(struct i2c_client *client,
 static int mt9t031_remove(struct i2c_client *client)
 {
         struct mt9t031 *mt9t031 = to_mt9t031(client);
-        struct soc_camera_device *icd = client->dev.platform_data;
 
-        if (icd)
-                icd->ops = NULL;
+        v4l2_device_unregister_subdev(&mt9t031->subdev);
+        v4l2_ctrl_handler_free(&mt9t031->hdl);
         kfree(mt9t031);
 
         return 0;
diff --git a/drivers/media/video/mt9t112.c b/drivers/media/video/mt9t112.c
index d2e0a50063a2..32114a3c0ca7 100644
--- a/drivers/media/video/mt9t112.c
+++ b/drivers/media/video/mt9t112.c
@@ -22,11 +22,11 @@
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/slab.h>
+#include <linux/v4l2-mediabus.h>
 #include <linux/videodev2.h>
 
 #include <media/mt9t112.h>
 #include <media/soc_camera.h>
-#include <media/soc_mediabus.h>
 #include <media/v4l2-chip-ident.h>
 #include <media/v4l2-common.h>
 
@@ -34,11 +34,7 @@
 /* #define EXT_CLOCK 24000000 */
 
 /************************************************************************
-
-
                         macro
-
-
 ************************************************************************/
 /*
  * frame size
@@ -80,17 +76,8 @@
 #define VAR8(id, offset) _VAR(id, offset, 0x8000)
 
 /************************************************************************
-
-
                         struct
-
-
 ************************************************************************/
-struct mt9t112_frame_size {
-        u16 width;
-        u16 height;
-};
-
 struct mt9t112_format {
         enum v4l2_mbus_pixelcode code;
         enum v4l2_colorspace colorspace;
@@ -102,21 +89,17 @@ struct mt9t112_priv {
         struct v4l2_subdev               subdev;
         struct mt9t112_camera_info      *info;
         struct i2c_client               *client;
-        struct soc_camera_device         icd;
-        struct mt9t112_frame_size        frame;
+        struct v4l2_rect                 frame;
         const struct mt9t112_format     *format;
         int                              model;
         u32                              flags;
 /* for flags */
-#define INIT_DONE  (1<<0)
+#define INIT_DONE       (1 << 0)
+#define PCLK_RISING     (1 << 1)
 };
 
 /************************************************************************
-
-
                         supported format
-
-
 ************************************************************************/
 
 static const struct mt9t112_format mt9t112_cfmts[] = {
@@ -154,11 +137,7 @@ static const struct mt9t112_format mt9t112_cfmts[] = {
 };
 
 /************************************************************************
-
-
                         general function
-
-
 ************************************************************************/
 static struct mt9t112_priv *to_mt9t112(const struct i2c_client *client)
 {
@@ -326,50 +305,47 @@ static int mt9t112_clock_info(const struct i2c_client *client, u32 ext)
         n = (n >> 8) & 0x003f;
 
         enable = ((6000 > ext) || (54000 < ext)) ? "X" : "";
-        dev_info(&client->dev, "EXTCLK          : %10u K %s\n", ext, enable);
+        dev_dbg(&client->dev, "EXTCLK          : %10u K %s\n", ext, enable);
 
         vco = 2 * m * ext / (n+1);
         enable = ((384000 > vco) || (768000 < vco)) ? "X" : "";
-        dev_info(&client->dev, "VCO             : %10u K %s\n", vco, enable);
+        dev_dbg(&client->dev, "VCO             : %10u K %s\n", vco, enable);
 
         clk = vco / (p1+1) / (p2+1);
         enable = (96000 < clk) ? "X" : "";
-        dev_info(&client->dev, "PIXCLK          : %10u K %s\n", clk, enable);
+        dev_dbg(&client->dev, "PIXCLK          : %10u K %s\n", clk, enable);
 
         clk = vco / (p3+1);
         enable = (768000 < clk) ? "X" : "";
-        dev_info(&client->dev, "MIPICLK         : %10u K %s\n", clk, enable);
+        dev_dbg(&client->dev, "MIPICLK         : %10u K %s\n", clk, enable);
 
         clk = vco / (p6+1);
         enable = (96000 < clk) ? "X" : "";
-        dev_info(&client->dev, "MCU CLK         : %10u K %s\n", clk, enable);
+        dev_dbg(&client->dev, "MCU CLK         : %10u K %s\n", clk, enable);
 
         clk = vco / (p5+1);
         enable = (54000 < clk) ? "X" : "";
-        dev_info(&client->dev, "SOC CLK         : %10u K %s\n", clk, enable);
+        dev_dbg(&client->dev, "SOC CLK         : %10u K %s\n", clk, enable);
 
         clk = vco / (p4+1);
         enable = (70000 < clk) ? "X" : "";
-        dev_info(&client->dev, "Sensor CLK      : %10u K %s\n", clk, enable);
+        dev_dbg(&client->dev, "Sensor CLK      : %10u K %s\n", clk, enable);
 
         clk = vco / (p7+1);
-        dev_info(&client->dev, "External sensor : %10u K\n", clk);
+        dev_dbg(&client->dev, "External sensor : %10u K\n", clk);
 
         clk = ext / (n+1);
         enable = ((2000 > clk) || (24000 < clk)) ? "X" : "";
-        dev_info(&client->dev, "PFD             : %10u K %s\n", clk, enable);
+        dev_dbg(&client->dev, "PFD             : %10u K %s\n", clk, enable);
 
         return 0;
 }
 #endif
 
-static void mt9t112_frame_check(u32 *width, u32 *height)
+static void mt9t112_frame_check(u32 *width, u32 *height, u32 *left, u32 *top)
 {
-        if (*width > MAX_WIDTH)
-                *width = MAX_WIDTH;
-
-        if (*height > MAX_HEIGHT)
-                *height = MAX_HEIGHT;
+        soc_camera_limit_side(left, width, 0, 0, MAX_WIDTH);
+        soc_camera_limit_side(top, height, 0, 0, MAX_HEIGHT);
 }
 
 static int mt9t112_set_a_frame_size(const struct i2c_client *client,
@@ -758,48 +734,7 @@ static int mt9t112_init_camera(const struct i2c_client *client)
 }
 
 /************************************************************************
-
-
-                        soc_camera_ops
-
-
-************************************************************************/
-static int mt9t112_set_bus_param(struct soc_camera_device *icd,
-                                 unsigned long  flags)
-{
-        return 0;
-}
-
-static unsigned long mt9t112_query_bus_param(struct soc_camera_device *icd)
-{
-        struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
-        struct mt9t112_priv *priv = to_mt9t112(client);
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-        unsigned long flags = SOCAM_MASTER | SOCAM_VSYNC_ACTIVE_HIGH |
-                SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_DATA_ACTIVE_HIGH;
-
-        flags |= (priv->info->flags & MT9T112_FLAG_PCLK_RISING_EDGE) ?
-                SOCAM_PCLK_SAMPLE_RISING : SOCAM_PCLK_SAMPLE_FALLING;
-
-        if (priv->info->flags & MT9T112_FLAG_DATAWIDTH_8)
-                flags |= SOCAM_DATAWIDTH_8;
-        else
-                flags |= SOCAM_DATAWIDTH_10;
-
-        return soc_camera_apply_sensor_flags(icl, flags);
-}
-
-static struct soc_camera_ops mt9t112_ops = {
-        .set_bus_param          = mt9t112_set_bus_param,
-        .query_bus_param        = mt9t112_query_bus_param,
-};
-
-/************************************************************************
-
-
                         v4l2_subdev_core_ops
-
-
 ************************************************************************/
 static int mt9t112_g_chip_ident(struct v4l2_subdev *sd,
                                 struct v4l2_dbg_chip_ident *id)
@@ -850,11 +785,7 @@ static struct v4l2_subdev_core_ops mt9t112_subdev_core_ops = {
 
 
 /************************************************************************
-
-
                         v4l2_subdev_video_ops
-
-
 ************************************************************************/
 static int mt9t112_s_stream(struct v4l2_subdev *sd, int enable)
 {
@@ -877,8 +808,7 @@ static int mt9t112_s_stream(struct v4l2_subdev *sd, int enable)
         }
 
         if (!(priv->flags & INIT_DONE)) {
-                u16 param = (MT9T112_FLAG_PCLK_RISING_EDGE &
-                             priv->info->flags) ? 0x0001 : 0x0000;
+                u16 param = PCLK_RISING & priv->flags ? 0x0001 : 0x0000;
 
                 ECHECKER(ret, mt9t112_init_camera(client));
 
@@ -910,19 +840,12 @@ static int mt9t112_s_stream(struct v4l2_subdev *sd, int enable)
         return ret;
 }
 
-static int mt9t112_set_params(struct i2c_client *client, u32 width, u32 height,
+static int mt9t112_set_params(struct mt9t112_priv *priv,
+                              const struct v4l2_rect *rect,
                               enum v4l2_mbus_pixelcode code)
 {
-        struct mt9t112_priv *priv = to_mt9t112(client);
         int i;
 
-        priv->format = NULL;
-
-        /*
-         * frame size check
-         */
-        mt9t112_frame_check(&width, &height);
-
         /*
          * get color format
          */
@@ -933,8 +856,13 @@ static int mt9t112_set_params(struct i2c_client *client, u32 width, u32 height,
         if (i == ARRAY_SIZE(mt9t112_cfmts))
                 return -EINVAL;
 
-        priv->frame.width  = (u16)width;
-        priv->frame.height = (u16)height;
+        priv->frame  = *rect;
+
+        /*
+         * frame size check
+         */
+        mt9t112_frame_check(&priv->frame.width, &priv->frame.height,
+                            &priv->frame.left, &priv->frame.top);
 
         priv->format = mt9t112_cfmts + i;
 
@@ -945,9 +873,12 @@ static int mt9t112_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
 {
         a->bounds.left                  = 0;
         a->bounds.top                   = 0;
-        a->bounds.width                 = VGA_WIDTH;
-        a->bounds.height                = VGA_HEIGHT;
-        a->defrect                      = a->bounds;
+        a->bounds.width                 = MAX_WIDTH;
+        a->bounds.height                = MAX_HEIGHT;
+        a->defrect.left                 = 0;
+        a->defrect.top                  = 0;
+        a->defrect.width                = VGA_WIDTH;
+        a->defrect.height               = VGA_HEIGHT;
         a->type                         = V4L2_BUF_TYPE_VIDEO_CAPTURE;
         a->pixelaspect.numerator        = 1;
         a->pixelaspect.denominator      = 1;
@@ -957,11 +888,11 @@ static int mt9t112_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
 
 static int mt9t112_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
 {
-        a->c.left       = 0;
-        a->c.top        = 0;
-        a->c.width      = VGA_WIDTH;
-        a->c.height     = VGA_HEIGHT;
-        a->type         = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct mt9t112_priv *priv = to_mt9t112(client);
+
+        a->c    = priv->frame;
+        a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
         return 0;
 }
@@ -969,10 +900,10 @@ static int mt9t112_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
 static int mt9t112_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
 {
         struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct mt9t112_priv *priv = to_mt9t112(client);
         struct v4l2_rect *rect = &a->c;
 
-        return mt9t112_set_params(client, rect->width, rect->height,
-                                 V4L2_MBUS_FMT_UYVY8_2X8);
+        return mt9t112_set_params(priv, rect, priv->format->code);
 }
 
 static int mt9t112_g_fmt(struct v4l2_subdev *sd,
@@ -981,16 +912,9 @@ static int mt9t112_g_fmt(struct v4l2_subdev *sd,
         struct i2c_client *client = v4l2_get_subdevdata(sd);
         struct mt9t112_priv *priv = to_mt9t112(client);
 
-        if (!priv->format) {
-                int ret = mt9t112_set_params(client, VGA_WIDTH, VGA_HEIGHT,
-                                             V4L2_MBUS_FMT_UYVY8_2X8);
-                if (ret < 0)
-                        return ret;
-        }
-
         mf->width       = priv->frame.width;
         mf->height      = priv->frame.height;
-        /* TODO: set colorspace */
+        mf->colorspace  = priv->format->colorspace;
         mf->code        = priv->format->code;
         mf->field       = V4L2_FIELD_NONE;
 
@@ -1001,17 +925,42 @@ static int mt9t112_s_fmt(struct v4l2_subdev *sd,
                          struct v4l2_mbus_framefmt *mf)
 {
         struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct mt9t112_priv *priv = to_mt9t112(client);
+        struct v4l2_rect rect = {
+                .width = mf->width,
+                .height = mf->height,
+                .left = priv->frame.left,
+                .top = priv->frame.top,
+        };
+        int ret;
+
+        ret = mt9t112_set_params(priv, &rect, mf->code);
+
+        if (!ret)
+                mf->colorspace = priv->format->colorspace;
 
-        /* TODO: set colorspace */
-        return mt9t112_set_params(client, mf->width, mf->height, mf->code);
+        return ret;
 }
 
 static int mt9t112_try_fmt(struct v4l2_subdev *sd,
                            struct v4l2_mbus_framefmt *mf)
 {
-        mt9t112_frame_check(&mf->width, &mf->height);
+        unsigned int top, left;
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(mt9t112_cfmts); i++)
+                if (mt9t112_cfmts[i].code == mf->code)
+                        break;
+
+        if (i == ARRAY_SIZE(mt9t112_cfmts)) {
+                mf->code = V4L2_MBUS_FMT_UYVY8_2X8;
+                mf->colorspace = V4L2_COLORSPACE_JPEG;
+        } else {
+                mf->colorspace  = mt9t112_cfmts[i].colorspace;
+        }
+
+        mt9t112_frame_check(&mf->width, &mf->height, &left, &top);
 
-        /* TODO: set colorspace */
         mf->field = V4L2_FIELD_NONE;
 
         return 0;
@@ -1024,6 +973,35 @@ static int mt9t112_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
                 return -EINVAL;
 
         *code = mt9t112_cfmts[index].code;
+
+        return 0;
+}
+
+static int mt9t112_g_mbus_config(struct v4l2_subdev *sd,
+                                 struct v4l2_mbus_config *cfg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+        cfg->flags = V4L2_MBUS_MASTER | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+                V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_DATA_ACTIVE_HIGH |
+                V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING;
+        cfg->type = V4L2_MBUS_PARALLEL;
+        cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+        return 0;
+}
+
+static int mt9t112_s_mbus_config(struct v4l2_subdev *sd,
+                                 const struct v4l2_mbus_config *cfg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+        struct mt9t112_priv *priv = to_mt9t112(client);
+
+        if (soc_camera_apply_board_flags(icl, cfg) & V4L2_MBUS_PCLK_SAMPLE_RISING)
+                priv->flags |= PCLK_RISING;
+
         return 0;
 }
 
@@ -1036,31 +1014,24 @@ static struct v4l2_subdev_video_ops mt9t112_subdev_video_ops = {
         .g_crop         = mt9t112_g_crop,
         .s_crop         = mt9t112_s_crop,
         .enum_mbus_fmt  = mt9t112_enum_fmt,
+        .g_mbus_config  = mt9t112_g_mbus_config,
+        .s_mbus_config  = mt9t112_s_mbus_config,
 };
 
 /************************************************************************
-
-
                         i2c driver
-
-
 ************************************************************************/
 static struct v4l2_subdev_ops mt9t112_subdev_ops = {
         .core   = &mt9t112_subdev_core_ops,
         .video  = &mt9t112_subdev_video_ops,
 };
 
-static int mt9t112_camera_probe(struct soc_camera_device *icd,
-                                struct i2c_client *client)
+static int mt9t112_camera_probe(struct i2c_client *client)
 {
         struct mt9t112_priv *priv = to_mt9t112(client);
         const char          *devname;
         int                  chipid;
 
-        /* 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
          */
@@ -1088,20 +1059,21 @@ static int mt9t112_camera_probe(struct soc_camera_device *icd,
 static int mt9t112_probe(struct i2c_client *client,
                          const struct i2c_device_id *did)
 {
-        struct mt9t112_priv        *priv;
-        struct soc_camera_device   *icd = client->dev.platform_data;
-        struct soc_camera_link     *icl;
-        int                         ret;
+        struct mt9t112_priv *priv;
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+        struct v4l2_rect rect = {
+                .width = VGA_WIDTH,
+                .height = VGA_HEIGHT,
+                .left = (MAX_WIDTH - VGA_WIDTH) / 2,
+                .top = (MAX_HEIGHT - VGA_HEIGHT) / 2,
+        };
+        int ret;
 
-        if (!icd) {
-                dev_err(&client->dev, "mt9t112: missing soc-camera data!\n");
+        if (!icl || !icl->priv) {
+                dev_err(&client->dev, "mt9t112: missing platform data!\n");
                 return -EINVAL;
         }
 
-        icl = to_soc_camera_link(icd);
-        if (!icl || !icl->priv)
-                return -EINVAL;
-
         priv = kzalloc(sizeof(*priv), GFP_KERNEL);
         if (!priv)
                 return -ENOMEM;
@@ -1110,13 +1082,12 @@ static int mt9t112_probe(struct i2c_client *client,
 
         v4l2_i2c_subdev_init(&priv->subdev, client, &mt9t112_subdev_ops);
 
-        icd->ops = &mt9t112_ops;
-
-        ret = mt9t112_camera_probe(icd, client);
-        if (ret) {
-                icd->ops = NULL;
+        ret = mt9t112_camera_probe(client);
+        if (ret)
                 kfree(priv);
-        }
+
+        /* Cannot fail: using the default supported pixel code */
+        mt9t112_set_params(priv, &rect, V4L2_MBUS_FMT_UYVY8_2X8);
 
         return ret;
 }
@@ -1124,9 +1095,7 @@ static int mt9t112_probe(struct i2c_client *client,
 static int mt9t112_remove(struct i2c_client *client)
 {
         struct mt9t112_priv *priv = to_mt9t112(client);
-        struct soc_camera_device *icd = client->dev.platform_data;
 
-        icd->ops = NULL;
         kfree(priv);
         return 0;
 }
@@ -1147,11 +1116,7 @@ static struct i2c_driver mt9t112_i2c_driver = {
 };
 
 /************************************************************************
-
-
                         module function
-
-
 ************************************************************************/
 static int __init mt9t112_module_init(void)
 {
diff --git a/drivers/media/video/mt9v022.c b/drivers/media/video/mt9v022.c
index 51b0fccbfe70..b6a29f7de82c 100644
--- a/drivers/media/video/mt9v022.c
+++ b/drivers/media/video/mt9v022.c
@@ -14,9 +14,11 @@
 #include <linux/delay.h>
 #include <linux/log2.h>
 
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-chip-ident.h>
-#include <media/soc_camera.h>
+#include <media/v4l2-ctrls.h>
 
 /*
  * mt9v022 i2c address 0x48, 0x4c, 0x58, 0x5c
@@ -100,6 +102,17 @@ static const struct mt9v022_datafmt mt9v022_monochrome_fmts[] = {
 
 struct mt9v022 {
         struct v4l2_subdev subdev;
+        struct v4l2_ctrl_handler hdl;
+        struct {
+                /* exposure/auto-exposure cluster */
+                struct v4l2_ctrl *autoexposure;
+                struct v4l2_ctrl *exposure;
+        };
+        struct {
+                /* gain/auto-gain cluster */
+                struct v4l2_ctrl *autogain;
+                struct v4l2_ctrl *gain;
+        };
         struct v4l2_rect rect;  /* Sensor window */
         const struct mt9v022_datafmt *fmt;
         const struct mt9v022_datafmt *fmts;
@@ -178,6 +191,8 @@ static int mt9v022_init(struct i2c_client *client)
                 ret = reg_clear(client, MT9V022_BLACK_LEVEL_CALIB_CTRL, 1);
         if (!ret)
                 ret = reg_write(client, MT9V022_DIGITAL_TEST_PATTERN, 0);
+        if (!ret)
+                return v4l2_ctrl_handler_setup(&mt9v022->hdl);
 
         return ret;
 }
@@ -199,78 +214,6 @@ static int mt9v022_s_stream(struct v4l2_subdev *sd, int enable)
         return 0;
 }
 
-static int mt9v022_set_bus_param(struct soc_camera_device *icd,
-                                 unsigned long flags)
-{
-        struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
-        struct mt9v022 *mt9v022 = to_mt9v022(client);
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-        unsigned int width_flag = flags & SOCAM_DATAWIDTH_MASK;
-        int ret;
-        u16 pixclk = 0;
-
-        /* Only one width bit may be set */
-        if (!is_power_of_2(width_flag))
-                return -EINVAL;
-
-        if (icl->set_bus_param) {
-                ret = icl->set_bus_param(icl, width_flag);
-                if (ret)
-                        return ret;
-        } else {
-                /*
-                 * Without board specific bus width settings we only support the
-                 * sensors native bus width
-                 */
-                if (width_flag != SOCAM_DATAWIDTH_10)
-                        return -EINVAL;
-        }
-
-        flags = soc_camera_apply_sensor_flags(icl, flags);
-
-        if (flags & SOCAM_PCLK_SAMPLE_FALLING)
-                pixclk |= 0x10;
-
-        if (!(flags & SOCAM_HSYNC_ACTIVE_HIGH))
-                pixclk |= 0x1;
-
-        if (!(flags & SOCAM_VSYNC_ACTIVE_HIGH))
-                pixclk |= 0x2;
-
-        ret = reg_write(client, MT9V022_PIXCLK_FV_LV, pixclk);
-        if (ret < 0)
-                return ret;
-
-        if (!(flags & SOCAM_MASTER))
-                mt9v022->chip_control &= ~0x8;
-
-        ret = reg_write(client, MT9V022_CHIP_CONTROL, mt9v022->chip_control);
-        if (ret < 0)
-                return ret;
-
-        dev_dbg(&client->dev, "Calculated pixclk 0x%x, chip control 0x%x\n",
-                pixclk, mt9v022->chip_control);
-
-        return 0;
-}
-
-static unsigned long mt9v022_query_bus_param(struct soc_camera_device *icd)
-{
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-        unsigned int flags = SOCAM_MASTER | SOCAM_SLAVE |
-                SOCAM_PCLK_SAMPLE_RISING | SOCAM_PCLK_SAMPLE_FALLING |
-                SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_LOW |
-                SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_LOW |
-                SOCAM_DATA_ACTIVE_HIGH;
-
-        if (icl->query_bus_param)
-                flags |= icl->query_bus_param(icl) & SOCAM_DATAWIDTH_MASK;
-        else
-                flags |= SOCAM_DATAWIDTH_10;
-
-        return soc_camera_apply_sensor_flags(icl, flags);
-}
-
 static int mt9v022_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
 {
         struct i2c_client *client = v4l2_get_subdevdata(sd);
@@ -389,7 +332,7 @@ static int mt9v022_s_fmt(struct v4l2_subdev *sd,
 
         /*
          * The caller provides a supported format, as verified per call to
-         * icd->try_fmt(), datawidth is from our supported format list
+         * .try_mbus_fmt(), datawidth is from our supported format list
          */
         switch (mf->code) {
         case V4L2_MBUS_FMT_Y8_1X8:
@@ -502,236 +445,131 @@ static int mt9v022_s_register(struct v4l2_subdev *sd,
 }
 #endif
 
-static const struct v4l2_queryctrl mt9v022_controls[] = {
-        {
-                .id             = V4L2_CID_VFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Vertically",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        }, {
-                .id             = V4L2_CID_HFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Horizontally",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        }, {
-                .id             = V4L2_CID_GAIN,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Analog Gain",
-                .minimum        = 64,
-                .maximum        = 127,
-                .step           = 1,
-                .default_value  = 64,
-                .flags          = V4L2_CTRL_FLAG_SLIDER,
-        }, {
-                .id             = V4L2_CID_EXPOSURE,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Exposure",
-                .minimum        = 1,
-                .maximum        = 255,
-                .step           = 1,
-                .default_value  = 255,
-                .flags          = V4L2_CTRL_FLAG_SLIDER,
-        }, {
-                .id             = V4L2_CID_AUTOGAIN,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Automatic Gain",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 1,
-        }, {
-                .id             = V4L2_CID_EXPOSURE_AUTO,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Automatic Exposure",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 1,
-        }
-};
-
-static struct soc_camera_ops mt9v022_ops = {
-        .set_bus_param          = mt9v022_set_bus_param,
-        .query_bus_param        = mt9v022_query_bus_param,
-        .controls               = mt9v022_controls,
-        .num_controls           = ARRAY_SIZE(mt9v022_controls),
-};
-
-static int mt9v022_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int mt9v022_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 {
+        struct mt9v022 *mt9v022 = container_of(ctrl->handler,
+                                               struct mt9v022, hdl);
+        struct v4l2_subdev *sd = &mt9v022->subdev;
         struct i2c_client *client = v4l2_get_subdevdata(sd);
-        const struct v4l2_queryctrl *qctrl;
+        struct v4l2_ctrl *gain = mt9v022->gain;
+        struct v4l2_ctrl *exp = mt9v022->exposure;
         unsigned long range;
         int data;
 
-        qctrl = soc_camera_find_qctrl(&mt9v022_ops, ctrl->id);
-
         switch (ctrl->id) {
-        case V4L2_CID_VFLIP:
-                data = reg_read(client, MT9V022_READ_MODE);
-                if (data < 0)
-                        return -EIO;
-                ctrl->value = !!(data & 0x10);
-                break;
-        case V4L2_CID_HFLIP:
-                data = reg_read(client, MT9V022_READ_MODE);
-                if (data < 0)
-                        return -EIO;
-                ctrl->value = !!(data & 0x20);
-                break;
-        case V4L2_CID_EXPOSURE_AUTO:
-                data = reg_read(client, MT9V022_AEC_AGC_ENABLE);
-                if (data < 0)
-                        return -EIO;
-                ctrl->value = !!(data & 0x1);
-                break;
         case V4L2_CID_AUTOGAIN:
-                data = reg_read(client, MT9V022_AEC_AGC_ENABLE);
-                if (data < 0)
-                        return -EIO;
-                ctrl->value = !!(data & 0x2);
-                break;
-        case V4L2_CID_GAIN:
                 data = reg_read(client, MT9V022_ANALOG_GAIN);
                 if (data < 0)
                         return -EIO;
 
-                range = qctrl->maximum - qctrl->minimum;
-                ctrl->value = ((data - 16) * range + 24) / 48 + qctrl->minimum;
-
-                break;
-        case V4L2_CID_EXPOSURE:
+                range = gain->maximum - gain->minimum;
+                gain->val = ((data - 16) * range + 24) / 48 + gain->minimum;
+                return 0;
+        case V4L2_CID_EXPOSURE_AUTO:
                 data = reg_read(client, MT9V022_TOTAL_SHUTTER_WIDTH);
                 if (data < 0)
                         return -EIO;
 
-                range = qctrl->maximum - qctrl->minimum;
-                ctrl->value = ((data - 1) * range + 239) / 479 + qctrl->minimum;
-
-                break;
+                range = exp->maximum - exp->minimum;
+                exp->val = ((data - 1) * range + 239) / 479 + exp->minimum;
+                return 0;
         }
-        return 0;
+        return -EINVAL;
 }
 
-static int mt9v022_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int mt9v022_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-        int data;
+        struct mt9v022 *mt9v022 = container_of(ctrl->handler,
+                                               struct mt9v022, hdl);
+        struct v4l2_subdev *sd = &mt9v022->subdev;
         struct i2c_client *client = v4l2_get_subdevdata(sd);
-        const struct v4l2_queryctrl *qctrl;
-
-        qctrl = soc_camera_find_qctrl(&mt9v022_ops, ctrl->id);
-        if (!qctrl)
-                return -EINVAL;
+        int data;
 
         switch (ctrl->id) {
         case V4L2_CID_VFLIP:
-                if (ctrl->value)
+                if (ctrl->val)
                         data = reg_set(client, MT9V022_READ_MODE, 0x10);
                 else
                         data = reg_clear(client, MT9V022_READ_MODE, 0x10);
                 if (data < 0)
                         return -EIO;
-                break;
+                return 0;
         case V4L2_CID_HFLIP:
-                if (ctrl->value)
+                if (ctrl->val)
                         data = reg_set(client, MT9V022_READ_MODE, 0x20);
                 else
                         data = reg_clear(client, MT9V022_READ_MODE, 0x20);
                 if (data < 0)
                         return -EIO;
-                break;
-        case V4L2_CID_GAIN:
-                /* mt9v022 has minimum == default */
-                if (ctrl->value > qctrl->maximum || ctrl->value < qctrl->minimum)
-                        return -EINVAL;
-                else {
-                        unsigned long range = qctrl->maximum - qctrl->minimum;
+                return 0;
+        case V4L2_CID_AUTOGAIN:
+                if (ctrl->val) {
+                        if (reg_set(client, MT9V022_AEC_AGC_ENABLE, 0x2) < 0)
+                                return -EIO;
+                } else {
+                        struct v4l2_ctrl *gain = mt9v022->gain;
+                        /* mt9v022 has minimum == default */
+                        unsigned long range = gain->maximum - gain->minimum;
                         /* Valid values 16 to 64, 32 to 64 must be even. */
-                        unsigned long gain = ((ctrl->value - qctrl->minimum) *
+                        unsigned long gain_val = ((gain->val - gain->minimum) *
                                               48 + range / 2) / range + 16;
-                        if (gain >= 32)
-                                gain &= ~1;
+
+                        if (gain_val >= 32)
+                                gain_val &= ~1;
+
                         /*
                          * The user wants to set gain manually, hope, she
                          * knows, what she's doing... Switch AGC off.
                          */
-
                         if (reg_clear(client, MT9V022_AEC_AGC_ENABLE, 0x2) < 0)
                                 return -EIO;
 
                         dev_dbg(&client->dev, "Setting gain from %d to %lu\n",
-                                reg_read(client, MT9V022_ANALOG_GAIN), gain);
-                        if (reg_write(client, MT9V022_ANALOG_GAIN, gain) < 0)
+                                reg_read(client, MT9V022_ANALOG_GAIN), gain_val);
+                        if (reg_write(client, MT9V022_ANALOG_GAIN, gain_val) < 0)
                                 return -EIO;
                 }
-                break;
-        case V4L2_CID_EXPOSURE:
-                /* mt9v022 has maximum == default */
-                if (ctrl->value > qctrl->maximum || ctrl->value < qctrl->minimum)
-                        return -EINVAL;
-                else {
-                        unsigned long range = qctrl->maximum - qctrl->minimum;
-                        unsigned long shutter = ((ctrl->value - qctrl->minimum) *
-                                                 479 + range / 2) / range + 1;
+                return 0;
+        case V4L2_CID_EXPOSURE_AUTO:
+                if (ctrl->val == V4L2_EXPOSURE_AUTO) {
+                        data = reg_set(client, MT9V022_AEC_AGC_ENABLE, 0x1);
+                } else {
+                        struct v4l2_ctrl *exp = mt9v022->exposure;
+                        unsigned long range = exp->maximum - exp->minimum;
+                        unsigned long shutter = ((exp->val - exp->minimum) *
+                                        479 + range / 2) / range + 1;
+
                         /*
                          * The user wants to set shutter width manually, hope,
                          * she knows, what she's doing... Switch AEC off.
                          */
-
-                        if (reg_clear(client, MT9V022_AEC_AGC_ENABLE, 0x1) < 0)
+                        data = reg_clear(client, MT9V022_AEC_AGC_ENABLE, 0x1);
+                        if (data < 0)
                                 return -EIO;
-
                         dev_dbg(&client->dev, "Shutter width from %d to %lu\n",
-                                reg_read(client, MT9V022_TOTAL_SHUTTER_WIDTH),
-                                shutter);
+                                        reg_read(client, MT9V022_TOTAL_SHUTTER_WIDTH),
+                                        shutter);
                         if (reg_write(client, MT9V022_TOTAL_SHUTTER_WIDTH,
-                                      shutter) < 0)
+                                                shutter) < 0)
                                 return -EIO;
                 }
-                break;
-        case V4L2_CID_AUTOGAIN:
-                if (ctrl->value)
-                        data = reg_set(client, MT9V022_AEC_AGC_ENABLE, 0x2);
-                else
-                        data = reg_clear(client, MT9V022_AEC_AGC_ENABLE, 0x2);
-                if (data < 0)
-                        return -EIO;
-                break;
-        case V4L2_CID_EXPOSURE_AUTO:
-                if (ctrl->value)
-                        data = reg_set(client, MT9V022_AEC_AGC_ENABLE, 0x1);
-                else
-                        data = reg_clear(client, MT9V022_AEC_AGC_ENABLE, 0x1);
-                if (data < 0)
-                        return -EIO;
-                break;
+                return 0;
         }
-        return 0;
+        return -EINVAL;
 }
 
 /*
  * Interface active, can use i2c. If it fails, it can indeed mean, that
  * this wasn't our capture interface, so, we wait for the right one
  */
-static int mt9v022_video_probe(struct soc_camera_device *icd,
-                               struct i2c_client *client)
+static int mt9v022_video_probe(struct i2c_client *client)
 {
         struct mt9v022 *mt9v022 = to_mt9v022(client);
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
         s32 data;
         int ret;
         unsigned long flags;
 
-        /* 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);
 
@@ -805,16 +643,6 @@ ei2c:
         return ret;
 }
 
-static void mt9v022_video_remove(struct soc_camera_device *icd)
-{
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-
-        dev_dbg(icd->pdev, "Video removed: %p, %p\n",
-                icd->parent, icd->vdev);
-        if (icl->free_bus)
-                icl->free_bus(icl);
-}
-
 static int mt9v022_g_skip_top_lines(struct v4l2_subdev *sd, u32 *lines)
 {
         struct i2c_client *client = v4l2_get_subdevdata(sd);
@@ -825,9 +653,12 @@ static int mt9v022_g_skip_top_lines(struct v4l2_subdev *sd, u32 *lines)
         return 0;
 }
 
+static const struct v4l2_ctrl_ops mt9v022_ctrl_ops = {
+        .g_volatile_ctrl = mt9v022_g_volatile_ctrl,
+        .s_ctrl = mt9v022_s_ctrl,
+};
+
 static struct v4l2_subdev_core_ops mt9v022_subdev_core_ops = {
-        .g_ctrl         = mt9v022_g_ctrl,
-        .s_ctrl         = mt9v022_s_ctrl,
         .g_chip_ident   = mt9v022_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register     = mt9v022_g_register,
@@ -848,6 +679,72 @@ static int mt9v022_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
         return 0;
 }
 
+static int mt9v022_g_mbus_config(struct v4l2_subdev *sd,
+                                struct v4l2_mbus_config *cfg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+        cfg->flags = V4L2_MBUS_MASTER | V4L2_MBUS_SLAVE |
+                V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING |
+                V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_LOW |
+                V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_LOW |
+                V4L2_MBUS_DATA_ACTIVE_HIGH;
+        cfg->type = V4L2_MBUS_PARALLEL;
+        cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+        return 0;
+}
+
+static int mt9v022_s_mbus_config(struct v4l2_subdev *sd,
+                                 const struct v4l2_mbus_config *cfg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+        struct mt9v022 *mt9v022 = to_mt9v022(client);
+        unsigned long flags = soc_camera_apply_board_flags(icl, cfg);
+        unsigned int bps = soc_mbus_get_fmtdesc(mt9v022->fmt->code)->bits_per_sample;
+        int ret;
+        u16 pixclk = 0;
+
+        if (icl->set_bus_param) {
+                ret = icl->set_bus_param(icl, 1 << (bps - 1));
+                if (ret)
+                        return ret;
+        } else if (bps != 10) {
+                /*
+                 * Without board specific bus width settings we only support the
+                 * sensors native bus width
+                 */
+                return -EINVAL;
+        }
+
+        if (flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
+                pixclk |= 0x10;
+
+        if (!(flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH))
+                pixclk |= 0x1;
+
+        if (!(flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH))
+                pixclk |= 0x2;
+
+        ret = reg_write(client, MT9V022_PIXCLK_FV_LV, pixclk);
+        if (ret < 0)
+                return ret;
+
+        if (!(flags & V4L2_MBUS_MASTER))
+                mt9v022->chip_control &= ~0x8;
+
+        ret = reg_write(client, MT9V022_CHIP_CONTROL, mt9v022->chip_control);
+        if (ret < 0)
+                return ret;
+
+        dev_dbg(&client->dev, "Calculated pixclk 0x%x, chip control 0x%x\n",
+                pixclk, mt9v022->chip_control);
+
+        return 0;
+}
+
 static struct v4l2_subdev_video_ops mt9v022_subdev_video_ops = {
         .s_stream       = mt9v022_s_stream,
         .s_mbus_fmt     = mt9v022_s_fmt,
@@ -857,6 +754,8 @@ static struct v4l2_subdev_video_ops mt9v022_subdev_video_ops = {
         .g_crop         = mt9v022_g_crop,
         .cropcap        = mt9v022_cropcap,
         .enum_mbus_fmt  = mt9v022_enum_fmt,
+        .g_mbus_config  = mt9v022_g_mbus_config,
+        .s_mbus_config  = mt9v022_s_mbus_config,
 };
 
 static struct v4l2_subdev_sensor_ops mt9v022_subdev_sensor_ops = {
@@ -873,17 +772,10 @@ static int mt9v022_probe(struct i2c_client *client,
                          const struct i2c_device_id *did)
 {
         struct mt9v022 *mt9v022;
-        struct soc_camera_device *icd = client->dev.platform_data;
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
         struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
-        struct soc_camera_link *icl;
         int ret;
 
-        if (!icd) {
-                dev_err(&client->dev, "MT9V022: missing soc-camera data!\n");
-                return -EINVAL;
-        }
-
-        icl = to_soc_camera_link(icd);
         if (!icl) {
                 dev_err(&client->dev, "MT9V022 driver needs platform data\n");
                 return -EINVAL;
@@ -900,10 +792,39 @@ static int mt9v022_probe(struct i2c_client *client,
                 return -ENOMEM;
 
         v4l2_i2c_subdev_init(&mt9v022->subdev, client, &mt9v022_subdev_ops);
+        v4l2_ctrl_handler_init(&mt9v022->hdl, 6);
+        v4l2_ctrl_new_std(&mt9v022->hdl, &mt9v022_ctrl_ops,
+                        V4L2_CID_VFLIP, 0, 1, 1, 0);
+        v4l2_ctrl_new_std(&mt9v022->hdl, &mt9v022_ctrl_ops,
+                        V4L2_CID_HFLIP, 0, 1, 1, 0);
+        mt9v022->autogain = v4l2_ctrl_new_std(&mt9v022->hdl, &mt9v022_ctrl_ops,
+                        V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+        mt9v022->gain = v4l2_ctrl_new_std(&mt9v022->hdl, &mt9v022_ctrl_ops,
+                        V4L2_CID_GAIN, 0, 127, 1, 64);
+
+        /*
+         * Simulated autoexposure. If enabled, we calculate shutter width
+         * ourselves in the driver based on vertical blanking and frame width
+         */
+        mt9v022->autoexposure = v4l2_ctrl_new_std_menu(&mt9v022->hdl,
+                        &mt9v022_ctrl_ops, V4L2_CID_EXPOSURE_AUTO, 1, 0,
+                        V4L2_EXPOSURE_AUTO);
+        mt9v022->exposure = v4l2_ctrl_new_std(&mt9v022->hdl, &mt9v022_ctrl_ops,
+                        V4L2_CID_EXPOSURE, 1, 255, 1, 255);
+
+        mt9v022->subdev.ctrl_handler = &mt9v022->hdl;
+        if (mt9v022->hdl.error) {
+                int err = mt9v022->hdl.error;
+
+                kfree(mt9v022);
+                return err;
+        }
+        v4l2_ctrl_auto_cluster(2, &mt9v022->autoexposure,
+                                V4L2_EXPOSURE_MANUAL, true);
+        v4l2_ctrl_auto_cluster(2, &mt9v022->autogain, 0, true);
 
         mt9v022->chip_control = MT9V022_CHIP_CONTROL_DEFAULT;
 
-        icd->ops                = &mt9v022_ops;
         /*
          * MT9V022 _really_ corrupts the first read out line.
          * TODO: verify on i.MX31
@@ -914,9 +835,9 @@ static int mt9v022_probe(struct i2c_client *client,
         mt9v022->rect.width     = MT9V022_MAX_WIDTH;
         mt9v022->rect.height    = MT9V022_MAX_HEIGHT;
 
-        ret = mt9v022_video_probe(icd, client);
+        ret = mt9v022_video_probe(client);
         if (ret) {
-                icd->ops = NULL;
+                v4l2_ctrl_handler_free(&mt9v022->hdl);
                 kfree(mt9v022);
         }
 
@@ -926,10 +847,12 @@ static int mt9v022_probe(struct i2c_client *client,
 static int mt9v022_remove(struct i2c_client *client)
 {
         struct mt9v022 *mt9v022 = to_mt9v022(client);
-        struct soc_camera_device *icd = client->dev.platform_data;
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
 
-        icd->ops = NULL;
-        mt9v022_video_remove(icd);
+        v4l2_device_unregister_subdev(&mt9v022->subdev);
+        if (icl->free_bus)
+                icl->free_bus(icl);
+        v4l2_ctrl_handler_free(&mt9v022->hdl);
         kfree(mt9v022);
 
         return 0;
diff --git a/drivers/media/video/mx1_camera.c b/drivers/media/video/mx1_camera.c
index 087db12a3a67..18e94c7d2be8 100644
--- a/drivers/media/video/mx1_camera.c
+++ b/drivers/media/video/mx1_camera.c
@@ -78,11 +78,10 @@
 #define CSI_IRQ_MASK    (CSISR_SFF_OR_INT | CSISR_RFF_OR_INT | \
                         CSISR_STATFF_INT | CSISR_RXFF_INT | CSISR_SOF_INT)
 
-#define CSI_BUS_FLAGS   (SOCAM_MASTER | SOCAM_HSYNC_ACTIVE_HIGH | \
-                        SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_LOW | \
-                        SOCAM_PCLK_SAMPLE_RISING | SOCAM_PCLK_SAMPLE_FALLING | \
-                        SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATA_ACTIVE_LOW | \
-                        SOCAM_DATAWIDTH_8)
+#define CSI_BUS_FLAGS   (V4L2_MBUS_MASTER | V4L2_MBUS_HSYNC_ACTIVE_HIGH | \
+                        V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_LOW | \
+                        V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING | \
+                        V4L2_MBUS_DATA_ACTIVE_HIGH | V4L2_MBUS_DATA_ACTIVE_LOW)
 
 #define MAX_VIDEO_MEM 16        /* Video memory limit in megabytes */
 
@@ -490,59 +489,73 @@ 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 v4l2_subdev *sd = soc_camera_to_subdev(icd);
         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx1_camera_dev *pcdev = ici->priv;
-        unsigned long camera_flags, common_flags;
+        struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+        unsigned long common_flags;
         unsigned int csicr1;
         int ret;
 
-        camera_flags = icd->ops->query_bus_param(icd);
-
         /* MX1 supports only 8bit buswidth */
-        common_flags = soc_camera_bus_param_compatible(camera_flags,
-                                                       CSI_BUS_FLAGS);
-        if (!common_flags)
-                return -EINVAL;
+        ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+        if (!ret) {
+                common_flags = soc_mbus_config_compatible(&cfg, CSI_BUS_FLAGS);
+                if (!common_flags) {
+                        dev_warn(icd->parent,
+                                 "Flags incompatible: camera 0x%x, host 0x%x\n",
+                                 cfg.flags, CSI_BUS_FLAGS);
+                        return -EINVAL;
+                }
+        } else if (ret != -ENOIOCTLCMD) {
+                return ret;
+        } else {
+                common_flags = CSI_BUS_FLAGS;
+        }
 
         /* Make choises, based on platform choice */
-        if ((common_flags & SOCAM_VSYNC_ACTIVE_HIGH) &&
-                (common_flags & SOCAM_VSYNC_ACTIVE_LOW)) {
+        if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
+                (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
                         if (!pcdev->pdata ||
                              pcdev->pdata->flags & MX1_CAMERA_VSYNC_HIGH)
-                                common_flags &= ~SOCAM_VSYNC_ACTIVE_LOW;
+                                common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
                         else
-                                common_flags &= ~SOCAM_VSYNC_ACTIVE_HIGH;
+                                common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
         }
 
-        if ((common_flags & SOCAM_PCLK_SAMPLE_RISING) &&
-                (common_flags & SOCAM_PCLK_SAMPLE_FALLING)) {
+        if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
+                (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
                         if (!pcdev->pdata ||
                              pcdev->pdata->flags & MX1_CAMERA_PCLK_RISING)
-                                common_flags &= ~SOCAM_PCLK_SAMPLE_FALLING;
+                                common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
                         else
-                                common_flags &= ~SOCAM_PCLK_SAMPLE_RISING;
+                                common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
         }
 
-        if ((common_flags & SOCAM_DATA_ACTIVE_HIGH) &&
-                (common_flags & SOCAM_DATA_ACTIVE_LOW)) {
+        if ((common_flags & V4L2_MBUS_DATA_ACTIVE_HIGH) &&
+                (common_flags & V4L2_MBUS_DATA_ACTIVE_LOW)) {
                         if (!pcdev->pdata ||
                              pcdev->pdata->flags & MX1_CAMERA_DATA_HIGH)
-                                common_flags &= ~SOCAM_DATA_ACTIVE_LOW;
+                                common_flags &= ~V4L2_MBUS_DATA_ACTIVE_LOW;
                         else
-                                common_flags &= ~SOCAM_DATA_ACTIVE_HIGH;
+                                common_flags &= ~V4L2_MBUS_DATA_ACTIVE_HIGH;
         }
 
-        ret = icd->ops->set_bus_param(icd, common_flags);
-        if (ret < 0)
+        cfg.flags = common_flags;
+        ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
+        if (ret < 0 && ret != -ENOIOCTLCMD) {
+                dev_dbg(icd->parent, "camera s_mbus_config(0x%lx) returned %d\n",
+                        common_flags, ret);
                 return ret;
+        }
 
         csicr1 = __raw_readl(pcdev->base + CSICR1);
 
-        if (common_flags & SOCAM_PCLK_SAMPLE_RISING)
+        if (common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
                 csicr1 |= CSICR1_REDGE;
-        if (common_flags & SOCAM_VSYNC_ACTIVE_HIGH)
+        if (common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
                 csicr1 |= CSICR1_SOF_POL;
-        if (common_flags & SOCAM_DATA_ACTIVE_LOW)
+        if (common_flags & V4L2_MBUS_DATA_ACTIVE_LOW)
                 csicr1 |= CSICR1_DATA_POL;
 
         __raw_writel(csicr1, pcdev->base + CSICR1);
diff --git a/drivers/media/video/mx2_camera.c b/drivers/media/video/mx2_camera.c
index ec2410c0c806..a803d9ea8fd6 100644
--- a/drivers/media/video/mx2_camera.c
+++ b/drivers/media/video/mx2_camera.c
@@ -686,16 +686,15 @@ static void mx2_camera_init_videobuf(struct videobuf_queue *q,
                         icd, &icd->video_lock);
 }
 
-#define MX2_BUS_FLAGS   (SOCAM_DATAWIDTH_8 | \
-                        SOCAM_MASTER | \
-                        SOCAM_VSYNC_ACTIVE_HIGH | \
-                        SOCAM_VSYNC_ACTIVE_LOW | \
-                        SOCAM_HSYNC_ACTIVE_HIGH | \
-                        SOCAM_HSYNC_ACTIVE_LOW | \
-                        SOCAM_PCLK_SAMPLE_RISING | \
-                        SOCAM_PCLK_SAMPLE_FALLING | \
-                        SOCAM_DATA_ACTIVE_HIGH | \
-                        SOCAM_DATA_ACTIVE_LOW)
+#define MX2_BUS_FLAGS   (V4L2_MBUS_MASTER | \
+                        V4L2_MBUS_VSYNC_ACTIVE_HIGH | \
+                        V4L2_MBUS_VSYNC_ACTIVE_LOW | \
+                        V4L2_MBUS_HSYNC_ACTIVE_HIGH | \
+                        V4L2_MBUS_HSYNC_ACTIVE_LOW | \
+                        V4L2_MBUS_PCLK_SAMPLE_RISING | \
+                        V4L2_MBUS_PCLK_SAMPLE_FALLING | \
+                        V4L2_MBUS_DATA_ACTIVE_HIGH | \
+                        V4L2_MBUS_DATA_ACTIVE_LOW)
 
 static int mx27_camera_emma_prp_reset(struct mx2_camera_dev *pcdev)
 {
@@ -770,46 +769,59 @@ static void mx27_camera_emma_buf_init(struct soc_camera_device *icd,
 static int mx2_camera_set_bus_param(struct soc_camera_device *icd,
                 __u32 pixfmt)
 {
-        struct soc_camera_host *ici =
-                to_soc_camera_host(icd->parent);
+        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx2_camera_dev *pcdev = ici->priv;
-        unsigned long camera_flags, common_flags;
-        int ret = 0;
+        struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+        unsigned long common_flags;
+        int ret;
         int bytesperline;
         u32 csicr1 = pcdev->csicr1;
 
-        camera_flags = icd->ops->query_bus_param(icd);
-
-        common_flags = soc_camera_bus_param_compatible(camera_flags,
-                                MX2_BUS_FLAGS);
-        if (!common_flags)
-                return -EINVAL;
+        ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+        if (!ret) {
+                common_flags = soc_mbus_config_compatible(&cfg, MX2_BUS_FLAGS);
+                if (!common_flags) {
+                        dev_warn(icd->parent,
+                                 "Flags incompatible: camera 0x%x, host 0x%x\n",
+                                 cfg.flags, MX2_BUS_FLAGS);
+                        return -EINVAL;
+                }
+        } else if (ret != -ENOIOCTLCMD) {
+                return ret;
+        } else {
+                common_flags = MX2_BUS_FLAGS;
+        }
 
-        if ((common_flags & SOCAM_HSYNC_ACTIVE_HIGH) &&
-            (common_flags & SOCAM_HSYNC_ACTIVE_LOW)) {
+        if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
+            (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
                 if (pcdev->platform_flags & MX2_CAMERA_HSYNC_HIGH)
-                        common_flags &= ~SOCAM_HSYNC_ACTIVE_LOW;
+                        common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
                 else
-                        common_flags &= ~SOCAM_HSYNC_ACTIVE_HIGH;
+                        common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
         }
 
-        if ((common_flags & SOCAM_PCLK_SAMPLE_RISING) &&
-            (common_flags & SOCAM_PCLK_SAMPLE_FALLING)) {
+        if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
+            (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
                 if (pcdev->platform_flags & MX2_CAMERA_PCLK_SAMPLE_RISING)
-                        common_flags &= ~SOCAM_PCLK_SAMPLE_FALLING;
+                        common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
                 else
-                        common_flags &= ~SOCAM_PCLK_SAMPLE_RISING;
+                        common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
         }
 
-        ret = icd->ops->set_bus_param(icd, common_flags);
-        if (ret < 0)
+        cfg.flags = common_flags;
+        ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
+        if (ret < 0 && ret != -ENOIOCTLCMD) {
+                dev_dbg(icd->parent, "camera s_mbus_config(0x%lx) returned %d\n",
+                        common_flags, ret);
                 return ret;
+        }
 
-        if (common_flags & SOCAM_PCLK_SAMPLE_RISING)
+        if (common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
                 csicr1 |= CSICR1_REDGE;
-        if (common_flags & SOCAM_VSYNC_ACTIVE_HIGH)
+        if (common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
                 csicr1 |= CSICR1_SOF_POL;
-        if (common_flags & SOCAM_HSYNC_ACTIVE_HIGH)
+        if (common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
                 csicr1 |= CSICR1_HSYNC_POL;
         if (pcdev->platform_flags & MX2_CAMERA_SWAP16)
                 csicr1 |= CSICR1_SWAP16_EN;
diff --git a/drivers/media/video/mx3_camera.c b/drivers/media/video/mx3_camera.c
index c8e958a07e91..f96f92f00f92 100644
--- a/drivers/media/video/mx3_camera.c
+++ b/drivers/media/video/mx3_camera.c
@@ -109,10 +109,12 @@ struct mx3_camera_dev {
 
         unsigned long           platform_flags;
         unsigned long           mclk;
+        u16                     width_flags;    /* max 15 bits */
 
         struct list_head        capture;
         spinlock_t              lock;           /* Protects video buffer lists */
         struct mx3_camera_buffer *active;
+        size_t                  buf_total;
         struct vb2_alloc_ctx    *alloc_ctx;
         enum v4l2_field         field;
         int                     sequence;
@@ -190,79 +192,53 @@ static void mx3_cam_dma_done(void *arg)
  * Calculate the __buffer__ (not data) size and number of buffers.
  */
 static int mx3_videobuf_setup(struct vb2_queue *vq,
+                        const struct v4l2_format *fmt,
                         unsigned int *count, unsigned int *num_planes,
                         unsigned int 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 mx3_camera_dev *mx3_cam = ici->priv;
-        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;
+        int bytes_per_line;
+        unsigned int height;
 
         if (!mx3_cam->idmac_channel[0])
                 return -EINVAL;
 
-        *num_planes = 1;
-
-        mx3_cam->sequence = 0;
-        sizes[0] = bytes_per_line * icd->user_height;
-        alloc_ctxs[0] = mx3_cam->alloc_ctx;
-
-        if (!*count)
-                *count = 32;
-
-        if (sizes[0] * *count > MAX_VIDEO_MEM * 1024 * 1024)
-                *count = MAX_VIDEO_MEM * 1024 * 1024 / sizes[0];
-
-        return 0;
-}
-
-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->parent);
-        struct mx3_camera_dev *mx3_cam = ici->priv;
-        struct idmac_channel *ichan = mx3_cam->idmac_channel[0];
-        struct scatterlist *sg;
-        struct mx3_camera_buffer *buf;
-        size_t new_size;
-        int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+        if (fmt) {
+                const struct soc_camera_format_xlate *xlate = soc_camera_xlate_by_fourcc(icd,
+                                                                fmt->fmt.pix.pixelformat);
+                if (!xlate)
+                        return -EINVAL;
+                bytes_per_line = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
+                                                         xlate->host_fmt);
+                height = fmt->fmt.pix.height;
+        } else {
+                /* Called from VIDIOC_REQBUFS or in compatibility mode */
+                bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
                                                 icd->current_fmt->host_fmt);
-
+                height = icd->user_height;
+        }
         if (bytes_per_line < 0)
                 return bytes_per_line;
 
-        buf = to_mx3_vb(vb);
-        sg = &buf->sg;
-
-        new_size = bytes_per_line * icd->user_height;
+        sizes[0] = bytes_per_line * height;
 
-        if (vb2_plane_size(vb, 0) < new_size) {
-                dev_err(icd->parent, "Buffer too small (%lu < %zu)\n",
-                        vb2_plane_size(vb, 0), new_size);
-                return -ENOBUFS;
-        }
+        alloc_ctxs[0] = mx3_cam->alloc_ctx;
 
-        if (buf->state == CSI_BUF_NEEDS_INIT) {
-                sg_dma_address(sg)      = vb2_dma_contig_plane_dma_addr(vb, 0);
-                sg_dma_len(sg)          = new_size;
+        if (!vq->num_buffers)
+                mx3_cam->sequence = 0;
 
-                buf->txd = ichan->dma_chan.device->device_prep_slave_sg(
-                        &ichan->dma_chan, sg, 1, DMA_FROM_DEVICE,
-                        DMA_PREP_INTERRUPT);
-                if (!buf->txd)
-                        return -EIO;
-
-                buf->txd->callback_param        = buf->txd;
-                buf->txd->callback              = mx3_cam_dma_done;
+        if (!*count)
+                *count = 2;
 
-                buf->state = CSI_BUF_PREPARED;
-        }
+        /* If *num_planes != 0, we have already verified *count. */
+        if (!*num_planes &&
+            sizes[0] * *count + mx3_cam->buf_total > MAX_VIDEO_MEM * 1024 * 1024)
+                *count = (MAX_VIDEO_MEM * 1024 * 1024 - mx3_cam->buf_total) /
+                        sizes[0];
 
-        vb2_set_plane_payload(vb, 0, new_size);
+        *num_planes = 1;
 
         return 0;
 }
@@ -286,28 +262,58 @@ static void mx3_videobuf_queue(struct vb2_buffer *vb)
         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;
-        struct idmac_channel *ichan = to_idmac_chan(txd->chan);
+        struct scatterlist *sg = &buf->sg;
+        struct dma_async_tx_descriptor *txd;
+        struct idmac_channel *ichan = mx3_cam->idmac_channel[0];
         struct idmac_video_param *video = &ichan->params.video;
-        dma_cookie_t cookie;
-        u32 fourcc = icd->current_fmt->host_fmt->fourcc;
+        const struct soc_mbus_pixelfmt *host_fmt = icd->current_fmt->host_fmt;
+        int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width, host_fmt);
         unsigned long flags;
+        dma_cookie_t cookie;
+        size_t new_size;
+
+        BUG_ON(bytes_per_line <= 0);
+
+        new_size = bytes_per_line * icd->user_height;
+
+        if (vb2_plane_size(vb, 0) < new_size) {
+                dev_err(icd->parent, "Buffer #%d too small (%lu < %zu)\n",
+                        vb->v4l2_buf.index, vb2_plane_size(vb, 0), new_size);
+                goto error;
+        }
+
+        if (buf->state == CSI_BUF_NEEDS_INIT) {
+                sg_dma_address(sg)      = vb2_dma_contig_plane_dma_addr(vb, 0);
+                sg_dma_len(sg)          = new_size;
+
+                txd = ichan->dma_chan.device->device_prep_slave_sg(
+                        &ichan->dma_chan, sg, 1, DMA_FROM_DEVICE,
+                        DMA_PREP_INTERRUPT);
+                if (!txd)
+                        goto error;
+
+                txd->callback_param     = txd;
+                txd->callback           = mx3_cam_dma_done;
+
+                buf->state              = CSI_BUF_PREPARED;
+                buf->txd                = txd;
+        } else {
+                txd = buf->txd;
+        }
+
+        vb2_set_plane_payload(vb, 0, new_size);
 
         /* This is the configuration of one sg-element */
-        video->out_pixel_fmt    = fourcc_to_ipu_pix(fourcc);
+        video->out_pixel_fmt = fourcc_to_ipu_pix(host_fmt->fourcc);
 
         if (video->out_pixel_fmt == IPU_PIX_FMT_GENERIC) {
                 /*
-                 * If the IPU DMA channel is configured to transport
-                 * generic 8-bit data, we have to set up correctly the
-                 * geometry parameters upon the current pixel format.
-                 * So, since the DMA horizontal parameters are expressed
-                 * in bytes not pixels, convert these in the right unit.
+                 * If the IPU DMA channel is configured to transfer generic
+                 * 8-bit data, we have to set up the geometry parameters
+                 * correctly, according to the current pixel format. The DMA
+                 * horizontal parameters in this case are expressed in bytes,
+                 * not in pixels.
                  */
-                int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-                                                icd->current_fmt->host_fmt);
-                BUG_ON(bytes_per_line <= 0);
-
                 video->out_width        = bytes_per_line;
                 video->out_height       = icd->user_height;
                 video->out_stride       = bytes_per_line;
@@ -351,6 +357,7 @@ static void mx3_videobuf_queue(struct vb2_buffer *vb)
                 mx3_cam->active = NULL;
 
         spin_unlock_irqrestore(&mx3_cam->lock, flags);
+error:
         vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
 }
 
@@ -384,17 +391,24 @@ static void mx3_videobuf_release(struct vb2_buffer *vb)
         }
 
         spin_unlock_irqrestore(&mx3_cam->lock, flags);
+
+        mx3_cam->buf_total -= vb2_plane_size(vb, 0);
 }
 
 static int mx3_videobuf_init(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 mx3_camera_dev *mx3_cam = ici->priv;
         struct mx3_camera_buffer *buf = to_mx3_vb(vb);
+
         /* This is for locking debugging only */
         INIT_LIST_HEAD(&buf->queue);
         sg_init_table(&buf->sg, 1);
 
         buf->state = CSI_BUF_NEEDS_INIT;
-        buf->txd = NULL;
+
+        mx3_cam->buf_total += vb2_plane_size(vb, 0);
 
         return 0;
 }
@@ -405,13 +419,12 @@ static int mx3_stop_streaming(struct vb2_queue *q)
         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;
         struct mx3_camera_buffer *buf, *tmp;
         unsigned long flags;
 
         if (ichan) {
-                chan = &ichan->dma_chan;
-                chan->device->device_control(chan, DMA_TERMINATE_ALL, 0);
+                struct dma_chan *chan = &ichan->dma_chan;
+                chan->device->device_control(chan, DMA_PAUSE, 0);
         }
 
         spin_lock_irqsave(&mx3_cam->lock, flags);
@@ -419,8 +432,8 @@ static int mx3_stop_streaming(struct vb2_queue *q)
         mx3_cam->active = NULL;
 
         list_for_each_entry_safe(buf, tmp, &mx3_cam->capture, queue) {
-                buf->state = CSI_BUF_NEEDS_INIT;
                 list_del_init(&buf->queue);
+                vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
         }
 
         spin_unlock_irqrestore(&mx3_cam->lock, flags);
@@ -430,7 +443,6 @@ static int mx3_stop_streaming(struct vb2_queue *q)
 
 static struct vb2_ops mx3_videobuf_ops = {
         .queue_setup    = mx3_videobuf_setup,
-        .buf_prepare    = mx3_videobuf_prepare,
         .buf_queue      = mx3_videobuf_queue,
         .buf_cleanup    = mx3_videobuf_release,
         .buf_init       = mx3_videobuf_init,
@@ -514,6 +526,7 @@ static int mx3_camera_add_device(struct soc_camera_device *icd)
 
         mx3_camera_activate(mx3_cam, icd);
 
+        mx3_cam->buf_total = 0;
         mx3_cam->icd = icd;
 
         dev_info(icd->parent, "MX3 Camera driver attached to camera %d\n",
@@ -548,58 +561,27 @@ static int test_platform_param(struct mx3_camera_dev *mx3_cam,
                                unsigned char buswidth, unsigned long *flags)
 {
         /*
+         * If requested data width is supported by the platform, use it or any
+         * possible lower value - i.MX31 is smart enough to shift bits
+         */
+        if (buswidth > fls(mx3_cam->width_flags))
+                return -EINVAL;
+
+        /*
          * Platform specified synchronization and pixel clock polarities are
          * only a recommendation and are only used during probing. MX3x
          * 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 |
-                SOCAM_DATA_ACTIVE_LOW;
-
-        /*
-         * If requested data width is supported by the platform, use it or any
-         * possible lower value - i.MX31 is smart enough to schift bits
-         */
-        if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_15)
-                *flags |= SOCAM_DATAWIDTH_15 | SOCAM_DATAWIDTH_10 |
-                        SOCAM_DATAWIDTH_8 | SOCAM_DATAWIDTH_4;
-        else if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_10)
-                *flags |= SOCAM_DATAWIDTH_10 | SOCAM_DATAWIDTH_8 |
-                        SOCAM_DATAWIDTH_4;
-        else if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_8)
-                *flags |= SOCAM_DATAWIDTH_8 | SOCAM_DATAWIDTH_4;
-        else if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_4)
-                *flags |= SOCAM_DATAWIDTH_4;
-
-        switch (buswidth) {
-        case 15:
-                if (!(*flags & SOCAM_DATAWIDTH_15))
-                        return -EINVAL;
-                break;
-        case 10:
-                if (!(*flags & SOCAM_DATAWIDTH_10))
-                        return -EINVAL;
-                break;
-        case 8:
-                if (!(*flags & SOCAM_DATAWIDTH_8))
-                        return -EINVAL;
-                break;
-        case 4:
-                if (!(*flags & SOCAM_DATAWIDTH_4))
-                        return -EINVAL;
-                break;
-        default:
-                dev_warn(mx3_cam->soc_host.v4l2_dev.dev,
-                         "Unsupported bus width %d\n", buswidth);
-                return -EINVAL;
-        }
+        *flags = V4L2_MBUS_MASTER |
+                V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+                V4L2_MBUS_HSYNC_ACTIVE_LOW |
+                V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+                V4L2_MBUS_VSYNC_ACTIVE_LOW |
+                V4L2_MBUS_PCLK_SAMPLE_RISING |
+                V4L2_MBUS_PCLK_SAMPLE_FALLING |
+                V4L2_MBUS_DATA_ACTIVE_HIGH |
+                V4L2_MBUS_DATA_ACTIVE_LOW;
 
         return 0;
 }
@@ -607,9 +589,11 @@ 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 v4l2_subdev *sd = soc_camera_to_subdev(icd);
         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;
+        struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+        unsigned long bus_flags, common_flags;
         int ret = test_platform_param(mx3_cam, depth, &bus_flags);
 
         dev_dbg(icd->parent, "request bus width %d bit: %d\n", depth, ret);
@@ -617,15 +601,21 @@ static int mx3_camera_try_bus_param(struct soc_camera_device *icd,
         if (ret < 0)
                 return ret;
 
-        camera_flags = icd->ops->query_bus_param(icd);
-
-        ret = soc_camera_bus_param_compatible(camera_flags, bus_flags);
-        if (ret < 0)
-                dev_warn(icd->parent,
-                         "Flags incompatible: camera %lx, host %lx\n",
-                         camera_flags, bus_flags);
+        ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+        if (!ret) {
+                common_flags = soc_mbus_config_compatible(&cfg,
+                                                          bus_flags);
+                if (!common_flags) {
+                        dev_warn(icd->parent,
+                                 "Flags incompatible: camera 0x%x, host 0x%lx\n",
+                                 cfg.flags, bus_flags);
+                        return -EINVAL;
+                }
+        } else if (ret != -ENOIOCTLCMD) {
+                return ret;
+        }
 
-        return ret;
+        return 0;
 }
 
 static bool chan_filter(struct dma_chan *chan, void *arg)
@@ -994,9 +984,11 @@ 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 v4l2_subdev *sd = soc_camera_to_subdev(icd);
         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;
+        struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+        unsigned long bus_flags, common_flags;
         u32 dw, sens_conf;
         const struct soc_mbus_pixelfmt *fmt;
         int buswidth;
@@ -1008,83 +1000,76 @@ static int mx3_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
         if (!fmt)
                 return -EINVAL;
 
-        buswidth = fmt->bits_per_sample;
-        ret = test_platform_param(mx3_cam, buswidth, &bus_flags);
-
         xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
         if (!xlate) {
                 dev_warn(dev, "Format %x not found\n", pixfmt);
                 return -EINVAL;
         }
 
+        buswidth = fmt->bits_per_sample;
+        ret = test_platform_param(mx3_cam, buswidth, &bus_flags);
+
         dev_dbg(dev, "requested bus width %d bit: %d\n", buswidth, ret);
 
         if (ret < 0)
                 return ret;
 
-        camera_flags = icd->ops->query_bus_param(icd);
-
-        common_flags = soc_camera_bus_param_compatible(camera_flags, bus_flags);
-        dev_dbg(dev, "Flags cam: 0x%lx host: 0x%lx common: 0x%lx\n",
-                camera_flags, bus_flags, common_flags);
-        if (!common_flags) {
-                dev_dbg(dev, "no common flags");
-                return -EINVAL;
+        ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+        if (!ret) {
+                common_flags = soc_mbus_config_compatible(&cfg,
+                                                          bus_flags);
+                if (!common_flags) {
+                        dev_warn(icd->parent,
+                                 "Flags incompatible: camera 0x%x, host 0x%lx\n",
+                                 cfg.flags, bus_flags);
+                        return -EINVAL;
+                }
+        } else if (ret != -ENOIOCTLCMD) {
+                return ret;
+        } else {
+                common_flags = bus_flags;
         }
 
+        dev_dbg(dev, "Flags cam: 0x%x host: 0x%lx common: 0x%lx\n",
+                cfg.flags, bus_flags, common_flags);
+
         /* Make choices, based on platform preferences */
-        if ((common_flags & SOCAM_HSYNC_ACTIVE_HIGH) &&
-            (common_flags & SOCAM_HSYNC_ACTIVE_LOW)) {
+        if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
+            (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
                 if (mx3_cam->platform_flags & MX3_CAMERA_HSP)
-                        common_flags &= ~SOCAM_HSYNC_ACTIVE_HIGH;
+                        common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
                 else
-                        common_flags &= ~SOCAM_HSYNC_ACTIVE_LOW;
+                        common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
         }
 
-        if ((common_flags & SOCAM_VSYNC_ACTIVE_HIGH) &&
-            (common_flags & SOCAM_VSYNC_ACTIVE_LOW)) {
+        if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
+            (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
                 if (mx3_cam->platform_flags & MX3_CAMERA_VSP)
-                        common_flags &= ~SOCAM_VSYNC_ACTIVE_HIGH;
+                        common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
                 else
-                        common_flags &= ~SOCAM_VSYNC_ACTIVE_LOW;
+                        common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
         }
 
-        if ((common_flags & SOCAM_DATA_ACTIVE_HIGH) &&
-            (common_flags & SOCAM_DATA_ACTIVE_LOW)) {
+        if ((common_flags & V4L2_MBUS_DATA_ACTIVE_HIGH) &&
+            (common_flags & V4L2_MBUS_DATA_ACTIVE_LOW)) {
                 if (mx3_cam->platform_flags & MX3_CAMERA_DP)
-                        common_flags &= ~SOCAM_DATA_ACTIVE_HIGH;
+                        common_flags &= ~V4L2_MBUS_DATA_ACTIVE_HIGH;
                 else
-                        common_flags &= ~SOCAM_DATA_ACTIVE_LOW;
+                        common_flags &= ~V4L2_MBUS_DATA_ACTIVE_LOW;
         }
 
-        if ((common_flags & SOCAM_PCLK_SAMPLE_RISING) &&
-            (common_flags & SOCAM_PCLK_SAMPLE_FALLING)) {
+        if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
+            (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
                 if (mx3_cam->platform_flags & MX3_CAMERA_PCP)
-                        common_flags &= ~SOCAM_PCLK_SAMPLE_RISING;
+                        common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
                 else
-                        common_flags &= ~SOCAM_PCLK_SAMPLE_FALLING;
+                        common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
         }
 
-        /*
-         * Make the camera work in widest common mode, we'll take care of
-         * the rest
-         */
-        if (common_flags & SOCAM_DATAWIDTH_15)
-                common_flags = (common_flags & ~SOCAM_DATAWIDTH_MASK) |
-                        SOCAM_DATAWIDTH_15;
-        else if (common_flags & SOCAM_DATAWIDTH_10)
-                common_flags = (common_flags & ~SOCAM_DATAWIDTH_MASK) |
-                        SOCAM_DATAWIDTH_10;
-        else if (common_flags & SOCAM_DATAWIDTH_8)
-                common_flags = (common_flags & ~SOCAM_DATAWIDTH_MASK) |
-                        SOCAM_DATAWIDTH_8;
-        else
-                common_flags = (common_flags & ~SOCAM_DATAWIDTH_MASK) |
-                        SOCAM_DATAWIDTH_4;
-
-        ret = icd->ops->set_bus_param(icd, common_flags);
-        if (ret < 0) {
-                dev_dbg(dev, "camera set_bus_param(%lx) returned %d\n",
+        cfg.flags = common_flags;
+        ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
+        if (ret < 0 && ret != -ENOIOCTLCMD) {
+                dev_dbg(dev, "camera s_mbus_config(0x%lx) returned %d\n",
                         common_flags, ret);
                 return ret;
         }
@@ -1108,13 +1093,13 @@ static int mx3_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
         /* This has been set in mx3_camera_activate(), but we clear it above */
         sens_conf |= CSI_SENS_CONF_DATA_FMT_BAYER;
 
-        if (common_flags & SOCAM_PCLK_SAMPLE_FALLING)
+        if (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
                 sens_conf |= 1 << CSI_SENS_CONF_PIX_CLK_POL_SHIFT;
-        if (common_flags & SOCAM_HSYNC_ACTIVE_LOW)
+        if (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
                 sens_conf |= 1 << CSI_SENS_CONF_HSYNC_POL_SHIFT;
-        if (common_flags & SOCAM_VSYNC_ACTIVE_LOW)
+        if (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
                 sens_conf |= 1 << CSI_SENS_CONF_VSYNC_POL_SHIFT;
-        if (common_flags & SOCAM_DATA_ACTIVE_LOW)
+        if (common_flags & V4L2_MBUS_DATA_ACTIVE_LOW)
                 sens_conf |= 1 << CSI_SENS_CONF_DATA_POL_SHIFT;
 
         /* Just do what we're asked to do */
@@ -1199,6 +1184,14 @@ static int __devinit mx3_camera_probe(struct platform_device *pdev)
                          "data widths, using default 8 bit\n");
                 mx3_cam->platform_flags |= MX3_CAMERA_DATAWIDTH_8;
         }
+        if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_4)
+                mx3_cam->width_flags = 1 << 3;
+        if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_8)
+                mx3_cam->width_flags |= 1 << 7;
+        if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_10)
+                mx3_cam->width_flags |= 1 << 9;
+        if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_15)
+                mx3_cam->width_flags |= 1 << 14;
 
         mx3_cam->mclk = mx3_cam->pdata->mclk_10khz * 10000;
         if (!mx3_cam->mclk) {
@@ -1281,8 +1274,6 @@ static int __devexit mx3_camera_remove(struct platform_device *pdev)
 
         dmaengine_put();
 
-        dev_info(&pdev->dev, "i.MX3x Camera driver unloaded\n");
-
         return 0;
 }
 
diff --git a/drivers/media/video/omap/omap_vout.c b/drivers/media/video/omap/omap_vout.c
index 30d8896bb710..9c5c19f142de 100644
--- a/drivers/media/video/omap/omap_vout.c
+++ b/drivers/media/video/omap/omap_vout.c
@@ -833,6 +833,15 @@ static void omap_vout_buffer_release(struct videobuf_queue *q,
 /*
  *  File operations
  */
+static unsigned int omap_vout_poll(struct file *file,
+                                   struct poll_table_struct *wait)
+{
+        struct omap_vout_device *vout = file->private_data;
+        struct videobuf_queue *q = &vout->vbq;
+
+        return videobuf_poll_stream(file, q, wait);
+}
+
 static void omap_vout_vm_open(struct vm_area_struct *vma)
 {
         struct omap_vout_device *vout = vma->vm_private_data;
@@ -1861,6 +1870,7 @@ static const struct v4l2_ioctl_ops vout_ioctl_ops = {
 
 static const struct v4l2_file_operations omap_vout_fops = {
         .owner          = THIS_MODULE,
+        .poll           = omap_vout_poll,
         .unlocked_ioctl = video_ioctl2,
         .mmap           = omap_vout_mmap,
         .open           = omap_vout_open,
diff --git a/drivers/media/video/omap1_camera.c b/drivers/media/video/omap1_camera.c
index 8a947e603aca..e87ae2f634b2 100644
--- a/drivers/media/video/omap1_camera.c
+++ b/drivers/media/video/omap1_camera.c
@@ -102,10 +102,10 @@
 /* end of OMAP1 Camera Interface registers */
 
 
-#define SOCAM_BUS_FLAGS (SOCAM_MASTER | \
-                        SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_HIGH | \
-                        SOCAM_PCLK_SAMPLE_RISING | SOCAM_PCLK_SAMPLE_FALLING | \
-                        SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATAWIDTH_8)
+#define SOCAM_BUS_FLAGS (V4L2_MBUS_MASTER | \
+                        V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH | \
+                        V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING | \
+                        V4L2_MBUS_DATA_ACTIVE_HIGH)
 
 
 #define FIFO_SIZE               ((THRESHOLD_MASK >> THRESHOLD_SHIFT) + 1)
@@ -1438,41 +1438,55 @@ 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 v4l2_subdev *sd = soc_camera_to_subdev(icd);
         struct device *dev = icd->parent;
         struct soc_camera_host *ici = to_soc_camera_host(dev);
         struct omap1_cam_dev *pcdev = ici->priv;
         const struct soc_camera_format_xlate *xlate;
         const struct soc_mbus_pixelfmt *fmt;
-        unsigned long camera_flags, common_flags;
+        struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+        unsigned long common_flags;
         u32 ctrlclock, mode;
         int ret;
 
-        camera_flags = icd->ops->query_bus_param(icd);
-
-        common_flags = soc_camera_bus_param_compatible(camera_flags,
-                        SOCAM_BUS_FLAGS);
-        if (!common_flags)
-                return -EINVAL;
+        ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+        if (!ret) {
+                common_flags = soc_mbus_config_compatible(&cfg, SOCAM_BUS_FLAGS);
+                if (!common_flags) {
+                        dev_warn(dev,
+                                 "Flags incompatible: camera 0x%x, host 0x%x\n",
+                                 cfg.flags, SOCAM_BUS_FLAGS);
+                        return -EINVAL;
+                }
+        } else if (ret != -ENOIOCTLCMD) {
+                return ret;
+        } else {
+                common_flags = SOCAM_BUS_FLAGS;
+        }
 
         /* Make choices, possibly based on platform configuration */
-        if ((common_flags & SOCAM_PCLK_SAMPLE_RISING) &&
-                        (common_flags & SOCAM_PCLK_SAMPLE_FALLING)) {
+        if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
+                        (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
                 if (!pcdev->pdata ||
                                 pcdev->pdata->flags & OMAP1_CAMERA_LCLK_RISING)
-                        common_flags &= ~SOCAM_PCLK_SAMPLE_FALLING;
+                        common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
                 else
-                        common_flags &= ~SOCAM_PCLK_SAMPLE_RISING;
+                        common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
         }
 
-        ret = icd->ops->set_bus_param(icd, common_flags);
-        if (ret < 0)
+        cfg.flags = common_flags;
+        ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
+        if (ret < 0 && ret != -ENOIOCTLCMD) {
+                dev_dbg(dev, "camera s_mbus_config(0x%lx) returned %d\n",
+                        common_flags, ret);
                 return ret;
+        }
 
         ctrlclock = CAM_READ_CACHE(pcdev, CTRLCLOCK);
         if (ctrlclock & LCLK_EN)
                 CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~LCLK_EN);
 
-        if (common_flags & SOCAM_PCLK_SAMPLE_RISING) {
+        if (common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) {
                 dev_dbg(dev, "CTRLCLOCK_REG |= POLCLK\n");
                 ctrlclock |= POLCLK;
         } else {
@@ -1565,10 +1579,10 @@ static int __init omap1_cam_probe(struct platform_device *pdev)
         pcdev->clk = clk;
 
         pcdev->pdata = pdev->dev.platform_data;
-        pcdev->pflags = pcdev->pdata->flags;
-
-        if (pcdev->pdata)
+        if (pcdev->pdata) {
+                pcdev->pflags = pcdev->pdata->flags;
                 pcdev->camexclk = pcdev->pdata->camexclk_khz * 1000;
+        }
 
         switch (pcdev->camexclk) {
         case 6000000:
@@ -1578,6 +1592,7 @@ static int __init omap1_cam_probe(struct platform_device *pdev)
         case 24000000:
                 break;
         default:
+                /* pcdev->camexclk != 0 => pcdev->pdata != NULL */
                 dev_warn(&pdev->dev,
                                 "Incorrect sensor clock frequency %ld kHz, "
                                 "should be one of 0, 6, 8, 9.6, 12 or 24 MHz, "
@@ -1585,8 +1600,7 @@ static int __init omap1_cam_probe(struct platform_device *pdev)
                                 pcdev->pdata->camexclk_khz);
                 pcdev->camexclk = 0;
         case 0:
-                dev_info(&pdev->dev,
-                                "Not providing sensor clock\n");
+                dev_info(&pdev->dev, "Not providing sensor clock\n");
         }
 
         INIT_LIST_HEAD(&pcdev->capture);
@@ -1716,5 +1730,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_VERSION(DRIVER_VERSION);
 MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/media/video/omap3isp/isp.c b/drivers/media/video/omap3isp/isp.c
index 678e1252047a..b818cacf420f 100644
--- a/drivers/media/video/omap3isp/isp.c
+++ b/drivers/media/video/omap3isp/isp.c
@@ -1704,6 +1704,7 @@ static int isp_register_entities(struct isp_device *isp)
         isp->media_dev.dev = isp->dev;
         strlcpy(isp->media_dev.model, "TI OMAP3 ISP",
                 sizeof(isp->media_dev.model));
+        isp->media_dev.hw_revision = isp->revision;
         isp->media_dev.link_notify = isp_pipeline_link_notify;
         ret = media_device_register(&isp->media_dev);
         if (ret < 0) {
@@ -2210,6 +2211,8 @@ error:
         regulator_put(isp->isp_csiphy2.vdd);
         regulator_put(isp->isp_csiphy1.vdd);
         platform_set_drvdata(pdev, NULL);
+
+        mutex_destroy(&isp->isp_mutex);
         kfree(isp);
 
         return ret;
diff --git a/drivers/media/video/omap3isp/ispccdc.c b/drivers/media/video/omap3isp/ispccdc.c
index 253fdcce2df2..b0b0fa5a3572 100644
--- a/drivers/media/video/omap3isp/ispccdc.c
+++ b/drivers/media/video/omap3isp/ispccdc.c
@@ -1836,7 +1836,7 @@ ccdc_try_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_fh *fh,
                  * callers to request an output size bigger than the input size
                  * up to the nearest multiple of 16.
                  */
-                fmt->width = clamp_t(u32, width, 32, (fmt->width + 15) & ~15);
+                fmt->width = clamp_t(u32, width, 32, fmt->width + 15);
                 fmt->width &= ~15;
                 fmt->height = clamp_t(u32, height, 32, fmt->height);
                 break;
@@ -2152,6 +2152,37 @@ static const struct media_entity_operations ccdc_media_ops = {
         .link_setup = ccdc_link_setup,
 };
 
+void omap3isp_ccdc_unregister_entities(struct isp_ccdc_device *ccdc)
+{
+        v4l2_device_unregister_subdev(&ccdc->subdev);
+        omap3isp_video_unregister(&ccdc->video_out);
+}
+
+int omap3isp_ccdc_register_entities(struct isp_ccdc_device *ccdc,
+        struct v4l2_device *vdev)
+{
+        int ret;
+
+        /* Register the subdev and video node. */
+        ret = v4l2_device_register_subdev(vdev, &ccdc->subdev);
+        if (ret < 0)
+                goto error;
+
+        ret = omap3isp_video_register(&ccdc->video_out, vdev);
+        if (ret < 0)
+                goto error;
+
+        return 0;
+
+error:
+        omap3isp_ccdc_unregister_entities(ccdc);
+        return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP CCDC initialisation and cleanup
+ */
+
 /*
  * ccdc_init_entities - Initialize V4L2 subdev and media entity
  * @ccdc: ISP CCDC module
@@ -2193,50 +2224,23 @@ static int ccdc_init_entities(struct isp_ccdc_device *ccdc)
 
         ret = omap3isp_video_init(&ccdc->video_out, "CCDC");
         if (ret < 0)
-                return ret;
+                goto error_video;
 
         /* Connect the CCDC subdev to the video node. */
         ret = media_entity_create_link(&ccdc->subdev.entity, CCDC_PAD_SOURCE_OF,
                         &ccdc->video_out.video.entity, 0, 0);
         if (ret < 0)
-                return ret;
-
-        return 0;
-}
-
-void omap3isp_ccdc_unregister_entities(struct isp_ccdc_device *ccdc)
-{
-        media_entity_cleanup(&ccdc->subdev.entity);
-
-        v4l2_device_unregister_subdev(&ccdc->subdev);
-        omap3isp_video_unregister(&ccdc->video_out);
-}
-
-int omap3isp_ccdc_register_entities(struct isp_ccdc_device *ccdc,
-        struct v4l2_device *vdev)
-{
-        int ret;
-
-        /* Register the subdev and video node. */
-        ret = v4l2_device_register_subdev(vdev, &ccdc->subdev);
-        if (ret < 0)
-                goto error;
-
-        ret = omap3isp_video_register(&ccdc->video_out, vdev);
-        if (ret < 0)
-                goto error;
+                goto error_link;
 
         return 0;
 
-error:
-        omap3isp_ccdc_unregister_entities(ccdc);
+error_link:
+        omap3isp_video_cleanup(&ccdc->video_out);
+error_video:
+        media_entity_cleanup(me);
         return ret;
 }
 
-/* -----------------------------------------------------------------------------
- * ISP CCDC initialisation and cleanup
- */
-
 /*
  * omap3isp_ccdc_init - CCDC module initialization.
  * @dev: Device pointer specific to the OMAP3 ISP.
@@ -2248,6 +2252,7 @@ error:
 int omap3isp_ccdc_init(struct isp_device *isp)
 {
         struct isp_ccdc_device *ccdc = &isp->isp_ccdc;
+        int ret;
 
         spin_lock_init(&ccdc->lock);
         init_waitqueue_head(&ccdc->wait);
@@ -2276,7 +2281,13 @@ int omap3isp_ccdc_init(struct isp_device *isp)
         ccdc->update = OMAP3ISP_CCDC_BLCLAMP;
         ccdc_apply_controls(ccdc);
 
-        return ccdc_init_entities(ccdc);
+        ret = ccdc_init_entities(ccdc);
+        if (ret < 0) {
+                mutex_destroy(&ccdc->ioctl_lock);
+                return ret;
+        }
+
+        return 0;
 }
 
 /*
@@ -2287,6 +2298,9 @@ void omap3isp_ccdc_cleanup(struct isp_device *isp)
 {
         struct isp_ccdc_device *ccdc = &isp->isp_ccdc;
 
+        omap3isp_video_cleanup(&ccdc->video_out);
+        media_entity_cleanup(&ccdc->subdev.entity);
+
         /* Free LSC requests. As the CCDC is stopped there's no active request,
          * so only the pending request and the free queue need to be handled.
          */
@@ -2296,4 +2310,6 @@ void omap3isp_ccdc_cleanup(struct isp_device *isp)
 
         if (ccdc->fpc.fpcaddr != 0)
                 omap_iommu_vfree(isp->domain, isp->iommu, ccdc->fpc.fpcaddr);
+
+        mutex_destroy(&ccdc->ioctl_lock);
 }
diff --git a/drivers/media/video/omap3isp/ispccp2.c b/drivers/media/video/omap3isp/ispccp2.c
index fa1d09b0ad98..904ca8c8b17f 100644
--- a/drivers/media/video/omap3isp/ispccp2.c
+++ b/drivers/media/video/omap3isp/ispccp2.c
@@ -1032,6 +1032,48 @@ static const struct media_entity_operations ccp2_media_ops = {
 };
 
 /*
+ * omap3isp_ccp2_unregister_entities - Unregister media entities: subdev
+ * @ccp2: Pointer to ISP CCP2 device
+ */
+void omap3isp_ccp2_unregister_entities(struct isp_ccp2_device *ccp2)
+{
+        v4l2_device_unregister_subdev(&ccp2->subdev);
+        omap3isp_video_unregister(&ccp2->video_in);
+}
+
+/*
+ * omap3isp_ccp2_register_entities - Register the subdev media entity
+ * @ccp2: Pointer to ISP CCP2 device
+ * @vdev: Pointer to v4l device
+ * return negative error code or zero on success
+ */
+
+int omap3isp_ccp2_register_entities(struct isp_ccp2_device *ccp2,
+                                    struct v4l2_device *vdev)
+{
+        int ret;
+
+        /* Register the subdev and video nodes. */
+        ret = v4l2_device_register_subdev(vdev, &ccp2->subdev);
+        if (ret < 0)
+                goto error;
+
+        ret = omap3isp_video_register(&ccp2->video_in, vdev);
+        if (ret < 0)
+                goto error;
+
+        return 0;
+
+error:
+        omap3isp_ccp2_unregister_entities(ccp2);
+        return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP ccp2 initialisation and cleanup
+ */
+
+/*
  * ccp2_init_entities - Initialize ccp2 subdev and media entity.
  * @ccp2: Pointer to ISP CCP2 device
  * return negative error code or zero on success
@@ -1083,72 +1125,23 @@ static int ccp2_init_entities(struct isp_ccp2_device *ccp2)
 
         ret = omap3isp_video_init(&ccp2->video_in, "CCP2");
         if (ret < 0)
-                return ret;
+                goto error_video;
 
         /* Connect the video node to the ccp2 subdev. */
         ret = media_entity_create_link(&ccp2->video_in.video.entity, 0,
                                        &ccp2->subdev.entity, CCP2_PAD_SINK, 0);
         if (ret < 0)
-                return ret;
+                goto error_link;
 
         return 0;
-}
 
-/*
- * omap3isp_ccp2_unregister_entities - Unregister media entities: subdev
- * @ccp2: Pointer to ISP CCP2 device
- */
-void omap3isp_ccp2_unregister_entities(struct isp_ccp2_device *ccp2)
-{
+error_link:
+        omap3isp_video_cleanup(&ccp2->video_in);
+error_video:
         media_entity_cleanup(&ccp2->subdev.entity);
-
-        v4l2_device_unregister_subdev(&ccp2->subdev);
-        omap3isp_video_unregister(&ccp2->video_in);
-}
-
-/*
- * omap3isp_ccp2_register_entities - Register the subdev media entity
- * @ccp2: Pointer to ISP CCP2 device
- * @vdev: Pointer to v4l device
- * return negative error code or zero on success
- */
-
-int omap3isp_ccp2_register_entities(struct isp_ccp2_device *ccp2,
-                                    struct v4l2_device *vdev)
-{
-        int ret;
-
-        /* Register the subdev and video nodes. */
-        ret = v4l2_device_register_subdev(vdev, &ccp2->subdev);
-        if (ret < 0)
-                goto error;
-
-        ret = omap3isp_video_register(&ccp2->video_in, vdev);
-        if (ret < 0)
-                goto error;
-
-        return 0;
-
-error:
-        omap3isp_ccp2_unregister_entities(ccp2);
         return ret;
 }
 
-/* -----------------------------------------------------------------------------
- * ISP ccp2 initialisation and cleanup
- */
-
-/*
- * omap3isp_ccp2_cleanup - CCP2 un-initialization
- * @isp : Pointer to ISP device
- */
-void omap3isp_ccp2_cleanup(struct isp_device *isp)
-{
-        struct isp_ccp2_device *ccp2 = &isp->isp_ccp2;
-
-        regulator_put(ccp2->vdds_csib);
-}
-
 /*
  * omap3isp_ccp2_init - CCP2 initialization.
  * @isp : Pointer to ISP device
@@ -1184,13 +1177,25 @@ int omap3isp_ccp2_init(struct isp_device *isp)
         }
 
         ret = ccp2_init_entities(ccp2);
-        if (ret < 0)
-                goto out;
+        if (ret < 0) {
+                regulator_put(ccp2->vdds_csib);
+                return ret;
+        }
 
         ccp2_reset(ccp2);
-out:
-        if (ret)
-                omap3isp_ccp2_cleanup(isp);
+        return 0;
+}
 
-        return ret;
+/*
+ * omap3isp_ccp2_cleanup - CCP2 un-initialization
+ * @isp : Pointer to ISP device
+ */
+void omap3isp_ccp2_cleanup(struct isp_device *isp)
+{
+        struct isp_ccp2_device *ccp2 = &isp->isp_ccp2;
+
+        omap3isp_video_cleanup(&ccp2->video_in);
+        media_entity_cleanup(&ccp2->subdev.entity);
+
+        regulator_put(ccp2->vdds_csib);
 }
diff --git a/drivers/media/video/omap3isp/ispcsi2.c b/drivers/media/video/omap3isp/ispcsi2.c
index 69161a682b3d..0c5f1cb9d99d 100644
--- a/drivers/media/video/omap3isp/ispcsi2.c
+++ b/drivers/media/video/omap3isp/ispcsi2.c
@@ -1187,6 +1187,37 @@ static const struct media_entity_operations csi2_media_ops = {
         .link_setup = csi2_link_setup,
 };
 
+void omap3isp_csi2_unregister_entities(struct isp_csi2_device *csi2)
+{
+        v4l2_device_unregister_subdev(&csi2->subdev);
+        omap3isp_video_unregister(&csi2->video_out);
+}
+
+int omap3isp_csi2_register_entities(struct isp_csi2_device *csi2,
+                                    struct v4l2_device *vdev)
+{
+        int ret;
+
+        /* Register the subdev and video nodes. */
+        ret = v4l2_device_register_subdev(vdev, &csi2->subdev);
+        if (ret < 0)
+                goto error;
+
+        ret = omap3isp_video_register(&csi2->video_out, vdev);
+        if (ret < 0)
+                goto error;
+
+        return 0;
+
+error:
+        omap3isp_csi2_unregister_entities(csi2);
+        return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP CSI2 initialisation and cleanup
+ */
+
 /*
  * csi2_init_entities - Initialize subdev and media entity.
  * @csi2: Pointer to csi2 structure.
@@ -1228,57 +1259,23 @@ static int csi2_init_entities(struct isp_csi2_device *csi2)
 
         ret = omap3isp_video_init(&csi2->video_out, "CSI2a");
         if (ret < 0)
-                return ret;
+                goto error_video;
 
         /* Connect the CSI2 subdev to the video node. */
         ret = media_entity_create_link(&csi2->subdev.entity, CSI2_PAD_SOURCE,
                                        &csi2->video_out.video.entity, 0, 0);
         if (ret < 0)
-                return ret;
+                goto error_link;
 
         return 0;
-}
 
-void omap3isp_csi2_unregister_entities(struct isp_csi2_device *csi2)
-{
+error_link:
+        omap3isp_video_cleanup(&csi2->video_out);
+error_video:
         media_entity_cleanup(&csi2->subdev.entity);
-
-        v4l2_device_unregister_subdev(&csi2->subdev);
-        omap3isp_video_unregister(&csi2->video_out);
-}
-
-int omap3isp_csi2_register_entities(struct isp_csi2_device *csi2,
-                                    struct v4l2_device *vdev)
-{
-        int ret;
-
-        /* Register the subdev and video nodes. */
-        ret = v4l2_device_register_subdev(vdev, &csi2->subdev);
-        if (ret < 0)
-                goto error;
-
-        ret = omap3isp_video_register(&csi2->video_out, vdev);
-        if (ret < 0)
-                goto error;
-
-        return 0;
-
-error:
-        omap3isp_csi2_unregister_entities(csi2);
         return ret;
 }
 
-/* -----------------------------------------------------------------------------
- * ISP CSI2 initialisation and cleanup
- */
-
-/*
- * omap3isp_csi2_cleanup - Routine for module driver cleanup
- */
-void omap3isp_csi2_cleanup(struct isp_device *isp)
-{
-}
-
 /*
  * omap3isp_csi2_init - Routine for module driver init
  */
@@ -1298,7 +1295,7 @@ int omap3isp_csi2_init(struct isp_device *isp)
 
         ret = csi2_init_entities(csi2a);
         if (ret < 0)
-                goto fail;
+                return ret;
 
         if (isp->revision == ISP_REVISION_15_0) {
                 csi2c->isp = isp;
@@ -1311,7 +1308,15 @@ int omap3isp_csi2_init(struct isp_device *isp)
         }
 
         return 0;
-fail:
-        omap3isp_csi2_cleanup(isp);
-        return ret;
+}
+
+/*
+ * omap3isp_csi2_cleanup - Routine for module driver cleanup
+ */
+void omap3isp_csi2_cleanup(struct isp_device *isp)
+{
+        struct isp_csi2_device *csi2a = &isp->isp_csi2a;
+
+        omap3isp_video_cleanup(&csi2a->video_out);
+        media_entity_cleanup(&csi2a->subdev.entity);
 }
diff --git a/drivers/media/video/omap3isp/isph3a_aewb.c b/drivers/media/video/omap3isp/isph3a_aewb.c
index 8068cefd8d89..a3c76bf18175 100644
--- a/drivers/media/video/omap3isp/isph3a_aewb.c
+++ b/drivers/media/video/omap3isp/isph3a_aewb.c
@@ -370,5 +370,5 @@ void omap3isp_h3a_aewb_cleanup(struct isp_device *isp)
 {
         kfree(isp->isp_aewb.priv);
         kfree(isp->isp_aewb.recover_priv);
-        omap3isp_stat_free(&isp->isp_aewb);
+        omap3isp_stat_cleanup(&isp->isp_aewb);
 }
diff --git a/drivers/media/video/omap3isp/isph3a_af.c b/drivers/media/video/omap3isp/isph3a_af.c
index ba54d0acdecf..58e0bc414899 100644
--- a/drivers/media/video/omap3isp/isph3a_af.c
+++ b/drivers/media/video/omap3isp/isph3a_af.c
@@ -425,5 +425,5 @@ void omap3isp_h3a_af_cleanup(struct isp_device *isp)
 {
         kfree(isp->isp_af.priv);
         kfree(isp->isp_af.recover_priv);
-        omap3isp_stat_free(&isp->isp_af);
+        omap3isp_stat_cleanup(&isp->isp_af);
 }
diff --git a/drivers/media/video/omap3isp/isphist.c b/drivers/media/video/omap3isp/isphist.c
index 1743856b30d1..1163907bcddc 100644
--- a/drivers/media/video/omap3isp/isphist.c
+++ b/drivers/media/video/omap3isp/isphist.c
@@ -516,5 +516,5 @@ void omap3isp_hist_cleanup(struct isp_device *isp)
         if (HIST_USING_DMA(&isp->isp_hist))
                 omap_free_dma(isp->isp_hist.dma_ch);
         kfree(isp->isp_hist.priv);
-        omap3isp_stat_free(&isp->isp_hist);
+        omap3isp_stat_cleanup(&isp->isp_hist);
 }
diff --git a/drivers/media/video/omap3isp/isppreview.c b/drivers/media/video/omap3isp/isppreview.c
index aba537af87e4..ccb876fe023f 100644
--- a/drivers/media/video/omap3isp/isppreview.c
+++ b/drivers/media/video/omap3isp/isppreview.c
@@ -76,9 +76,51 @@ static struct omap3isp_prev_csc flr_prev_csc = {
 
 #define DEF_DETECT_CORRECT_VAL  0xe
 
-#define PREV_MIN_WIDTH          64
-#define PREV_MIN_HEIGHT         8
-#define PREV_MAX_HEIGHT         16384
+/*
+ * Margins and image size limits.
+ *
+ * The preview engine crops several rows and columns internally depending on
+ * which filters are enabled. To avoid format changes when the filters are
+ * enabled or disabled (which would prevent them from being turned on or off
+ * during streaming), the driver assumes all the filters are enabled when
+ * computing sink crop and source format limits.
+ *
+ * If a filter is disabled, additional cropping is automatically added at the
+ * preview engine input by the driver to avoid overflow at line and frame end.
+ * This is completely transparent for applications.
+ *
+ * Median filter                4 pixels
+ * Noise filter,
+ * Faulty pixels correction     4 pixels, 4 lines
+ * CFA filter                   4 pixels, 4 lines in Bayer mode
+ *                                        2 lines in other modes
+ * Color suppression            2 pixels
+ * or luma enhancement
+ * -------------------------------------------------------------
+ * Maximum total                14 pixels, 8 lines
+ *
+ * The color suppression and luma enhancement filters are applied after bayer to
+ * YUV conversion. They thus can crop one pixel on the left and one pixel on the
+ * right side of the image without changing the color pattern. When both those
+ * filters are disabled, the driver must crop the two pixels on the same side of
+ * the image to avoid changing the bayer pattern. The left margin is thus set to
+ * 8 pixels and the right margin to 6 pixels.
+ */
+
+#define PREV_MARGIN_LEFT        8
+#define PREV_MARGIN_RIGHT       6
+#define PREV_MARGIN_TOP         4
+#define PREV_MARGIN_BOTTOM      4
+
+#define PREV_MIN_IN_WIDTH       64
+#define PREV_MIN_IN_HEIGHT      8
+#define PREV_MAX_IN_HEIGHT      16384
+
+#define PREV_MIN_OUT_WIDTH      0
+#define PREV_MIN_OUT_HEIGHT     0
+#define PREV_MAX_OUT_WIDTH      1280
+#define PREV_MAX_OUT_WIDTH_ES2  3300
+#define PREV_MAX_OUT_WIDTH_3630 4096
 
 /*
  * Coeficient Tables for the submodules in Preview.
@@ -979,52 +1021,36 @@ static void preview_config_averager(struct isp_prev_device *prev, u8 average)
  * enabled when reporting source pad formats to userspace. If this assumption is
  * not true, rows and columns must be manually cropped at the preview engine
  * input to avoid overflows at the end of lines and frames.
+ *
+ * See the explanation at the PREV_MARGIN_* definitions for more details.
  */
 static void preview_config_input_size(struct isp_prev_device *prev)
 {
         struct isp_device *isp = to_isp_device(prev);
         struct prev_params *params = &prev->params;
-        struct v4l2_mbus_framefmt *format = &prev->formats[PREV_PAD_SINK];
-        unsigned int sph = 0;
-        unsigned int eph = format->width - 1;
-        unsigned int slv = 0;
-        unsigned int elv = format->height - 1;
-
-        if (prev->input == PREVIEW_INPUT_CCDC) {
-                sph += 2;
-                eph -= 2;
+        unsigned int sph = prev->crop.left;
+        unsigned int eph = prev->crop.left + prev->crop.width - 1;
+        unsigned int slv = prev->crop.top;
+        unsigned int elv = prev->crop.top + prev->crop.height - 1;
+
+        if (params->features & PREV_CFA) {
+                sph -= 2;
+                eph += 2;
+                slv -= 2;
+                elv += 2;
         }
-
-        /*
-         * Median filter        4 pixels
-         * Noise filter         4 pixels, 4 lines
-         * or faulty pixels correction
-         * CFA filter           4 pixels, 4 lines in Bayer mode
-         *                                2 lines in other modes
-         * Color suppression    2 pixels
-         * or luma enhancement
-         * -------------------------------------------------------------
-         * Maximum total        14 pixels, 8 lines
-         */
-
-        if (!(params->features & PREV_CFA)) {
-                sph += 2;
-                eph -= 2;
-                slv += 2;
-                elv -= 2;
+        if (params->features & (PREV_DEFECT_COR | PREV_NOISE_FILTER)) {
+                sph -= 2;
+                eph += 2;
+                slv -= 2;
+                elv += 2;
         }
-        if (!(params->features & (PREV_DEFECT_COR | PREV_NOISE_FILTER))) {
-                sph += 2;
-                eph -= 2;
-                slv += 2;
-                elv -= 2;
+        if (params->features & PREV_HORZ_MEDIAN_FILTER) {
+                sph -= 2;
+                eph += 2;
         }
-        if (!(params->features & PREV_HORZ_MEDIAN_FILTER)) {
-                sph += 2;
-                eph -= 2;
-        }
-        if (!(params->features & (PREV_CHROMA_SUPPRESS | PREV_LUMA_ENHANCE)))
-                sph += 2;
+        if (params->features & (PREV_CHROMA_SUPPRESS | PREV_LUMA_ENHANCE))
+                sph -= 2;
 
         isp_reg_writel(isp, (sph << ISPPRV_HORZ_INFO_SPH_SHIFT) | eph,
                        OMAP3_ISP_IOMEM_PREV, ISPPRV_HORZ_INFO);
@@ -1228,7 +1254,6 @@ static void preview_init_params(struct isp_prev_device *prev)
         /* Init values */
         params->contrast = ISPPRV_CONTRAST_DEF * ISPPRV_CONTRAST_UNITS;
         params->brightness = ISPPRV_BRIGHT_DEF * ISPPRV_BRIGHT_UNITS;
-        params->average = NO_AVE;
         params->cfa.format = OMAP3ISP_CFAFMT_BAYER;
         memcpy(params->cfa.table, cfa_coef_table,
                sizeof(params->cfa.table));
@@ -1281,14 +1306,14 @@ static unsigned int preview_max_out_width(struct isp_prev_device *prev)
 
         switch (isp->revision) {
         case ISP_REVISION_1_0:
-                return ISPPRV_MAXOUTPUT_WIDTH;
+                return PREV_MAX_OUT_WIDTH;
 
         case ISP_REVISION_2_0:
         default:
-                return ISPPRV_MAXOUTPUT_WIDTH_ES2;
+                return PREV_MAX_OUT_WIDTH_ES2;
 
         case ISP_REVISION_15_0:
-                return ISPPRV_MAXOUTPUT_WIDTH_3630;
+                return PREV_MAX_OUT_WIDTH_3630;
         }
 }
 
@@ -1296,8 +1321,6 @@ static void preview_configure(struct isp_prev_device *prev)
 {
         struct isp_device *isp = to_isp_device(prev);
         struct v4l2_mbus_framefmt *format;
-        unsigned int max_out_width;
-        unsigned int format_avg;
 
         preview_setup_hw(prev);
 
@@ -1335,10 +1358,7 @@ static void preview_configure(struct isp_prev_device *prev)
                 preview_config_outlineoffset(prev,
                                 ALIGN(format->width, 0x10) * 2);
 
-        max_out_width = preview_max_out_width(prev);
-
-        format_avg = fls(DIV_ROUND_UP(format->width, max_out_width) - 1);
-        preview_config_averager(prev, format_avg);
+        preview_config_averager(prev, 0);
         preview_config_ycpos(prev, format->code);
 }
 
@@ -1597,6 +1617,16 @@ __preview_get_format(struct isp_prev_device *prev, struct v4l2_subdev_fh *fh,
                 return &prev->formats[pad];
 }
 
+static struct v4l2_rect *
+__preview_get_crop(struct isp_prev_device *prev, struct v4l2_subdev_fh *fh,
+                   enum v4l2_subdev_format_whence which)
+{
+        if (which == V4L2_SUBDEV_FORMAT_TRY)
+                return v4l2_subdev_get_try_crop(fh, PREV_PAD_SINK);
+        else
+                return &prev->crop;
+}
+
 /* previewer format descriptions */
 static const unsigned int preview_input_fmts[] = {
         V4L2_MBUS_FMT_SGRBG10_1X10,
@@ -1611,24 +1641,25 @@ static const unsigned int preview_output_fmts[] = {
 };
 
 /*
- * preview_try_format - Handle try format by pad subdev method
- * @prev: ISP preview device
- * @fh : V4L2 subdev file handle
- * @pad: pad num
- * @fmt: pointer to v4l2 format structure
+ * preview_try_format - Validate a format
+ * @prev: ISP preview engine
+ * @fh: V4L2 subdev file handle
+ * @pad: pad number
+ * @fmt: format to be validated
+ * @which: try/active format selector
+ *
+ * Validate and adjust the given format for the given pad based on the preview
+ * engine limits and the format and crop rectangles on other pads.
  */
 static void preview_try_format(struct isp_prev_device *prev,
                                struct v4l2_subdev_fh *fh, unsigned int pad,
                                struct v4l2_mbus_framefmt *fmt,
                                enum v4l2_subdev_format_whence which)
 {
-        struct v4l2_mbus_framefmt *format;
-        unsigned int max_out_width;
         enum v4l2_mbus_pixelcode pixelcode;
+        struct v4l2_rect *crop;
         unsigned int i;
 
-        max_out_width = preview_max_out_width(prev);
-
         switch (pad) {
         case PREV_PAD_SINK:
                 /* When reading data from the CCDC, the input size has already
@@ -1641,10 +1672,11 @@ static void preview_try_format(struct isp_prev_device *prev,
                  * filter array interpolation.
                  */
                 if (prev->input == PREVIEW_INPUT_MEMORY) {
-                        fmt->width = clamp_t(u32, fmt->width, PREV_MIN_WIDTH,
-                                             max_out_width * 8);
-                        fmt->height = clamp_t(u32, fmt->height, PREV_MIN_HEIGHT,
-                                              PREV_MAX_HEIGHT);
+                        fmt->width = clamp_t(u32, fmt->width, PREV_MIN_IN_WIDTH,
+                                             preview_max_out_width(prev));
+                        fmt->height = clamp_t(u32, fmt->height,
+                                              PREV_MIN_IN_HEIGHT,
+                                              PREV_MAX_IN_HEIGHT);
                 }
 
                 fmt->colorspace = V4L2_COLORSPACE_SRGB;
@@ -1661,15 +1693,8 @@ static void preview_try_format(struct isp_prev_device *prev,
 
         case PREV_PAD_SOURCE:
                 pixelcode = fmt->code;
-                format = __preview_get_format(prev, fh, PREV_PAD_SINK, which);
-                memcpy(fmt, format, sizeof(*fmt));
+                *fmt = *__preview_get_format(prev, fh, PREV_PAD_SINK, which);
 
-                /* The preview module output size is configurable through the
-                 * input interface (horizontal and vertical cropping) and the
-                 * averager (horizontal scaling by 1/1, 1/2, 1/4 or 1/8). In
-                 * spite of this, hardcode the output size to the biggest
-                 * possible value for simplicity reasons.
-                 */
                 switch (pixelcode) {
                 case V4L2_MBUS_FMT_YUYV8_1X16:
                 case V4L2_MBUS_FMT_UYVY8_1X16:
@@ -1681,31 +1706,14 @@ static void preview_try_format(struct isp_prev_device *prev,
                         break;
                 }
 
-                /* The TRM states (12.1.4.7.1.2) that 2 pixels must be cropped
-                 * from the left and right sides when the input source is the
-                 * CCDC. This seems not to be needed in practice, investigation
-                 * is required.
-                 */
-                if (prev->input == PREVIEW_INPUT_CCDC)
-                        fmt->width -= 4;
-
-                /* The preview module can output a maximum of 3312 pixels
-                 * horizontally due to fixed memory-line sizes. Compute the
-                 * horizontal averaging factor accordingly. Note that the limit
-                 * applies to the noise filter and CFA interpolation blocks, so
-                 * it doesn't take cropping by further blocks into account.
-                 *
-                 * ES 1.0 hardware revision is limited to 1280 pixels
-                 * horizontally.
-                 */
-                fmt->width >>= fls(DIV_ROUND_UP(fmt->width, max_out_width) - 1);
-
-                /* Assume that all blocks are enabled and crop pixels and lines
-                 * accordingly. See preview_config_input_size() for more
-                 * information.
+                /* The preview module output size is configurable through the
+                 * averager (horizontal scaling by 1/1, 1/2, 1/4 or 1/8). This
+                 * is not supported yet, hardcode the output size to the crop
+                 * rectangle size.
                  */
-                fmt->width -= 14;
-                fmt->height -= 8;
+                crop = __preview_get_crop(prev, fh, which);
+                fmt->width = crop->width;
+                fmt->height = crop->height;
 
                 fmt->colorspace = V4L2_COLORSPACE_JPEG;
                 break;
@@ -1715,6 +1723,49 @@ static void preview_try_format(struct isp_prev_device *prev,
 }
 
 /*
+ * preview_try_crop - Validate a crop rectangle
+ * @prev: ISP preview engine
+ * @sink: format on the sink pad
+ * @crop: crop rectangle to be validated
+ *
+ * The preview engine crops lines and columns for its internal operation,
+ * depending on which filters are enabled. Enforce minimum crop margins to
+ * handle that transparently for userspace.
+ *
+ * See the explanation at the PREV_MARGIN_* definitions for more details.
+ */
+static void preview_try_crop(struct isp_prev_device *prev,
+                             const struct v4l2_mbus_framefmt *sink,
+                             struct v4l2_rect *crop)
+{
+        unsigned int left = PREV_MARGIN_LEFT;
+        unsigned int right = sink->width - PREV_MARGIN_RIGHT;
+        unsigned int top = PREV_MARGIN_TOP;
+        unsigned int bottom = sink->height - PREV_MARGIN_BOTTOM;
+
+        /* When processing data on-the-fly from the CCDC, at least 2 pixels must
+         * be cropped from the left and right sides of the image. As we don't
+         * know which filters will be enabled, increase the left and right
+         * margins by two.
+         */
+        if (prev->input == PREVIEW_INPUT_CCDC) {
+                left += 2;
+                right -= 2;
+        }
+
+        /* Restrict left/top to even values to keep the Bayer pattern. */
+        crop->left &= ~1;
+        crop->top &= ~1;
+
+        crop->left = clamp_t(u32, crop->left, left, right - PREV_MIN_OUT_WIDTH);
+        crop->top = clamp_t(u32, crop->top, top, bottom - PREV_MIN_OUT_HEIGHT);
+        crop->width = clamp_t(u32, crop->width, PREV_MIN_OUT_WIDTH,
+                              right - crop->left);
+        crop->height = clamp_t(u32, crop->height, PREV_MIN_OUT_HEIGHT,
+                               bottom - crop->top);
+}
+
+/*
  * preview_enum_mbus_code - Handle pixel format enumeration
  * @sd     : pointer to v4l2 subdev structure
  * @fh     : V4L2 subdev file handle
@@ -1776,6 +1827,60 @@ static int preview_enum_frame_size(struct v4l2_subdev *sd,
 }
 
 /*
+ * preview_get_crop - Retrieve the crop rectangle on a pad
+ * @sd: ISP preview V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ * @crop: crop rectangle
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int preview_get_crop(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+                            struct v4l2_subdev_crop *crop)
+{
+        struct isp_prev_device *prev = v4l2_get_subdevdata(sd);
+
+        /* Cropping is only supported on the sink pad. */
+        if (crop->pad != PREV_PAD_SINK)
+                return -EINVAL;
+
+        crop->rect = *__preview_get_crop(prev, fh, crop->which);
+        return 0;
+}
+
+/*
+ * preview_set_crop - Retrieve the crop rectangle on a pad
+ * @sd: ISP preview V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ * @crop: crop rectangle
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int preview_set_crop(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+                            struct v4l2_subdev_crop *crop)
+{
+        struct isp_prev_device *prev = v4l2_get_subdevdata(sd);
+        struct v4l2_mbus_framefmt *format;
+
+        /* Cropping is only supported on the sink pad. */
+        if (crop->pad != PREV_PAD_SINK)
+                return -EINVAL;
+
+        /* The crop rectangle can't be changed while streaming. */
+        if (prev->state != ISP_PIPELINE_STREAM_STOPPED)
+                return -EBUSY;
+
+        format = __preview_get_format(prev, fh, PREV_PAD_SINK, crop->which);
+        preview_try_crop(prev, format, &crop->rect);
+        *__preview_get_crop(prev, fh, crop->which) = crop->rect;
+
+        /* Update the source format. */
+        format = __preview_get_format(prev, fh, PREV_PAD_SOURCE, crop->which);
+        preview_try_format(prev, fh, PREV_PAD_SOURCE, format, crop->which);
+
+        return 0;
+}
+
+/*
  * preview_get_format - Handle get format by pads subdev method
  * @sd : pointer to v4l2 subdev structure
  * @fh : V4L2 subdev file handle
@@ -1808,6 +1913,7 @@ static int preview_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 {
         struct isp_prev_device *prev = v4l2_get_subdevdata(sd);
         struct v4l2_mbus_framefmt *format;
+        struct v4l2_rect *crop;
 
         format = __preview_get_format(prev, fh, fmt->pad, fmt->which);
         if (format == NULL)
@@ -1818,9 +1924,18 @@ static int preview_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 
         /* Propagate the format from sink to source */
         if (fmt->pad == PREV_PAD_SINK) {
+                /* Reset the crop rectangle. */
+                crop = __preview_get_crop(prev, fh, fmt->which);
+                crop->left = 0;
+                crop->top = 0;
+                crop->width = fmt->format.width;
+                crop->height = fmt->format.height;
+
+                preview_try_crop(prev, &fmt->format, crop);
+
+                /* Update the source format. */
                 format = __preview_get_format(prev, fh, PREV_PAD_SOURCE,
                                               fmt->which);
-                *format = fmt->format;
                 preview_try_format(prev, fh, PREV_PAD_SOURCE, format,
                                    fmt->which);
         }
@@ -1869,6 +1984,8 @@ static const struct v4l2_subdev_pad_ops preview_v4l2_pad_ops = {
         .enum_frame_size = preview_enum_frame_size,
         .get_fmt = preview_get_format,
         .set_fmt = preview_set_format,
+        .get_crop = preview_get_crop,
+        .set_crop = preview_set_crop,
 };
 
 /* subdev operations */
@@ -1966,8 +2083,44 @@ static const struct media_entity_operations preview_media_ops = {
         .link_setup = preview_link_setup,
 };
 
+void omap3isp_preview_unregister_entities(struct isp_prev_device *prev)
+{
+        v4l2_device_unregister_subdev(&prev->subdev);
+        omap3isp_video_unregister(&prev->video_in);
+        omap3isp_video_unregister(&prev->video_out);
+}
+
+int omap3isp_preview_register_entities(struct isp_prev_device *prev,
+        struct v4l2_device *vdev)
+{
+        int ret;
+
+        /* Register the subdev and video nodes. */
+        ret = v4l2_device_register_subdev(vdev, &prev->subdev);
+        if (ret < 0)
+                goto error;
+
+        ret = omap3isp_video_register(&prev->video_in, vdev);
+        if (ret < 0)
+                goto error;
+
+        ret = omap3isp_video_register(&prev->video_out, vdev);
+        if (ret < 0)
+                goto error;
+
+        return 0;
+
+error:
+        omap3isp_preview_unregister_entities(prev);
+        return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP previewer initialisation and cleanup
+ */
+
 /*
- * review_init_entities - Initialize subdev and media entity.
+ * preview_init_entities - Initialize subdev and media entity.
  * @prev : Pointer to preview structure
  * return -ENOMEM or zero on success
  */
@@ -2024,69 +2177,34 @@ static int preview_init_entities(struct isp_prev_device *prev)
 
         ret = omap3isp_video_init(&prev->video_in, "preview");
         if (ret < 0)
-                return ret;
+                goto error_video_in;
 
         ret = omap3isp_video_init(&prev->video_out, "preview");
         if (ret < 0)
-                return ret;
+                goto error_video_out;
 
         /* Connect the video nodes to the previewer subdev. */
         ret = media_entity_create_link(&prev->video_in.video.entity, 0,
                         &prev->subdev.entity, PREV_PAD_SINK, 0);
         if (ret < 0)
-                return ret;
+                goto error_link;
 
         ret = media_entity_create_link(&prev->subdev.entity, PREV_PAD_SOURCE,
                         &prev->video_out.video.entity, 0, 0);
         if (ret < 0)
-                return ret;
+                goto error_link;
 
         return 0;
-}
 
-void omap3isp_preview_unregister_entities(struct isp_prev_device *prev)
-{
+error_link:
+        omap3isp_video_cleanup(&prev->video_out);
+error_video_out:
+        omap3isp_video_cleanup(&prev->video_in);
+error_video_in:
         media_entity_cleanup(&prev->subdev.entity);
-
-        v4l2_device_unregister_subdev(&prev->subdev);
-        v4l2_ctrl_handler_free(&prev->ctrls);
-        omap3isp_video_unregister(&prev->video_in);
-        omap3isp_video_unregister(&prev->video_out);
-}
-
-int omap3isp_preview_register_entities(struct isp_prev_device *prev,
-        struct v4l2_device *vdev)
-{
-        int ret;
-
-        /* Register the subdev and video nodes. */
-        ret = v4l2_device_register_subdev(vdev, &prev->subdev);
-        if (ret < 0)
-                goto error;
-
-        ret = omap3isp_video_register(&prev->video_in, vdev);
-        if (ret < 0)
-                goto error;
-
-        ret = omap3isp_video_register(&prev->video_out, vdev);
-        if (ret < 0)
-                goto error;
-
-        return 0;
-
-error:
-        omap3isp_preview_unregister_entities(prev);
         return ret;
 }
 
-/* -----------------------------------------------------------------------------
- * ISP previewer initialisation and cleanup
- */
-
-void omap3isp_preview_cleanup(struct isp_device *isp)
-{
-}
-
 /*
  * isp_preview_init - Previewer initialization.
  * @dev : Pointer to ISP device
@@ -2095,19 +2213,20 @@ void omap3isp_preview_cleanup(struct isp_device *isp)
 int omap3isp_preview_init(struct isp_device *isp)
 {
         struct isp_prev_device *prev = &isp->isp_prev;
-        int ret;
 
         spin_lock_init(&prev->lock);
         init_waitqueue_head(&prev->wait);
         preview_init_params(prev);
 
-        ret = preview_init_entities(prev);
-        if (ret < 0)
-                goto out;
+        return preview_init_entities(prev);
+}
 
-out:
-        if (ret)
-                omap3isp_preview_cleanup(isp);
+void omap3isp_preview_cleanup(struct isp_device *isp)
+{
+        struct isp_prev_device *prev = &isp->isp_prev;
 
-        return ret;
+        v4l2_ctrl_handler_free(&prev->ctrls);
+        omap3isp_video_cleanup(&prev->video_in);
+        omap3isp_video_cleanup(&prev->video_out);
+        media_entity_cleanup(&prev->subdev.entity);
 }
diff --git a/drivers/media/video/omap3isp/isppreview.h b/drivers/media/video/omap3isp/isppreview.h
index fa943bd05c7f..f54e775c2df4 100644
--- a/drivers/media/video/omap3isp/isppreview.h
+++ b/drivers/media/video/omap3isp/isppreview.h
@@ -45,11 +45,6 @@
 #define ISPPRV_CONTRAST_HIGH            0xFF
 #define ISPPRV_CONTRAST_UNITS           0x1
 
-#define NO_AVE                          0x0
-#define AVE_2_PIX                       0x1
-#define AVE_4_PIX                       0x2
-#define AVE_8_PIX                       0x3
-
 /* Features list */
 #define PREV_LUMA_ENHANCE               OMAP3ISP_PREV_LUMAENH
 #define PREV_INVERSE_ALAW               OMAP3ISP_PREV_INVALAW
@@ -106,7 +101,6 @@ enum preview_ycpos_mode {
  * @rgb2ycbcr: RGB to ycbcr parameters.
  * @hmed: Horizontal median filter.
  * @yclimit: YC limits parameters.
- * @average: Downsampling rate for averager.
  * @contrast: Contrast.
  * @brightness: Brightness.
  */
@@ -124,7 +118,6 @@ struct prev_params {
         struct omap3isp_prev_csc rgb2ycbcr;
         struct omap3isp_prev_hmed hmed;
         struct omap3isp_prev_yclimit yclimit;
-        u8 average;
         u8 contrast;
         u8 brightness;
 };
@@ -159,6 +152,7 @@ struct isptables_update {
  * @subdev: V4L2 subdevice
  * @pads: Media entity pads
  * @formats: Active formats at the subdev pad
+ * @crop: Active crop rectangle
  * @input: Module currently connected to the input pad
  * @output: Bitmask of the active output
  * @video_in: Input video entity
@@ -177,6 +171,7 @@ struct isp_prev_device {
         struct v4l2_subdev subdev;
         struct media_pad pads[PREV_PADS_NUM];
         struct v4l2_mbus_framefmt formats[PREV_PADS_NUM];
+        struct v4l2_rect crop;
 
         struct v4l2_ctrl_handler ctrls;
 
diff --git a/drivers/media/video/omap3isp/ispreg.h b/drivers/media/video/omap3isp/ispreg.h
index 69f6af6f6b9c..084ea77d65a7 100644
--- a/drivers/media/video/omap3isp/ispreg.h
+++ b/drivers/media/video/omap3isp/ispreg.h
@@ -402,9 +402,6 @@
 #define ISPPRV_YENH_TABLE_ADDR          0x1000
 #define ISPPRV_CFA_TABLE_ADDR           0x1400
 
-#define ISPPRV_MAXOUTPUT_WIDTH          1280
-#define ISPPRV_MAXOUTPUT_WIDTH_ES2      3300
-#define ISPPRV_MAXOUTPUT_WIDTH_3630     4096
 #define ISPRSZ_MIN_OUTPUT               64
 #define ISPRSZ_MAX_OUTPUT               3312
 
diff --git a/drivers/media/video/omap3isp/ispresizer.c b/drivers/media/video/omap3isp/ispresizer.c
index 0bb0f8cd36f5..50e593bfcfaf 100644
--- a/drivers/media/video/omap3isp/ispresizer.c
+++ b/drivers/media/video/omap3isp/ispresizer.c
@@ -1608,6 +1608,42 @@ static const struct media_entity_operations resizer_media_ops = {
         .link_setup = resizer_link_setup,
 };
 
+void omap3isp_resizer_unregister_entities(struct isp_res_device *res)
+{
+        v4l2_device_unregister_subdev(&res->subdev);
+        omap3isp_video_unregister(&res->video_in);
+        omap3isp_video_unregister(&res->video_out);
+}
+
+int omap3isp_resizer_register_entities(struct isp_res_device *res,
+                                       struct v4l2_device *vdev)
+{
+        int ret;
+
+        /* Register the subdev and video nodes. */
+        ret = v4l2_device_register_subdev(vdev, &res->subdev);
+        if (ret < 0)
+                goto error;
+
+        ret = omap3isp_video_register(&res->video_in, vdev);
+        if (ret < 0)
+                goto error;
+
+        ret = omap3isp_video_register(&res->video_out, vdev);
+        if (ret < 0)
+                goto error;
+
+        return 0;
+
+error:
+        omap3isp_resizer_unregister_entities(res);
+        return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP resizer initialization and cleanup
+ */
+
 /*
  * resizer_init_entities - Initialize resizer subdev and media entity.
  * @res : Pointer to resizer device structure
@@ -1652,68 +1688,34 @@ static int resizer_init_entities(struct isp_res_device *res)
 
         ret = omap3isp_video_init(&res->video_in, "resizer");
         if (ret < 0)
-                return ret;
+                goto error_video_in;
 
         ret = omap3isp_video_init(&res->video_out, "resizer");
         if (ret < 0)
-                return ret;
+                goto error_video_out;
 
         /* Connect the video nodes to the resizer subdev. */
         ret = media_entity_create_link(&res->video_in.video.entity, 0,
                         &res->subdev.entity, RESZ_PAD_SINK, 0);
         if (ret < 0)
-                return ret;
+                goto error_link;
 
         ret = media_entity_create_link(&res->subdev.entity, RESZ_PAD_SOURCE,
                         &res->video_out.video.entity, 0, 0);
         if (ret < 0)
-                return ret;
+                goto error_link;
 
         return 0;
-}
 
-void omap3isp_resizer_unregister_entities(struct isp_res_device *res)
-{
+error_link:
+        omap3isp_video_cleanup(&res->video_out);
+error_video_out:
+        omap3isp_video_cleanup(&res->video_in);
+error_video_in:
         media_entity_cleanup(&res->subdev.entity);
-
-        v4l2_device_unregister_subdev(&res->subdev);
-        omap3isp_video_unregister(&res->video_in);
-        omap3isp_video_unregister(&res->video_out);
-}
-
-int omap3isp_resizer_register_entities(struct isp_res_device *res,
-                                       struct v4l2_device *vdev)
-{
-        int ret;
-
-        /* Register the subdev and video nodes. */
-        ret = v4l2_device_register_subdev(vdev, &res->subdev);
-        if (ret < 0)
-                goto error;
-
-        ret = omap3isp_video_register(&res->video_in, vdev);
-        if (ret < 0)
-                goto error;
-
-        ret = omap3isp_video_register(&res->video_out, vdev);
-        if (ret < 0)
-                goto error;
-
-        return 0;
-
-error:
-        omap3isp_resizer_unregister_entities(res);
         return ret;
 }
 
-/* -----------------------------------------------------------------------------
- * ISP resizer initialization and cleanup
- */
-
-void omap3isp_resizer_cleanup(struct isp_device *isp)
-{
-}
-
 /*
  * isp_resizer_init - Resizer initialization.
  * @isp : Pointer to ISP device
@@ -1722,17 +1724,17 @@ void omap3isp_resizer_cleanup(struct isp_device *isp)
 int omap3isp_resizer_init(struct isp_device *isp)
 {
         struct isp_res_device *res = &isp->isp_res;
-        int ret;
 
         init_waitqueue_head(&res->wait);
         atomic_set(&res->stopping, 0);
-        ret = resizer_init_entities(res);
-        if (ret < 0)
-                goto out;
+        return resizer_init_entities(res);
+}
 
-out:
-        if (ret)
-                omap3isp_resizer_cleanup(isp);
+void omap3isp_resizer_cleanup(struct isp_device *isp)
+{
+        struct isp_res_device *res = &isp->isp_res;
 
-        return ret;
+        omap3isp_video_cleanup(&res->video_in);
+        omap3isp_video_cleanup(&res->video_out);
+        media_entity_cleanup(&res->subdev.entity);
 }
diff --git a/drivers/media/video/omap3isp/ispstat.c b/drivers/media/video/omap3isp/ispstat.c
index 732905552261..68d539456c55 100644
--- a/drivers/media/video/omap3isp/ispstat.c
+++ b/drivers/media/video/omap3isp/ispstat.c
@@ -1023,24 +1023,6 @@ void omap3isp_stat_dma_isr(struct ispstat *stat)
         __stat_isr(stat, 1);
 }
 
-static int isp_stat_init_entities(struct ispstat *stat, const char *name,
-                                  const struct v4l2_subdev_ops *sd_ops)
-{
-        struct v4l2_subdev *subdev = &stat->subdev;
-        struct media_entity *me = &subdev->entity;
-
-        v4l2_subdev_init(subdev, sd_ops);
-        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;
-        v4l2_set_subdevdata(subdev, stat);
-
-        stat->pad.flags = MEDIA_PAD_FL_SINK;
-        me->ops = NULL;
-
-        return media_entity_init(me, 1, &stat->pad, 0);
-}
-
 int omap3isp_stat_subscribe_event(struct v4l2_subdev *subdev,
                                   struct v4l2_fh *fh,
                                   struct v4l2_event_subscription *sub)
@@ -1062,7 +1044,6 @@ int omap3isp_stat_unsubscribe_event(struct v4l2_subdev *subdev,
 
 void omap3isp_stat_unregister_entities(struct ispstat *stat)
 {
-        media_entity_cleanup(&stat->subdev.entity);
         v4l2_device_unregister_subdev(&stat->subdev);
 }
 
@@ -1072,21 +1053,50 @@ int omap3isp_stat_register_entities(struct ispstat *stat,
         return v4l2_device_register_subdev(vdev, &stat->subdev);
 }
 
+static int isp_stat_init_entities(struct ispstat *stat, const char *name,
+                                  const struct v4l2_subdev_ops *sd_ops)
+{
+        struct v4l2_subdev *subdev = &stat->subdev;
+        struct media_entity *me = &subdev->entity;
+
+        v4l2_subdev_init(subdev, sd_ops);
+        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;
+        v4l2_set_subdevdata(subdev, stat);
+
+        stat->pad.flags = MEDIA_PAD_FL_SINK;
+        me->ops = NULL;
+
+        return media_entity_init(me, 1, &stat->pad, 0);
+}
+
 int omap3isp_stat_init(struct ispstat *stat, const char *name,
                        const struct v4l2_subdev_ops *sd_ops)
 {
+        int ret;
+
         stat->buf = kcalloc(STAT_MAX_BUFS, sizeof(*stat->buf), GFP_KERNEL);
         if (!stat->buf)
                 return -ENOMEM;
+
         isp_stat_buf_clear(stat);
         mutex_init(&stat->ioctl_lock);
         atomic_set(&stat->buf_err, 0);
 
-        return isp_stat_init_entities(stat, name, sd_ops);
+        ret = isp_stat_init_entities(stat, name, sd_ops);
+        if (ret < 0) {
+                mutex_destroy(&stat->ioctl_lock);
+                kfree(stat->buf);
+        }
+
+        return ret;
 }
 
-void omap3isp_stat_free(struct ispstat *stat)
+void omap3isp_stat_cleanup(struct ispstat *stat)
 {
+        media_entity_cleanup(&stat->subdev.entity);
+        mutex_destroy(&stat->ioctl_lock);
         isp_stat_bufs_free(stat);
         kfree(stat->buf);
 }
diff --git a/drivers/media/video/omap3isp/ispstat.h b/drivers/media/video/omap3isp/ispstat.h
index d86da94fa50d..9b7c8654dc8a 100644
--- a/drivers/media/video/omap3isp/ispstat.h
+++ b/drivers/media/video/omap3isp/ispstat.h
@@ -144,7 +144,7 @@ int omap3isp_stat_request_statistics(struct ispstat *stat,
                                      struct omap3isp_stat_data *data);
 int omap3isp_stat_init(struct ispstat *stat, const char *name,
                        const struct v4l2_subdev_ops *sd_ops);
-void omap3isp_stat_free(struct ispstat *stat);
+void omap3isp_stat_cleanup(struct ispstat *stat);
 int omap3isp_stat_subscribe_event(struct v4l2_subdev *subdev,
                                   struct v4l2_fh *fh,
                                   struct v4l2_event_subscription *sub);
diff --git a/drivers/media/video/omap3isp/ispvideo.c b/drivers/media/video/omap3isp/ispvideo.c
index 0cb8a9f9d675..d1000723c5ae 100644
--- a/drivers/media/video/omap3isp/ispvideo.c
+++ b/drivers/media/video/omap3isp/ispvideo.c
@@ -1325,6 +1325,13 @@ int omap3isp_video_init(struct isp_video *video, const char *name)
         return 0;
 }
 
+void omap3isp_video_cleanup(struct isp_video *video)
+{
+        media_entity_cleanup(&video->video.entity);
+        mutex_destroy(&video->stream_lock);
+        mutex_destroy(&video->mutex);
+}
+
 int omap3isp_video_register(struct isp_video *video, struct v4l2_device *vdev)
 {
         int ret;
@@ -1341,8 +1348,6 @@ int omap3isp_video_register(struct isp_video *video, struct v4l2_device *vdev)
 
 void omap3isp_video_unregister(struct isp_video *video)
 {
-        if (video_is_registered(&video->video)) {
-                media_entity_cleanup(&video->video.entity);
+        if (video_is_registered(&video->video))
                 video_unregister_device(&video->video);
-        }
 }
diff --git a/drivers/media/video/omap3isp/ispvideo.h b/drivers/media/video/omap3isp/ispvideo.h
index 53160aa24e6e..08cbfa144e6e 100644
--- a/drivers/media/video/omap3isp/ispvideo.h
+++ b/drivers/media/video/omap3isp/ispvideo.h
@@ -190,6 +190,7 @@ struct isp_video_fh {
                                 container_of(q, struct isp_video_fh, queue)
 
 int omap3isp_video_init(struct isp_video *video, const char *name);
+void omap3isp_video_cleanup(struct isp_video *video);
 int omap3isp_video_register(struct isp_video *video,
                             struct v4l2_device *vdev);
 void omap3isp_video_unregister(struct isp_video *video);
diff --git a/drivers/media/video/ov2640.c b/drivers/media/video/ov2640.c
index 9ce2fa037b94..b5247cb64fde 100644
--- a/drivers/media/video/ov2640.c
+++ b/drivers/media/video/ov2640.c
@@ -18,11 +18,13 @@
 #include <linux/i2c.h>
 #include <linux/slab.h>
 #include <linux/delay.h>
+#include <linux/v4l2-mediabus.h>
 #include <linux/videodev2.h>
+
+#include <media/soc_camera.h>
 #include <media/v4l2-chip-ident.h>
 #include <media/v4l2-subdev.h>
-#include <media/soc_camera.h>
-#include <media/soc_mediabus.h>
+#include <media/v4l2-ctrls.h>
 
 #define VAL_SET(x, mask, rshift, lshift)  \
                 ((((x) >> rshift) & mask) << lshift)
@@ -299,12 +301,10 @@ struct ov2640_win_size {
 
 struct ov2640_priv {
         struct v4l2_subdev              subdev;
-        struct ov2640_camera_info       *info;
+        struct v4l2_ctrl_handler        hdl;
         enum v4l2_mbus_pixelcode        cfmt_code;
         const struct ov2640_win_size    *win;
         int                             model;
-        u16                             flag_vflip:1;
-        u16                             flag_hflip:1;
 };
 
 /*
@@ -610,29 +610,6 @@ static enum v4l2_mbus_pixelcode ov2640_codes[] = {
 };
 
 /*
- * Supported controls
- */
-static const struct v4l2_queryctrl ov2640_controls[] = {
-        {
-                .id             = V4L2_CID_VFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Vertically",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        }, {
-                .id             = V4L2_CID_HFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Horizontally",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        },
-};
-
-/*
  * General functions
  */
 static struct ov2640_priv *to_ov2640(const struct i2c_client *client)
@@ -701,81 +678,23 @@ static int ov2640_s_stream(struct v4l2_subdev *sd, int enable)
         return 0;
 }
 
-static int ov2640_set_bus_param(struct soc_camera_device *icd,
-                                unsigned long flags)
-{
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-        unsigned long width_flag = flags & SOCAM_DATAWIDTH_MASK;
-
-        /* Only one width bit may be set */
-        if (!is_power_of_2(width_flag))
-                return -EINVAL;
-
-        if (icl->set_bus_param)
-                return icl->set_bus_param(icl, width_flag);
-
-        /*
-         * Without board specific bus width settings we support only the
-         * sensors native bus width witch are tested working
-         */
-        if (width_flag & (SOCAM_DATAWIDTH_10 | SOCAM_DATAWIDTH_8))
-                return 0;
-
-        return 0;
-}
-
-static unsigned long ov2640_query_bus_param(struct soc_camera_device *icd)
-{
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-        unsigned long flags = SOCAM_PCLK_SAMPLE_RISING | SOCAM_MASTER |
-                SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_HIGH |
-                SOCAM_DATA_ACTIVE_HIGH;
-
-        if (icl->query_bus_param)
-                flags |= icl->query_bus_param(icl) & SOCAM_DATAWIDTH_MASK;
-        else
-                flags |= SOCAM_DATAWIDTH_10;
-
-        return soc_camera_apply_sensor_flags(icl, flags);
-}
-
-static int ov2640_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int ov2640_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+        struct v4l2_subdev *sd =
+                &container_of(ctrl->handler, struct ov2640_priv, hdl)->subdev;
         struct i2c_client  *client = v4l2_get_subdevdata(sd);
-        struct ov2640_priv *priv = to_ov2640(client);
-
-        switch (ctrl->id) {
-        case V4L2_CID_VFLIP:
-                ctrl->value = priv->flag_vflip;
-                break;
-        case V4L2_CID_HFLIP:
-                ctrl->value = priv->flag_hflip;
-                break;
-        }
-        return 0;
-}
-
-static int ov2640_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-        struct i2c_client  *client = v4l2_get_subdevdata(sd);
-        struct ov2640_priv *priv = to_ov2640(client);
-        int ret = 0;
         u8 val;
 
         switch (ctrl->id) {
         case V4L2_CID_VFLIP:
-                val = ctrl->value ? REG04_VFLIP_IMG : 0x00;
-                priv->flag_vflip = ctrl->value ? 1 : 0;
-                ret = ov2640_mask_set(client, REG04, REG04_VFLIP_IMG, val);
-                break;
+                val = ctrl->val ? REG04_VFLIP_IMG : 0x00;
+                return ov2640_mask_set(client, REG04, REG04_VFLIP_IMG, val);
         case V4L2_CID_HFLIP:
-                val = ctrl->value ? REG04_HFLIP_IMG : 0x00;
-                priv->flag_hflip = ctrl->value ? 1 : 0;
-                ret = ov2640_mask_set(client, REG04, REG04_HFLIP_IMG, val);
-                break;
+                val = ctrl->val ? REG04_HFLIP_IMG : 0x00;
+                return ov2640_mask_set(client, REG04, REG04_HFLIP_IMG, val);
         }
 
-        return ret;
+        return -EINVAL;
 }
 
 static int ov2640_g_chip_ident(struct v4l2_subdev *sd,
@@ -1023,18 +942,13 @@ static int ov2640_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
         return 0;
 }
 
-static int ov2640_video_probe(struct soc_camera_device *icd,
-                              struct i2c_client *client)
+static int ov2640_video_probe(struct i2c_client *client)
 {
         struct ov2640_priv *priv = to_ov2640(client);
         u8 pid, ver, midh, midl;
         const char *devname;
         int ret;
 
-        /* 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
          */
@@ -1060,22 +974,17 @@ static int ov2640_video_probe(struct soc_camera_device *icd,
                  "%s Product ID %0x:%0x Manufacturer ID %x:%x\n",
                  devname, pid, ver, midh, midl);
 
-        return 0;
+        return v4l2_ctrl_handler_setup(&priv->hdl);
 
 err:
         return ret;
 }
 
-static struct soc_camera_ops ov2640_ops = {
-        .set_bus_param          = ov2640_set_bus_param,
-        .query_bus_param        = ov2640_query_bus_param,
-        .controls               = ov2640_controls,
-        .num_controls           = ARRAY_SIZE(ov2640_controls),
+static const struct v4l2_ctrl_ops ov2640_ctrl_ops = {
+        .s_ctrl = ov2640_s_ctrl,
 };
 
 static struct v4l2_subdev_core_ops ov2640_subdev_core_ops = {
-        .g_ctrl         = ov2640_g_ctrl,
-        .s_ctrl         = ov2640_s_ctrl,
         .g_chip_ident   = ov2640_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register     = ov2640_g_register,
@@ -1083,6 +992,21 @@ static struct v4l2_subdev_core_ops ov2640_subdev_core_ops = {
 #endif
 };
 
+static int ov2640_g_mbus_config(struct v4l2_subdev *sd,
+                                struct v4l2_mbus_config *cfg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+        cfg->flags = V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_MASTER |
+                V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+                V4L2_MBUS_DATA_ACTIVE_HIGH;
+        cfg->type = V4L2_MBUS_PARALLEL;
+        cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+        return 0;
+}
+
 static struct v4l2_subdev_video_ops ov2640_subdev_video_ops = {
         .s_stream       = ov2640_s_stream,
         .g_mbus_fmt     = ov2640_g_fmt,
@@ -1091,6 +1015,7 @@ static struct v4l2_subdev_video_ops ov2640_subdev_video_ops = {
         .cropcap        = ov2640_cropcap,
         .g_crop         = ov2640_g_crop,
         .enum_mbus_fmt  = ov2640_enum_fmt,
+        .g_mbus_config  = ov2640_g_mbus_config,
 };
 
 static struct v4l2_subdev_ops ov2640_subdev_ops = {
@@ -1104,18 +1029,11 @@ static struct v4l2_subdev_ops ov2640_subdev_ops = {
 static int ov2640_probe(struct i2c_client *client,
                         const struct i2c_device_id *did)
 {
-        struct ov2640_priv        *priv;
-        struct soc_camera_device  *icd = client->dev.platform_data;
-        struct i2c_adapter        *adapter = to_i2c_adapter(client->dev.parent);
-        struct soc_camera_link    *icl;
-        int                        ret;
-
-        if (!icd) {
-                dev_err(&adapter->dev, "OV2640: missing soc-camera data!\n");
-                return -EINVAL;
-        }
+        struct ov2640_priv      *priv;
+        struct soc_camera_link  *icl = soc_camera_i2c_to_link(client);
+        struct i2c_adapter      *adapter = to_i2c_adapter(client->dev.parent);
+        int                     ret;
 
-        icl = to_soc_camera_link(icd);
         if (!icl) {
                 dev_err(&adapter->dev,
                         "OV2640: Missing platform_data for driver\n");
@@ -1135,15 +1053,23 @@ static int ov2640_probe(struct i2c_client *client,
                 return -ENOMEM;
         }
 
-        priv->info = icl->priv;
-
         v4l2_i2c_subdev_init(&priv->subdev, client, &ov2640_subdev_ops);
+        v4l2_ctrl_handler_init(&priv->hdl, 2);
+        v4l2_ctrl_new_std(&priv->hdl, &ov2640_ctrl_ops,
+                        V4L2_CID_VFLIP, 0, 1, 1, 0);
+        v4l2_ctrl_new_std(&priv->hdl, &ov2640_ctrl_ops,
+                        V4L2_CID_HFLIP, 0, 1, 1, 0);
+        priv->subdev.ctrl_handler = &priv->hdl;
+        if (priv->hdl.error) {
+                int err = priv->hdl.error;
 
-        icd->ops = &ov2640_ops;
+                kfree(priv);
+                return err;
+        }
 
-        ret = ov2640_video_probe(icd, client);
+        ret = ov2640_video_probe(client);
         if (ret) {
-                icd->ops = NULL;
+                v4l2_ctrl_handler_free(&priv->hdl);
                 kfree(priv);
         } else {
                 dev_info(&adapter->dev, "OV2640 Probed\n");
@@ -1155,9 +1081,9 @@ static int ov2640_probe(struct i2c_client *client,
 static int ov2640_remove(struct i2c_client *client)
 {
         struct ov2640_priv       *priv = to_ov2640(client);
-        struct soc_camera_device *icd = client->dev.platform_data;
 
-        icd->ops = NULL;
+        v4l2_device_unregister_subdev(&priv->subdev);
+        v4l2_ctrl_handler_free(&priv->hdl);
         kfree(priv);
         return 0;
 }
diff --git a/drivers/media/video/ov5642.c b/drivers/media/video/ov5642.c
index 349a4ad3ccc1..bb37ec80f274 100644
--- a/drivers/media/video/ov5642.c
+++ b/drivers/media/video/ov5642.c
@@ -14,14 +14,16 @@
  * published by the Free Software Foundation.
  */
 
+#include <linux/bitops.h>
 #include <linux/delay.h>
 #include <linux/i2c.h>
+#include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 #include <linux/module.h>
+#include <linux/v4l2-mediabus.h>
 
 #include <media/soc_camera.h>
-#include <media/soc_mediabus.h>
 #include <media/v4l2-chip-ident.h>
 #include <media/v4l2-subdev.h>
 
@@ -35,7 +37,7 @@
 #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_HIGH          0x3806
 #define REG_WINDOW_HEIGHT_LOW           0x3807
 #define REG_OUT_WIDTH_HIGH              0x3808
 #define REG_OUT_WIDTH_LOW               0x3809
@@ -45,19 +47,44 @@
 #define REG_OUT_TOTAL_WIDTH_LOW         0x380d
 #define REG_OUT_TOTAL_HEIGHT_HIGH       0x380e
 #define REG_OUT_TOTAL_HEIGHT_LOW        0x380f
+#define REG_OUTPUT_FORMAT               0x4300
+#define REG_ISP_CTRL_01                 0x5001
+#define REG_AVG_WINDOW_END_X_HIGH       0x5682
+#define REG_AVG_WINDOW_END_X_LOW        0x5683
+#define REG_AVG_WINDOW_END_Y_HIGH       0x5686
+#define REG_AVG_WINDOW_END_Y_LOW        0x5687
+
+/* active pixel array size */
+#define OV5642_SENSOR_SIZE_X    2592
+#define OV5642_SENSOR_SIZE_Y    1944
 
 /*
- * define standard resolution.
- * Works currently only for up to 720 lines
- * eg. 320x240, 640x480, 800x600, 1280x720, 2048x720
+ * About OV5642 resolution, cropping and binning:
+ * This sensor supports it all, at least in the feature description.
+ * Unfortunately, no combination of appropriate registers settings could make
+ * the chip work the intended way. As it works with predefined register lists,
+ * some undocumented registers are presumably changed there to achieve their
+ * goals.
+ * This driver currently only works for resolutions up to 720 lines with a
+ * 1:1 scale. Hopefully these restrictions will be removed in the future.
  */
+#define OV5642_MAX_WIDTH        OV5642_SENSOR_SIZE_X
+#define OV5642_MAX_HEIGHT       720
 
-#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
+/* default sizes */
+#define OV5642_DEFAULT_WIDTH    1280
+#define OV5642_DEFAULT_HEIGHT   OV5642_MAX_HEIGHT
+
+/* minimum extra blanking */
+#define BLANKING_EXTRA_WIDTH            500
+#define BLANKING_EXTRA_HEIGHT           20
+
+/*
+ * the sensor's autoexposure is buggy when setting total_height low.
+ * It tries to expose longer than 1 frame period without taking care of it
+ * and this leads to weird output. So we set 1000 lines as minimum.
+ */
+#define BLANKING_MIN_HEIGHT             1000
 
 struct regval_list {
         u16 reg_num;
@@ -582,6 +609,11 @@ struct ov5642_datafmt {
 struct ov5642 {
         struct v4l2_subdev              subdev;
         const struct ov5642_datafmt     *fmt;
+        struct v4l2_rect                crop_rect;
+
+        /* blanking information */
+        int total_width;
+        int total_height;
 };
 
 static const struct ov5642_datafmt ov5642_colour_fmts[] = {
@@ -642,6 +674,21 @@ static int reg_write(struct i2c_client *client, u16 reg, u8 val)
 
         return 0;
 }
+
+/*
+ * convenience function to write 16 bit register values that are split up
+ * into two consecutive high and low parts
+ */
+static int reg_write16(struct i2c_client *client, u16 reg, u16 val16)
+{
+        int ret;
+
+        ret = reg_write(client, reg, val16 >> 8);
+        if (ret)
+                return ret;
+        return reg_write(client, reg + 1, val16 & 0x00ff);
+}
+
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 static int ov5642_get_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
 {
@@ -685,58 +732,55 @@ static int ov5642_write_array(struct i2c_client *client,
         return 0;
 }
 
-static int ov5642_set_resolution(struct i2c_client *client)
+static int ov5642_set_resolution(struct v4l2_subdev *sd)
 {
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct ov5642 *priv = to_ov5642(client);
+        int width = priv->crop_rect.width;
+        int height = priv->crop_rect.height;
+        int total_width = priv->total_width;
+        int total_height = priv->total_height;
+        int start_x = (OV5642_SENSOR_SIZE_X - width) / 2;
+        int start_y = (OV5642_SENSOR_SIZE_Y - height) / 2;
         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);
 
+        /*
+         * This should set the starting point for cropping.
+         * Doesn't work so far.
+         */
+        ret = reg_write16(client, REG_WINDOW_START_X_HIGH, start_x);
         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);
+                ret = reg_write16(client, REG_WINDOW_START_Y_HIGH, start_y);
+        if (!ret) {
+                priv->crop_rect.left = start_x;
+                priv->crop_rect.top = start_y;
+        }
 
         if (!ret)
-                ret = reg_write(client, REG_OUT_WIDTH_HIGH, width_high);
+                ret = reg_write16(client, REG_WINDOW_WIDTH_HIGH, width);
         if (!ret)
-                ret = reg_write(client, REG_OUT_WIDTH_LOW , width_low);
-        if (!ret)
-                ret = reg_write(client, REG_OUT_HEIGHT_HIGH, height_high);
+                ret = reg_write16(client, REG_WINDOW_HEIGHT_HIGH, height);
+        if (ret)
+                return ret;
+        priv->crop_rect.width = width;
+        priv->crop_rect.height = height;
+
+        /* Set the output window size. Only 1:1 scale is supported so far. */
+        ret = reg_write16(client, REG_OUT_WIDTH_HIGH, width);
         if (!ret)
-                ret = reg_write(client, REG_OUT_HEIGHT_LOW,  height_low);
+                ret = reg_write16(client, REG_OUT_HEIGHT_HIGH, height);
 
+        /* Total width = output size + blanking */
         if (!ret)
-                ret = reg_write(client, REG_OUT_TOTAL_WIDTH_HIGH, total_width_high);
+                ret = reg_write16(client, REG_OUT_TOTAL_WIDTH_HIGH, total_width);
         if (!ret)
-                ret = reg_write(client, REG_OUT_TOTAL_WIDTH_LOW, total_width_low);
+                ret = reg_write16(client, REG_OUT_TOTAL_HEIGHT_HIGH, total_height);
+
+        /* Sets the window for AWB calculations */
         if (!ret)
-                ret = reg_write(client, REG_OUT_TOTAL_HEIGHT_HIGH, total_height_high);
+                ret = reg_write16(client, REG_AVG_WINDOW_END_X_HIGH, width);
         if (!ret)
-                ret = reg_write(client, REG_OUT_TOTAL_HEIGHT_LOW,  total_height_low);
+                ret = reg_write16(client, REG_AVG_WINDOW_END_Y_HIGH, height);
 
         return ret;
 }
@@ -744,18 +788,18 @@ static int ov5642_set_resolution(struct i2c_client *client)
 static int ov5642_try_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 = 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);
+        mf->width = priv->crop_rect.width;
+        mf->height = priv->crop_rect.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;
@@ -767,20 +811,13 @@ static int ov5642_s_fmt(struct v4l2_subdev *sd,
         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;
 }
 
@@ -794,8 +831,8 @@ static int ov5642_g_fmt(struct v4l2_subdev *sd,
 
         mf->code        = fmt->code;
         mf->colorspace  = fmt->colorspace;
-        mf->width       = OV5642_WIDTH;
-        mf->height      = OV5642_HEIGHT;
+        mf->width       = priv->crop_rect.width;
+        mf->height      = priv->crop_rect.height;
         mf->field       = V4L2_FIELD_NONE;
 
         return 0;
@@ -828,15 +865,44 @@ static int ov5642_g_chip_ident(struct v4l2_subdev *sd,
         return 0;
 }
 
+static int ov5642_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct ov5642 *priv = to_ov5642(client);
+        struct v4l2_rect *rect = &a->c;
+        int ret;
+
+        v4l_bound_align_image(&rect->width, 48, OV5642_MAX_WIDTH, 1,
+                              &rect->height, 32, OV5642_MAX_HEIGHT, 1, 0);
+
+        priv->crop_rect.width   = rect->width;
+        priv->crop_rect.height  = rect->height;
+        priv->total_width       = rect->width + BLANKING_EXTRA_WIDTH;
+        priv->total_height      = max_t(int, rect->height +
+                                                        BLANKING_EXTRA_HEIGHT,
+                                                        BLANKING_MIN_HEIGHT);
+        priv->crop_rect.width           = rect->width;
+        priv->crop_rect.height          = rect->height;
+
+        ret = ov5642_write_array(client, ov5642_default_regs_init);
+        if (!ret)
+                ret = ov5642_set_resolution(sd);
+        if (!ret)
+                ret = ov5642_write_array(client, ov5642_default_regs_finalise);
+
+        return ret;
+}
+
 static int ov5642_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
 {
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct ov5642 *priv = to_ov5642(client);
         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;
+        if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+                return -EINVAL;
+
+        *rect = priv->crop_rect;
 
         return 0;
 }
@@ -845,8 +911,8 @@ 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->bounds.width                 = OV5642_MAX_WIDTH;
+        a->bounds.height                = OV5642_MAX_HEIGHT;
         a->defrect                      = a->bounds;
         a->type                         = V4L2_BUF_TYPE_VIDEO_CAPTURE;
         a->pixelaspect.numerator        = 1;
@@ -855,16 +921,47 @@ static int ov5642_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
         return 0;
 }
 
+static int ov5642_g_mbus_config(struct v4l2_subdev *sd,
+                                struct v4l2_mbus_config *cfg)
+{
+        cfg->type = V4L2_MBUS_CSI2;
+        cfg->flags = V4L2_MBUS_CSI2_2_LANE | V4L2_MBUS_CSI2_CHANNEL_0 |
+                                        V4L2_MBUS_CSI2_CONTINUOUS_CLOCK;
+
+        return 0;
+}
+
+static int ov5642_s_power(struct v4l2_subdev *sd, int on)
+{
+        struct i2c_client *client;
+        int ret;
+
+        if (!on)
+                return 0;
+
+        client = v4l2_get_subdevdata(sd);
+        ret = ov5642_write_array(client, ov5642_default_regs_init);
+        if (!ret)
+                ret = ov5642_set_resolution(sd);
+        if (!ret)
+                ret = ov5642_write_array(client, ov5642_default_regs_finalise);
+
+        return ret;
+}
+
 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,
+        .s_crop         = ov5642_s_crop,
         .g_crop         = ov5642_g_crop,
         .cropcap        = ov5642_cropcap,
+        .g_mbus_config  = ov5642_g_mbus_config,
 };
 
 static struct v4l2_subdev_core_ops ov5642_subdev_core_ops = {
+        .s_power        = ov5642_s_power,
         .g_chip_ident   = ov5642_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register     = ov5642_get_register,
@@ -877,28 +974,7 @@ static struct v4l2_subdev_ops ov5642_subdev_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)
+static int ov5642_video_probe(struct i2c_client *client)
 {
         int ret;
         u8 id_high, id_low;
@@ -929,16 +1005,9 @@ 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;
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
         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;
@@ -950,17 +1019,24 @@ static int ov5642_probe(struct i2c_client *client,
 
         v4l2_i2c_subdev_init(&priv->subdev, client, &ov5642_subdev_ops);
 
-        icd->ops        = &soc_ov5642_ops;
-        priv->fmt       = &ov5642_colour_fmts[0];
+        priv->fmt               = &ov5642_colour_fmts[0];
+
+        priv->crop_rect.width   = OV5642_DEFAULT_WIDTH;
+        priv->crop_rect.height  = OV5642_DEFAULT_HEIGHT;
+        priv->crop_rect.left    = (OV5642_MAX_WIDTH - OV5642_DEFAULT_WIDTH) / 2;
+        priv->crop_rect.top     = (OV5642_MAX_HEIGHT - OV5642_DEFAULT_HEIGHT) / 2;
+        priv->crop_rect.width   = OV5642_DEFAULT_WIDTH;
+        priv->crop_rect.height  = OV5642_DEFAULT_HEIGHT;
+        priv->total_width = OV5642_DEFAULT_WIDTH + BLANKING_EXTRA_WIDTH;
+        priv->total_height = BLANKING_MIN_HEIGHT;
 
-        ret = ov5642_video_probe(icd, client);
+        ret = ov5642_video_probe(client);
         if (ret < 0)
                 goto error;
 
         return 0;
 
 error:
-        icd->ops = NULL;
         kfree(priv);
         return ret;
 }
@@ -968,10 +1044,8 @@ error:
 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);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
 
-        icd->ops = NULL;
         if (icl->free_bus)
                 icl->free_bus(icl);
         kfree(priv);
diff --git a/drivers/media/video/ov6650.c b/drivers/media/video/ov6650.c
index 456d9ad9ae5a..d5b057207a7b 100644
--- a/drivers/media/video/ov6650.c
+++ b/drivers/media/video/ov6650.c
@@ -28,10 +28,11 @@
 #include <linux/delay.h>
 #include <linux/i2c.h>
 #include <linux/slab.h>
+#include <linux/v4l2-mediabus.h>
 
 #include <media/soc_camera.h>
 #include <media/v4l2-chip-ident.h>
-
+#include <media/v4l2-ctrls.h>
 
 /* Register definitions */
 #define REG_GAIN                0x00    /* range 00 - 3F */
@@ -177,20 +178,23 @@ struct ov6650_reg {
 
 struct ov6650 {
         struct v4l2_subdev      subdev;
-
-        int                     gain;
-        int                     blue;
-        int                     red;
-        int                     saturation;
-        int                     hue;
-        int                     brightness;
-        int                     exposure;
-        int                     gamma;
-        int                     aec;
-        bool                    vflip;
-        bool                    hflip;
-        bool                    awb;
-        bool                    agc;
+        struct v4l2_ctrl_handler hdl;
+        struct {
+                /* exposure/autoexposure cluster */
+                struct v4l2_ctrl *autoexposure;
+                struct v4l2_ctrl *exposure;
+        };
+        struct {
+                /* gain/autogain cluster */
+                struct v4l2_ctrl *autogain;
+                struct v4l2_ctrl *gain;
+        };
+        struct {
+                /* blue/red/autowhitebalance cluster */
+                struct v4l2_ctrl *autowb;
+                struct v4l2_ctrl *blue;
+                struct v4l2_ctrl *red;
+        };
         bool                    half_scale;     /* scale down output by 2 */
         struct v4l2_rect        rect;           /* sensor cropping window */
         unsigned long           pclk_limit;     /* from host */
@@ -210,126 +214,6 @@ static enum v4l2_mbus_pixelcode ov6650_codes[] = {
         V4L2_MBUS_FMT_Y8_1X8,
 };
 
-static const struct v4l2_queryctrl ov6650_controls[] = {
-        {
-                .id             = V4L2_CID_AUTOGAIN,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "AGC",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 1,
-        },
-        {
-                .id             = V4L2_CID_GAIN,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Gain",
-                .minimum        = 0,
-                .maximum        = 0x3f,
-                .step           = 1,
-                .default_value  = DEF_GAIN,
-        },
-        {
-                .id             = V4L2_CID_AUTO_WHITE_BALANCE,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "AWB",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 1,
-        },
-        {
-                .id             = V4L2_CID_BLUE_BALANCE,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Blue",
-                .minimum        = 0,
-                .maximum        = 0xff,
-                .step           = 1,
-                .default_value  = DEF_BLUE,
-        },
-        {
-                .id             = V4L2_CID_RED_BALANCE,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Red",
-                .minimum        = 0,
-                .maximum        = 0xff,
-                .step           = 1,
-                .default_value  = DEF_RED,
-        },
-        {
-                .id             = V4L2_CID_SATURATION,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Saturation",
-                .minimum        = 0,
-                .maximum        = 0xf,
-                .step           = 1,
-                .default_value  = 0x8,
-        },
-        {
-                .id             = V4L2_CID_HUE,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Hue",
-                .minimum        = 0,
-                .maximum        = HUE_MASK,
-                .step           = 1,
-                .default_value  = DEF_HUE,
-        },
-        {
-                .id             = V4L2_CID_BRIGHTNESS,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Brightness",
-                .minimum        = 0,
-                .maximum        = 0xff,
-                .step           = 1,
-                .default_value  = 0x80,
-        },
-        {
-                .id             = V4L2_CID_EXPOSURE_AUTO,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "AEC",
-                .minimum        = 0,
-                .maximum        = 3,
-                .step           = 1,
-                .default_value  = 0,
-        },
-        {
-                .id             = V4L2_CID_EXPOSURE,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Exposure",
-                .minimum        = 0,
-                .maximum        = 0xff,
-                .step           = 1,
-                .default_value  = DEF_AECH,
-        },
-        {
-                .id             = V4L2_CID_GAMMA,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Gamma",
-                .minimum        = 0,
-                .maximum        = 0xff,
-                .step           = 1,
-                .default_value  = 0x12,
-        },
-        {
-                .id             = V4L2_CID_VFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Vertically",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        },
-        {
-                .id             = V4L2_CID_HFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Horizontally",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        },
-};
-
 /* read a register */
 static int ov6650_reg_read(struct i2c_client *client, u8 reg, u8 *val)
 {
@@ -419,213 +303,90 @@ static int ov6650_s_stream(struct v4l2_subdev *sd, int enable)
         return 0;
 }
 
-/* Alter bus settings on camera side */
-static int ov6650_set_bus_param(struct soc_camera_device *icd,
-                                unsigned long flags)
-{
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-        struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
-        int ret;
-
-        flags = soc_camera_apply_sensor_flags(icl, flags);
-
-        if (flags & SOCAM_PCLK_SAMPLE_RISING)
-                ret = ov6650_reg_rmw(client, REG_COMJ, COMJ_PCLK_RISING, 0);
-        else
-                ret = ov6650_reg_rmw(client, REG_COMJ, 0, COMJ_PCLK_RISING);
-        if (ret)
-                return ret;
-
-        if (flags & SOCAM_HSYNC_ACTIVE_LOW)
-                ret = ov6650_reg_rmw(client, REG_COMF, COMF_HREF_LOW, 0);
-        else
-                ret = ov6650_reg_rmw(client, REG_COMF, 0, COMF_HREF_LOW);
-        if (ret)
-                return ret;
-
-        if (flags & SOCAM_VSYNC_ACTIVE_HIGH)
-                ret = ov6650_reg_rmw(client, REG_COMJ, COMJ_VSYNC_HIGH, 0);
-        else
-                ret = ov6650_reg_rmw(client, REG_COMJ, 0, COMJ_VSYNC_HIGH);
-
-        return ret;
-}
-
-/* Request bus settings on camera side */
-static unsigned long ov6650_query_bus_param(struct soc_camera_device *icd)
-{
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-
-        unsigned long flags = SOCAM_MASTER |
-                SOCAM_PCLK_SAMPLE_RISING | SOCAM_PCLK_SAMPLE_FALLING |
-                SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_LOW |
-                SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_LOW |
-                SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATAWIDTH_8;
-
-        return soc_camera_apply_sensor_flags(icl, flags);
-}
-
 /* Get status of additional camera capabilities */
-static int ov6650_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int ov6550_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 {
+        struct ov6650 *priv = container_of(ctrl->handler, struct ov6650, hdl);
+        struct v4l2_subdev *sd = &priv->subdev;
         struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct ov6650 *priv = to_ov6650(client);
-        uint8_t reg;
-        int ret = 0;
+        uint8_t reg, reg2;
+        int ret;
 
         switch (ctrl->id) {
         case V4L2_CID_AUTOGAIN:
-                ctrl->value = priv->agc;
-                break;
-        case V4L2_CID_GAIN:
-                if (priv->agc) {
-                        ret = ov6650_reg_read(client, REG_GAIN, &reg);
-                        ctrl->value = reg;
-                } else {
-                        ctrl->value = priv->gain;
-                }
-                break;
+                ret = ov6650_reg_read(client, REG_GAIN, &reg);
+                if (!ret)
+                        priv->gain->val = reg;
+                return ret;
         case V4L2_CID_AUTO_WHITE_BALANCE:
-                ctrl->value = priv->awb;
-                break;
-        case V4L2_CID_BLUE_BALANCE:
-                if (priv->awb) {
-                        ret = ov6650_reg_read(client, REG_BLUE, &reg);
-                        ctrl->value = reg;
-                } else {
-                        ctrl->value = priv->blue;
-                }
-                break;
-        case V4L2_CID_RED_BALANCE:
-                if (priv->awb) {
-                        ret = ov6650_reg_read(client, REG_RED, &reg);
-                        ctrl->value = reg;
-                } else {
-                        ctrl->value = priv->red;
+                ret = ov6650_reg_read(client, REG_BLUE, &reg);
+                if (!ret)
+                        ret = ov6650_reg_read(client, REG_RED, &reg2);
+                if (!ret) {
+                        priv->blue->val = reg;
+                        priv->red->val = reg2;
                 }
-                break;
-        case V4L2_CID_SATURATION:
-                ctrl->value = priv->saturation;
-                break;
-        case V4L2_CID_HUE:
-                ctrl->value = priv->hue;
-                break;
-        case V4L2_CID_BRIGHTNESS:
-                ctrl->value = priv->brightness;
-                break;
+                return ret;
         case V4L2_CID_EXPOSURE_AUTO:
-                ctrl->value = priv->aec;
-                break;
-        case V4L2_CID_EXPOSURE:
-                if (priv->aec) {
-                        ret = ov6650_reg_read(client, REG_AECH, &reg);
-                        ctrl->value = reg;
-                } else {
-                        ctrl->value = priv->exposure;
-                }
-                break;
-        case V4L2_CID_GAMMA:
-                ctrl->value = priv->gamma;
-                break;
-        case V4L2_CID_VFLIP:
-                ctrl->value = priv->vflip;
-                break;
-        case V4L2_CID_HFLIP:
-                ctrl->value = priv->hflip;
-                break;
+                ret = ov6650_reg_read(client, REG_AECH, &reg);
+                if (!ret)
+                        priv->exposure->val = reg;
+                return ret;
         }
-        return ret;
+        return -EINVAL;
 }
 
 /* Set status of additional camera capabilities */
-static int ov6650_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int ov6550_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+        struct ov6650 *priv = container_of(ctrl->handler, struct ov6650, hdl);
+        struct v4l2_subdev *sd = &priv->subdev;
         struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct ov6650 *priv = to_ov6650(client);
-        int ret = 0;
+        int ret;
 
         switch (ctrl->id) {
         case V4L2_CID_AUTOGAIN:
                 ret = ov6650_reg_rmw(client, REG_COMB,
-                                ctrl->value ? COMB_AGC : 0, COMB_AGC);
-                if (!ret)
-                        priv->agc = ctrl->value;
-                break;
-        case V4L2_CID_GAIN:
-                ret = ov6650_reg_write(client, REG_GAIN, ctrl->value);
-                if (!ret)
-                        priv->gain = ctrl->value;
-                break;
+                                ctrl->val ? COMB_AGC : 0, COMB_AGC);
+                if (!ret && !ctrl->val)
+                        ret = ov6650_reg_write(client, REG_GAIN, priv->gain->val);
+                return ret;
         case V4L2_CID_AUTO_WHITE_BALANCE:
                 ret = ov6650_reg_rmw(client, REG_COMB,
-                                ctrl->value ? COMB_AWB : 0, COMB_AWB);
-                if (!ret)
-                        priv->awb = ctrl->value;
-                break;
-        case V4L2_CID_BLUE_BALANCE:
-                ret = ov6650_reg_write(client, REG_BLUE, ctrl->value);
-                if (!ret)
-                        priv->blue = ctrl->value;
-                break;
-        case V4L2_CID_RED_BALANCE:
-                ret = ov6650_reg_write(client, REG_RED, ctrl->value);
-                if (!ret)
-                        priv->red = ctrl->value;
-                break;
+                                ctrl->val ? COMB_AWB : 0, COMB_AWB);
+                if (!ret && !ctrl->val) {
+                        ret = ov6650_reg_write(client, REG_BLUE, priv->blue->val);
+                        if (!ret)
+                                ret = ov6650_reg_write(client, REG_RED,
+                                                        priv->red->val);
+                }
+                return ret;
         case V4L2_CID_SATURATION:
-                ret = ov6650_reg_rmw(client, REG_SAT, SET_SAT(ctrl->value),
+                return ov6650_reg_rmw(client, REG_SAT, SET_SAT(ctrl->val),
                                 SAT_MASK);
-                if (!ret)
-                        priv->saturation = ctrl->value;
-                break;
         case V4L2_CID_HUE:
-                ret = ov6650_reg_rmw(client, REG_HUE, SET_HUE(ctrl->value),
+                return ov6650_reg_rmw(client, REG_HUE, SET_HUE(ctrl->val),
                                 HUE_MASK);
-                if (!ret)
-                        priv->hue = ctrl->value;
-                break;
         case V4L2_CID_BRIGHTNESS:
-                ret = ov6650_reg_write(client, REG_BRT, ctrl->value);
-                if (!ret)
-                        priv->brightness = ctrl->value;
-                break;
+                return ov6650_reg_write(client, REG_BRT, ctrl->val);
         case V4L2_CID_EXPOSURE_AUTO:
-                switch (ctrl->value) {
-                case V4L2_EXPOSURE_AUTO:
-                        ret = ov6650_reg_rmw(client, REG_COMB, COMB_AEC, 0);
-                        break;
-                default:
-                        ret = ov6650_reg_rmw(client, REG_COMB, 0, COMB_AEC);
-                        break;
-                }
-                if (!ret)
-                        priv->aec = ctrl->value;
-                break;
-        case V4L2_CID_EXPOSURE:
-                ret = ov6650_reg_write(client, REG_AECH, ctrl->value);
-                if (!ret)
-                        priv->exposure = ctrl->value;
-                break;
+                ret = ov6650_reg_rmw(client, REG_COMB, ctrl->val ==
+                                V4L2_EXPOSURE_AUTO ? COMB_AEC : 0, COMB_AEC);
+                if (!ret && ctrl->val == V4L2_EXPOSURE_MANUAL)
+                        ret = ov6650_reg_write(client, REG_AECH,
+                                                priv->exposure->val);
+                return ret;
         case V4L2_CID_GAMMA:
-                ret = ov6650_reg_write(client, REG_GAM1, ctrl->value);
-                if (!ret)
-                        priv->gamma = ctrl->value;
-                break;
+                return ov6650_reg_write(client, REG_GAM1, ctrl->val);
         case V4L2_CID_VFLIP:
-                ret = ov6650_reg_rmw(client, REG_COMB,
-                                ctrl->value ? COMB_FLIP_V : 0, COMB_FLIP_V);
-                if (!ret)
-                        priv->vflip = ctrl->value;
-                break;
+                return ov6650_reg_rmw(client, REG_COMB,
+                                ctrl->val ? COMB_FLIP_V : 0, COMB_FLIP_V);
         case V4L2_CID_HFLIP:
-                ret = ov6650_reg_rmw(client, REG_COMB,
-                                ctrl->value ? COMB_FLIP_H : 0, COMB_FLIP_H);
-                if (!ret)
-                        priv->hflip = ctrl->value;
-                break;
+                return ov6650_reg_rmw(client, REG_COMB,
+                                ctrl->val ? COMB_FLIP_H : 0, COMB_FLIP_H);
         }
 
-        return ret;
+        return -EINVAL;
 }
 
 /* Get chip identification */
@@ -778,7 +539,7 @@ static u8 to_clkrc(struct v4l2_fract *timeperframe,
 static int ov6650_s_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
 {
         struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct soc_camera_device *icd = client->dev.platform_data;
+        struct soc_camera_device *icd = (struct soc_camera_device *)sd->grp_id;
         struct soc_camera_sense *sense = icd->sense;
         struct ov6650 *priv = to_ov6650(client);
         bool half_scale = !is_unscaled_ok(mf->width, mf->height, &priv->rect);
@@ -1057,8 +818,7 @@ static int ov6650_prog_dflt(struct i2c_client *client)
         return ret;
 }
 
-static int ov6650_video_probe(struct soc_camera_device *icd,
-                                struct i2c_client *client)
+static int ov6650_video_probe(struct i2c_client *client)
 {
         u8              pidh, pidl, midh, midl;
         int             ret = 0;
@@ -1094,16 +854,12 @@ static int ov6650_video_probe(struct soc_camera_device *icd,
         return ret;
 }
 
-static struct soc_camera_ops ov6650_ops = {
-        .set_bus_param          = ov6650_set_bus_param,
-        .query_bus_param        = ov6650_query_bus_param,
-        .controls               = ov6650_controls,
-        .num_controls           = ARRAY_SIZE(ov6650_controls),
+static const struct v4l2_ctrl_ops ov6550_ctrl_ops = {
+        .g_volatile_ctrl = ov6550_g_volatile_ctrl,
+        .s_ctrl = ov6550_s_ctrl,
 };
 
 static struct v4l2_subdev_core_ops ov6650_core_ops = {
-        .g_ctrl                 = ov6650_g_ctrl,
-        .s_ctrl                 = ov6650_s_ctrl,
         .g_chip_ident           = ov6650_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register             = ov6650_get_register,
@@ -1111,6 +867,55 @@ static struct v4l2_subdev_core_ops ov6650_core_ops = {
 #endif
 };
 
+/* Request bus settings on camera side */
+static int ov6650_g_mbus_config(struct v4l2_subdev *sd,
+                                struct v4l2_mbus_config *cfg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+        cfg->flags = V4L2_MBUS_MASTER |
+                V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING |
+                V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_LOW |
+                V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_LOW |
+                V4L2_MBUS_DATA_ACTIVE_HIGH;
+        cfg->type = V4L2_MBUS_PARALLEL;
+        cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+        return 0;
+}
+
+/* Alter bus settings on camera side */
+static int ov6650_s_mbus_config(struct v4l2_subdev *sd,
+                                const struct v4l2_mbus_config *cfg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+        unsigned long flags = soc_camera_apply_board_flags(icl, cfg);
+        int ret;
+
+        if (flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+                ret = ov6650_reg_rmw(client, REG_COMJ, COMJ_PCLK_RISING, 0);
+        else
+                ret = ov6650_reg_rmw(client, REG_COMJ, 0, COMJ_PCLK_RISING);
+        if (ret)
+                return ret;
+
+        if (flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+                ret = ov6650_reg_rmw(client, REG_COMF, COMF_HREF_LOW, 0);
+        else
+                ret = ov6650_reg_rmw(client, REG_COMF, 0, COMF_HREF_LOW);
+        if (ret)
+                return ret;
+
+        if (flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
+                ret = ov6650_reg_rmw(client, REG_COMJ, COMJ_VSYNC_HIGH, 0);
+        else
+                ret = ov6650_reg_rmw(client, REG_COMJ, 0, COMJ_VSYNC_HIGH);
+
+        return ret;
+}
+
 static struct v4l2_subdev_video_ops ov6650_video_ops = {
         .s_stream       = ov6650_s_stream,
         .g_mbus_fmt     = ov6650_g_fmt,
@@ -1122,6 +927,8 @@ static struct v4l2_subdev_video_ops ov6650_video_ops = {
         .s_crop         = ov6650_s_crop,
         .g_parm         = ov6650_g_parm,
         .s_parm         = ov6650_s_parm,
+        .g_mbus_config  = ov6650_g_mbus_config,
+        .s_mbus_config  = ov6650_s_mbus_config,
 };
 
 static struct v4l2_subdev_ops ov6650_subdev_ops = {
@@ -1136,16 +943,9 @@ static int ov6650_probe(struct i2c_client *client,
                         const struct i2c_device_id *did)
 {
         struct ov6650 *priv;
-        struct soc_camera_device *icd = client->dev.platform_data;
-        struct soc_camera_link *icl;
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
         int ret;
 
-        if (!icd) {
-                dev_err(&client->dev, "Missing soc-camera data!\n");
-                return -EINVAL;
-        }
-
-        icl = to_soc_camera_link(icd);
         if (!icl) {
                 dev_err(&client->dev, "Missing platform_data for driver\n");
                 return -EINVAL;
@@ -1159,8 +959,46 @@ static int ov6650_probe(struct i2c_client *client,
         }
 
         v4l2_i2c_subdev_init(&priv->subdev, client, &ov6650_subdev_ops);
+        v4l2_ctrl_handler_init(&priv->hdl, 13);
+        v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+                        V4L2_CID_VFLIP, 0, 1, 1, 0);
+        v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+                        V4L2_CID_HFLIP, 0, 1, 1, 0);
+        priv->autogain = v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+                        V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+        priv->gain = v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+                        V4L2_CID_GAIN, 0, 0x3f, 1, DEF_GAIN);
+        priv->autowb = v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+                        V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
+        priv->blue = v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+                        V4L2_CID_BLUE_BALANCE, 0, 0xff, 1, DEF_BLUE);
+        priv->red = v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+                        V4L2_CID_RED_BALANCE, 0, 0xff, 1, DEF_RED);
+        v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+                        V4L2_CID_SATURATION, 0, 0xf, 1, 0x8);
+        v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+                        V4L2_CID_HUE, 0, HUE_MASK, 1, DEF_HUE);
+        v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+                        V4L2_CID_BRIGHTNESS, 0, 0xff, 1, 0x80);
+        priv->autoexposure = v4l2_ctrl_new_std_menu(&priv->hdl,
+                        &ov6550_ctrl_ops, V4L2_CID_EXPOSURE_AUTO,
+                        V4L2_EXPOSURE_MANUAL, 0, V4L2_EXPOSURE_AUTO);
+        priv->exposure = v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+                        V4L2_CID_EXPOSURE, 0, 0xff, 1, DEF_AECH);
+        v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+                        V4L2_CID_GAMMA, 0, 0xff, 1, 0x12);
+
+        priv->subdev.ctrl_handler = &priv->hdl;
+        if (priv->hdl.error) {
+                int err = priv->hdl.error;
 
-        icd->ops = &ov6650_ops;
+                kfree(priv);
+                return err;
+        }
+        v4l2_ctrl_auto_cluster(2, &priv->autogain, 0, true);
+        v4l2_ctrl_auto_cluster(3, &priv->autowb, 0, true);
+        v4l2_ctrl_auto_cluster(2, &priv->autoexposure,
+                                V4L2_EXPOSURE_MANUAL, true);
 
         priv->rect.left   = DEF_HSTRT << 1;
         priv->rect.top    = DEF_VSTRT << 1;
@@ -1170,10 +1008,12 @@ static int ov6650_probe(struct i2c_client *client,
         priv->code        = V4L2_MBUS_FMT_YUYV8_2X8;
         priv->colorspace  = V4L2_COLORSPACE_JPEG;
 
-        ret = ov6650_video_probe(icd, client);
+        ret = ov6650_video_probe(client);
+        if (!ret)
+                ret = v4l2_ctrl_handler_setup(&priv->hdl);
 
         if (ret) {
-                icd->ops = NULL;
+                v4l2_ctrl_handler_free(&priv->hdl);
                 kfree(priv);
         }
 
@@ -1184,6 +1024,8 @@ static int ov6650_remove(struct i2c_client *client)
 {
         struct ov6650 *priv = to_ov6650(client);
 
+        v4l2_device_unregister_subdev(&priv->subdev);
+        v4l2_ctrl_handler_free(&priv->hdl);
         kfree(priv);
         return 0;
 }
diff --git a/drivers/media/video/ov772x.c b/drivers/media/video/ov772x.c
index 397870f076c1..9f6ce3d8a29e 100644
--- a/drivers/media/video/ov772x.c
+++ b/drivers/media/video/ov772x.c
@@ -20,12 +20,14 @@
 #include <linux/i2c.h>
 #include <linux/slab.h>
 #include <linux/delay.h>
+#include <linux/v4l2-mediabus.h>
 #include <linux/videodev2.h>
+
+#include <media/ov772x.h>
+#include <media/soc_camera.h>
+#include <media/v4l2-ctrls.h>
 #include <media/v4l2-chip-ident.h>
 #include <media/v4l2-subdev.h>
-#include <media/soc_camera.h>
-#include <media/soc_mediabus.h>
-#include <media/ov772x.h>
 
 /*
  * register offset
@@ -400,6 +402,7 @@ struct ov772x_win_size {
 
 struct ov772x_priv {
         struct v4l2_subdev                subdev;
+        struct v4l2_ctrl_handler          hdl;
         struct ov772x_camera_info        *info;
         const struct ov772x_color_format *cfmt;
         const struct ov772x_win_size     *win;
@@ -517,36 +520,6 @@ static const struct ov772x_win_size ov772x_win_qvga = {
         .regs     = ov772x_qvga_regs,
 };
 
-static const struct v4l2_queryctrl ov772x_controls[] = {
-        {
-                .id             = V4L2_CID_VFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Vertically",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        },
-        {
-                .id             = V4L2_CID_HFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Horizontally",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        },
-        {
-                .id             = V4L2_CID_BAND_STOP_FILTER,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Band-stop filter",
-                .minimum        = 0,
-                .maximum        = 256,
-                .step           = 1,
-                .default_value  = 0,
-        },
-};
-
 /*
  * general function
  */
@@ -620,75 +593,30 @@ static int ov772x_s_stream(struct v4l2_subdev *sd, int enable)
         return 0;
 }
 
-static int ov772x_set_bus_param(struct soc_camera_device *icd,
-                                unsigned long             flags)
-{
-        return 0;
-}
-
-static unsigned long ov772x_query_bus_param(struct soc_camera_device *icd)
-{
-        struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
-        struct ov772x_priv *priv = i2c_get_clientdata(client);
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-        unsigned long flags = SOCAM_PCLK_SAMPLE_RISING | SOCAM_MASTER |
-                SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_HIGH |
-                SOCAM_DATA_ACTIVE_HIGH;
-
-        if (priv->info->flags & OV772X_FLAG_8BIT)
-                flags |= SOCAM_DATAWIDTH_8;
-        else
-                flags |= SOCAM_DATAWIDTH_10;
-
-        return soc_camera_apply_sensor_flags(icl, flags);
-}
-
-static int ov772x_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-        struct ov772x_priv *priv = container_of(sd, struct ov772x_priv, subdev);
-
-        switch (ctrl->id) {
-        case V4L2_CID_VFLIP:
-                ctrl->value = priv->flag_vflip;
-                break;
-        case V4L2_CID_HFLIP:
-                ctrl->value = priv->flag_hflip;
-                break;
-        case V4L2_CID_BAND_STOP_FILTER:
-                ctrl->value = priv->band_filter;
-                break;
-        }
-        return 0;
-}
-
-static int ov772x_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int ov772x_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+        struct ov772x_priv *priv = container_of(ctrl->handler,
+                                                struct ov772x_priv, hdl);
+        struct v4l2_subdev *sd = &priv->subdev;
         struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct ov772x_priv *priv = container_of(sd, struct ov772x_priv, subdev);
         int ret = 0;
         u8 val;
 
         switch (ctrl->id) {
         case V4L2_CID_VFLIP:
-                val = ctrl->value ? VFLIP_IMG : 0x00;
-                priv->flag_vflip = ctrl->value;
+                val = ctrl->val ? VFLIP_IMG : 0x00;
+                priv->flag_vflip = ctrl->val;
                 if (priv->info->flags & OV772X_FLAG_VFLIP)
                         val ^= VFLIP_IMG;
-                ret = ov772x_mask_set(client, COM3, VFLIP_IMG, val);
-                break;
+                return ov772x_mask_set(client, COM3, VFLIP_IMG, val);
         case V4L2_CID_HFLIP:
-                val = ctrl->value ? HFLIP_IMG : 0x00;
-                priv->flag_hflip = ctrl->value;
+                val = ctrl->val ? HFLIP_IMG : 0x00;
+                priv->flag_hflip = ctrl->val;
                 if (priv->info->flags & OV772X_FLAG_HFLIP)
                         val ^= HFLIP_IMG;
-                ret = ov772x_mask_set(client, COM3, HFLIP_IMG, val);
-                break;
+                return ov772x_mask_set(client, COM3, HFLIP_IMG, val);
         case V4L2_CID_BAND_STOP_FILTER:
-                if ((unsigned)ctrl->value > 256)
-                        ctrl->value = 256;
-                if (ctrl->value == priv->band_filter)
-                        break;
-                if (!ctrl->value) {
+                if (!ctrl->val) {
                         /* Switch the filter off, it is on now */
                         ret = ov772x_mask_set(client, BDBASE, 0xff, 0xff);
                         if (!ret)
@@ -696,7 +624,7 @@ static int ov772x_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
                                                       BNDF_ON_OFF, 0);
                 } else {
                         /* Switch the filter on, set AEC low limit */
-                        val = 256 - ctrl->value;
+                        val = 256 - ctrl->val;
                         ret = ov772x_mask_set(client, COM8,
                                               BNDF_ON_OFF, BNDF_ON_OFF);
                         if (!ret)
@@ -704,11 +632,11 @@ static int ov772x_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
                                                       0xff, val);
                 }
                 if (!ret)
-                        priv->band_filter = ctrl->value;
-                break;
+                        priv->band_filter = ctrl->val;
+                return ret;
         }
 
-        return ret;
+        return -EINVAL;
 }
 
 static int ov772x_g_chip_ident(struct v4l2_subdev *sd,
@@ -822,13 +750,13 @@ static int ov772x_set_params(struct i2c_client *client, u32 *width, u32 *height,
                         goto ov772x_set_fmt_error;
 
                 ret = ov772x_mask_set(client,
-                                      EDGE_TRSHLD, EDGE_THRESHOLD_MASK,
+                                      EDGE_TRSHLD, OV772X_EDGE_THRESHOLD_MASK,
                                       priv->info->edgectrl.threshold);
                 if (ret < 0)
                         goto ov772x_set_fmt_error;
 
                 ret = ov772x_mask_set(client,
-                                      EDGE_STRNGT, EDGE_STRENGTH_MASK,
+                                      EDGE_STRNGT, OV772X_EDGE_STRENGTH_MASK,
                                       priv->info->edgectrl.strength);
                 if (ret < 0)
                         goto ov772x_set_fmt_error;
@@ -840,13 +768,13 @@ static int ov772x_set_params(struct i2c_client *client, u32 *width, u32 *height,
                  * set upper and lower limit
                  */
                 ret = ov772x_mask_set(client,
-                                      EDGE_UPPER, EDGE_UPPER_MASK,
+                                      EDGE_UPPER, OV772X_EDGE_UPPER_MASK,
                                       priv->info->edgectrl.upper);
                 if (ret < 0)
                         goto ov772x_set_fmt_error;
 
                 ret = ov772x_mask_set(client,
-                                      EDGE_LOWER, EDGE_LOWER_MASK,
+                                      EDGE_LOWER, OV772X_EDGE_LOWER_MASK,
                                       priv->info->edgectrl.lower);
                 if (ret < 0)
                         goto ov772x_set_fmt_error;
@@ -1025,17 +953,12 @@ static int ov772x_try_fmt(struct v4l2_subdev *sd,
         return 0;
 }
 
-static int ov772x_video_probe(struct soc_camera_device *icd,
-                              struct i2c_client *client)
+static int ov772x_video_probe(struct i2c_client *client)
 {
         struct ov772x_priv *priv = to_ov772x(client);
         u8                  pid, ver;
         const char         *devname;
 
-        /* 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
          */
@@ -1064,20 +987,14 @@ static int ov772x_video_probe(struct soc_camera_device *icd,
                  ver,
                  i2c_smbus_read_byte_data(client, MIDH),
                  i2c_smbus_read_byte_data(client, MIDL));
-
-        return 0;
+        return v4l2_ctrl_handler_setup(&priv->hdl);
 }
 
-static struct soc_camera_ops ov772x_ops = {
-        .set_bus_param          = ov772x_set_bus_param,
-        .query_bus_param        = ov772x_query_bus_param,
-        .controls               = ov772x_controls,
-        .num_controls           = ARRAY_SIZE(ov772x_controls),
+static const struct v4l2_ctrl_ops ov772x_ctrl_ops = {
+        .s_ctrl = ov772x_s_ctrl,
 };
 
 static struct v4l2_subdev_core_ops ov772x_subdev_core_ops = {
-        .g_ctrl         = ov772x_g_ctrl,
-        .s_ctrl         = ov772x_s_ctrl,
         .g_chip_ident   = ov772x_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register     = ov772x_g_register,
@@ -1095,6 +1012,21 @@ static int ov772x_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
         return 0;
 }
 
+static int ov772x_g_mbus_config(struct v4l2_subdev *sd,
+                                struct v4l2_mbus_config *cfg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+        cfg->flags = V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_MASTER |
+                V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+                V4L2_MBUS_DATA_ACTIVE_HIGH;
+        cfg->type = V4L2_MBUS_PARALLEL;
+        cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+        return 0;
+}
+
 static struct v4l2_subdev_video_ops ov772x_subdev_video_ops = {
         .s_stream       = ov772x_s_stream,
         .g_mbus_fmt     = ov772x_g_fmt,
@@ -1103,6 +1035,7 @@ static struct v4l2_subdev_video_ops ov772x_subdev_video_ops = {
         .cropcap        = ov772x_cropcap,
         .g_crop         = ov772x_g_crop,
         .enum_mbus_fmt  = ov772x_enum_fmt,
+        .g_mbus_config  = ov772x_g_mbus_config,
 };
 
 static struct v4l2_subdev_ops ov772x_subdev_ops = {
@@ -1117,20 +1050,15 @@ static struct v4l2_subdev_ops ov772x_subdev_ops = {
 static int ov772x_probe(struct i2c_client *client,
                         const struct i2c_device_id *did)
 {
-        struct ov772x_priv        *priv;
-        struct soc_camera_device  *icd = client->dev.platform_data;
-        struct i2c_adapter        *adapter = to_i2c_adapter(client->dev.parent);
-        struct soc_camera_link    *icl;
-        int                        ret;
-
-        if (!icd) {
-                dev_err(&client->dev, "OV772X: missing soc-camera data!\n");
-                return -EINVAL;
-        }
+        struct ov772x_priv      *priv;
+        struct soc_camera_link  *icl = soc_camera_i2c_to_link(client);
+        struct i2c_adapter      *adapter = to_i2c_adapter(client->dev.parent);
+        int                     ret;
 
-        icl = to_soc_camera_link(icd);
-        if (!icl || !icl->priv)
+        if (!icl || !icl->priv) {
+                dev_err(&client->dev, "OV772X: missing platform data!\n");
                 return -EINVAL;
+        }
 
         if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
                 dev_err(&adapter->dev,
@@ -1146,12 +1074,24 @@ static int ov772x_probe(struct i2c_client *client,
         priv->info = icl->priv;
 
         v4l2_i2c_subdev_init(&priv->subdev, client, &ov772x_subdev_ops);
+        v4l2_ctrl_handler_init(&priv->hdl, 3);
+        v4l2_ctrl_new_std(&priv->hdl, &ov772x_ctrl_ops,
+                        V4L2_CID_VFLIP, 0, 1, 1, 0);
+        v4l2_ctrl_new_std(&priv->hdl, &ov772x_ctrl_ops,
+                        V4L2_CID_HFLIP, 0, 1, 1, 0);
+        v4l2_ctrl_new_std(&priv->hdl, &ov772x_ctrl_ops,
+                        V4L2_CID_BAND_STOP_FILTER, 0, 256, 1, 0);
+        priv->subdev.ctrl_handler = &priv->hdl;
+        if (priv->hdl.error) {
+                int err = priv->hdl.error;
 
-        icd->ops                = &ov772x_ops;
+                kfree(priv);
+                return err;
+        }
 
-        ret = ov772x_video_probe(icd, client);
+        ret = ov772x_video_probe(client);
         if (ret) {
-                icd->ops = NULL;
+                v4l2_ctrl_handler_free(&priv->hdl);
                 kfree(priv);
         }
 
@@ -1161,9 +1101,9 @@ static int ov772x_probe(struct i2c_client *client,
 static int ov772x_remove(struct i2c_client *client)
 {
         struct ov772x_priv *priv = to_ov772x(client);
-        struct soc_camera_device *icd = client->dev.platform_data;
 
-        icd->ops = NULL;
+        v4l2_device_unregister_subdev(&priv->subdev);
+        v4l2_ctrl_handler_free(&priv->hdl);
         kfree(priv);
         return 0;
 }
diff --git a/drivers/media/video/ov9640.c b/drivers/media/video/ov9640.c
index 3681a6ff0815..a4f99797eb56 100644
--- a/drivers/media/video/ov9640.c
+++ b/drivers/media/video/ov9640.c
@@ -24,10 +24,13 @@
 #include <linux/i2c.h>
 #include <linux/slab.h>
 #include <linux/delay.h>
+#include <linux/v4l2-mediabus.h>
 #include <linux/videodev2.h>
+
+#include <media/soc_camera.h>
 #include <media/v4l2-chip-ident.h>
 #include <media/v4l2-common.h>
-#include <media/soc_camera.h>
+#include <media/v4l2-ctrls.h>
 
 #include "ov9640.h"
 
@@ -162,27 +165,6 @@ static enum v4l2_mbus_pixelcode ov9640_codes[] = {
         V4L2_MBUS_FMT_RGB565_2X8_LE,
 };
 
-static const struct v4l2_queryctrl ov9640_controls[] = {
-        {
-                .id             = V4L2_CID_VFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Vertically",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        },
-        {
-                .id             = V4L2_CID_HFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Horizontally",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        },
-};
-
 /* read a register */
 static int ov9640_reg_read(struct i2c_client *client, u8 reg, u8 *val)
 {
@@ -284,75 +266,25 @@ static int ov9640_s_stream(struct v4l2_subdev *sd, int enable)
         return 0;
 }
 
-/* Alter bus settings on camera side */
-static int ov9640_set_bus_param(struct soc_camera_device *icd,
-                                unsigned long flags)
-{
-        return 0;
-}
-
-/* Request bus settings on camera side */
-static unsigned long ov9640_query_bus_param(struct soc_camera_device *icd)
-{
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-
-        /*
-         * REVISIT: the camera probably can do 10 bit transfers, but I don't
-         *          have those pins connected on my hardware.
-         */
-        unsigned long flags = SOCAM_PCLK_SAMPLE_RISING | SOCAM_MASTER |
-                SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_HIGH |
-                SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATAWIDTH_8;
-
-        return soc_camera_apply_sensor_flags(icl, flags);
-}
-
-/* Get status of additional camera capabilities */
-static int ov9640_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-        struct ov9640_priv *priv = to_ov9640_sensor(sd);
-
-        switch (ctrl->id) {
-        case V4L2_CID_VFLIP:
-                ctrl->value = priv->flag_vflip;
-                break;
-        case V4L2_CID_HFLIP:
-                ctrl->value = priv->flag_hflip;
-                break;
-        }
-        return 0;
-}
-
 /* Set status of additional camera capabilities */
-static int ov9640_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int ov9640_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-        struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct ov9640_priv *priv = to_ov9640_sensor(sd);
-
-        int ret = 0;
+        struct ov9640_priv *priv = container_of(ctrl->handler, struct ov9640_priv, hdl);
+        struct i2c_client *client = v4l2_get_subdevdata(&priv->subdev);
 
         switch (ctrl->id) {
         case V4L2_CID_VFLIP:
-                priv->flag_vflip = ctrl->value;
-                if (ctrl->value)
-                        ret = ov9640_reg_rmw(client, OV9640_MVFP,
+                if (ctrl->val)
+                        return ov9640_reg_rmw(client, OV9640_MVFP,
                                                         OV9640_MVFP_V, 0);
-                else
-                        ret = ov9640_reg_rmw(client, OV9640_MVFP,
-                                                        0, OV9640_MVFP_V);
-                break;
+                return ov9640_reg_rmw(client, OV9640_MVFP, 0, OV9640_MVFP_V);
         case V4L2_CID_HFLIP:
-                priv->flag_hflip = ctrl->value;
-                if (ctrl->value)
-                        ret = ov9640_reg_rmw(client, OV9640_MVFP,
+                if (ctrl->val)
+                        return ov9640_reg_rmw(client, OV9640_MVFP,
                                                         OV9640_MVFP_H, 0);
-                else
-                        ret = ov9640_reg_rmw(client, OV9640_MVFP,
-                                                        0, OV9640_MVFP_H);
-                break;
+                return ov9640_reg_rmw(client, OV9640_MVFP, 0, OV9640_MVFP_H);
         }
-
-        return ret;
+        return -EINVAL;
 }
 
 /* Get chip identification */
@@ -646,10 +578,7 @@ static int ov9640_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
         return 0;
 }
 
-
-
-static int ov9640_video_probe(struct soc_camera_device *icd,
-                                struct i2c_client *client)
+static int ov9640_video_probe(struct i2c_client *client)
 {
         struct v4l2_subdev *sd = i2c_get_clientdata(client);
         struct ov9640_priv *priv = to_ov9640_sensor(sd);
@@ -657,29 +586,19 @@ 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. */
-        BUG_ON(!icd->parent ||
-               to_soc_camera_host(icd->parent)->nr != icd->iface);
-
         /*
          * check and show product ID and manufacturer ID
          */
 
         ret = ov9640_reg_read(client, OV9640_PID, &pid);
+        if (!ret)
+                ret = ov9640_reg_read(client, OV9640_VER, &ver);
+        if (!ret)
+                ret = ov9640_reg_read(client, OV9640_MIDH, &midh);
+        if (!ret)
+                ret = ov9640_reg_read(client, OV9640_MIDL, &midl);
         if (ret)
-                goto err;
-
-        ret = ov9640_reg_read(client, OV9640_VER, &ver);
-        if (ret)
-                goto err;
-
-        ret = ov9640_reg_read(client, OV9640_MIDH, &midh);
-        if (ret)
-                goto err;
-
-        ret = ov9640_reg_read(client, OV9640_MIDL, &midl);
-        if (ret)
-                goto err;
+                return ret;
 
         switch (VERSION(pid, ver)) {
         case OV9640_V2:
@@ -693,27 +612,20 @@ static int ov9640_video_probe(struct soc_camera_device *icd,
                 break;
         default:
                 dev_err(&client->dev, "Product ID error %x:%x\n", pid, ver);
-                ret = -ENODEV;
-                goto err;
+                return -ENODEV;
         }
 
         dev_info(&client->dev, "%s Product ID %0x:%0x Manufacturer ID %x:%x\n",
                  devname, pid, ver, midh, midl);
 
-err:
-        return ret;
+        return v4l2_ctrl_handler_setup(&priv->hdl);
 }
 
-static struct soc_camera_ops ov9640_ops = {
-        .set_bus_param          = ov9640_set_bus_param,
-        .query_bus_param        = ov9640_query_bus_param,
-        .controls               = ov9640_controls,
-        .num_controls           = ARRAY_SIZE(ov9640_controls),
+static const struct v4l2_ctrl_ops ov9640_ctrl_ops = {
+        .s_ctrl = ov9640_s_ctrl,
 };
 
 static struct v4l2_subdev_core_ops ov9640_core_ops = {
-        .g_ctrl                 = ov9640_g_ctrl,
-        .s_ctrl                 = ov9640_s_ctrl,
         .g_chip_ident           = ov9640_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register             = ov9640_get_register,
@@ -722,6 +634,22 @@ static struct v4l2_subdev_core_ops ov9640_core_ops = {
 
 };
 
+/* Request bus settings on camera side */
+static int ov9640_g_mbus_config(struct v4l2_subdev *sd,
+                                struct v4l2_mbus_config *cfg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+        cfg->flags = V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_MASTER |
+                V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+                V4L2_MBUS_DATA_ACTIVE_HIGH;
+        cfg->type = V4L2_MBUS_PARALLEL;
+        cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+        return 0;
+}
+
 static struct v4l2_subdev_video_ops ov9640_video_ops = {
         .s_stream       = ov9640_s_stream,
         .s_mbus_fmt     = ov9640_s_fmt,
@@ -729,7 +657,7 @@ static struct v4l2_subdev_video_ops ov9640_video_ops = {
         .enum_mbus_fmt  = ov9640_enum_fmt,
         .cropcap        = ov9640_cropcap,
         .g_crop         = ov9640_g_crop,
-
+        .g_mbus_config  = ov9640_g_mbus_config,
 };
 
 static struct v4l2_subdev_ops ov9640_subdev_ops = {
@@ -744,16 +672,9 @@ static int ov9640_probe(struct i2c_client *client,
                         const struct i2c_device_id *did)
 {
         struct ov9640_priv *priv;
-        struct soc_camera_device *icd   = client->dev.platform_data;
-        struct soc_camera_link *icl;
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
         int ret;
 
-        if (!icd) {
-                dev_err(&client->dev, "Missing soc-camera data!\n");
-                return -EINVAL;
-        }
-
-        icl = to_soc_camera_link(icd);
         if (!icl) {
                 dev_err(&client->dev, "Missing platform_data for driver\n");
                 return -EINVAL;
@@ -768,12 +689,23 @@ static int ov9640_probe(struct i2c_client *client,
 
         v4l2_i2c_subdev_init(&priv->subdev, client, &ov9640_subdev_ops);
 
-        icd->ops        = &ov9640_ops;
+        v4l2_ctrl_handler_init(&priv->hdl, 2);
+        v4l2_ctrl_new_std(&priv->hdl, &ov9640_ctrl_ops,
+                        V4L2_CID_VFLIP, 0, 1, 1, 0);
+        v4l2_ctrl_new_std(&priv->hdl, &ov9640_ctrl_ops,
+                        V4L2_CID_HFLIP, 0, 1, 1, 0);
+        priv->subdev.ctrl_handler = &priv->hdl;
+        if (priv->hdl.error) {
+                int err = priv->hdl.error;
+
+                kfree(priv);
+                return err;
+        }
 
-        ret = ov9640_video_probe(icd, client);
+        ret = ov9640_video_probe(client);
 
         if (ret) {
-                icd->ops = NULL;
+                v4l2_ctrl_handler_free(&priv->hdl);
                 kfree(priv);
         }
 
@@ -785,6 +717,8 @@ static int ov9640_remove(struct i2c_client *client)
         struct v4l2_subdev *sd = i2c_get_clientdata(client);
         struct ov9640_priv *priv = to_ov9640_sensor(sd);
 
+        v4l2_device_unregister_subdev(&priv->subdev);
+        v4l2_ctrl_handler_free(&priv->hdl);
         kfree(priv);
         return 0;
 }
diff --git a/drivers/media/video/ov9640.h b/drivers/media/video/ov9640.h
index f8a51b70792e..6b33a972c83c 100644
--- a/drivers/media/video/ov9640.h
+++ b/drivers/media/video/ov9640.h
@@ -198,12 +198,10 @@ struct ov9640_reg {
 
 struct ov9640_priv {
         struct v4l2_subdev              subdev;
+        struct v4l2_ctrl_handler        hdl;
 
         int                             model;
         int                             revision;
-
-        bool                            flag_vflip;
-        bool                            flag_hflip;
 };
 
 #endif  /* __DRIVERS_MEDIA_VIDEO_OV9640_H__ */
diff --git a/drivers/media/video/ov9740.c b/drivers/media/video/ov9740.c
index edd1ffcca30b..d9a9f7174f7a 100644
--- a/drivers/media/video/ov9740.c
+++ b/drivers/media/video/ov9740.c
@@ -14,8 +14,11 @@
 #include <linux/module.h>
 #include <linux/i2c.h>
 #include <linux/slab.h>
-#include <media/v4l2-chip-ident.h>
+#include <linux/v4l2-mediabus.h>
+
 #include <media/soc_camera.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
 
 #define to_ov9740(sd)           container_of(sd, struct ov9740_priv, subdev)
 
@@ -192,6 +195,7 @@ struct ov9740_reg {
 
 struct ov9740_priv {
         struct v4l2_subdev              subdev;
+        struct v4l2_ctrl_handler        hdl;
 
         int                             ident;
         u16                             model;
@@ -392,27 +396,6 @@ static enum v4l2_mbus_pixelcode ov9740_codes[] = {
         V4L2_MBUS_FMT_YUYV8_2X8,
 };
 
-static const struct v4l2_queryctrl ov9740_controls[] = {
-        {
-                .id             = V4L2_CID_VFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Vertically",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        },
-        {
-                .id             = V4L2_CID_HFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Horizontally",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        },
-};
-
 /* read a register */
 static int ov9740_reg_read(struct i2c_client *client, u16 reg, u8 *val)
 {
@@ -560,25 +543,6 @@ static int ov9740_s_stream(struct v4l2_subdev *sd, int enable)
         return ret;
 }
 
-/* Alter bus settings on camera side */
-static int ov9740_set_bus_param(struct soc_camera_device *icd,
-                                unsigned long flags)
-{
-        return 0;
-}
-
-/* Request bus settings on camera side */
-static unsigned long ov9740_query_bus_param(struct soc_camera_device *icd)
-{
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-
-        unsigned long flags = SOCAM_PCLK_SAMPLE_RISING | SOCAM_MASTER |
-                SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_HIGH |
-                SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATAWIDTH_8;
-
-        return soc_camera_apply_sensor_flags(icl, flags);
-}
-
 /* select nearest higher resolution for capture */
 static void ov9740_res_roundup(u32 *width, u32 *height)
 {
@@ -788,36 +752,18 @@ static int ov9740_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
         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)
+static int ov9740_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-        struct ov9740_priv *priv = to_ov9740(sd);
+        struct ov9740_priv *priv =
+                container_of(ctrl->handler, struct ov9740_priv, hdl);
 
         switch (ctrl->id) {
         case V4L2_CID_VFLIP:
-                priv->flag_vflip = ctrl->value;
+                priv->flag_vflip = ctrl->val;
                 break;
         case V4L2_CID_HFLIP:
-                priv->flag_hflip = ctrl->value;
+                priv->flag_hflip = ctrl->val;
                 break;
         default:
                 return -EINVAL;
@@ -890,18 +836,13 @@ static int ov9740_set_register(struct v4l2_subdev *sd,
 }
 #endif
 
-static int ov9740_video_probe(struct soc_camera_device *icd,
-                              struct i2c_client *client)
+static int ov9740_video_probe(struct i2c_client *client)
 {
         struct v4l2_subdev *sd = i2c_get_clientdata(client);
         struct ov9740_priv *priv = to_ov9740(sd);
         u8 modelhi, modello;
         int ret;
 
-        /* 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
          */
@@ -942,25 +883,33 @@ err:
         return ret;
 }
 
-static struct soc_camera_ops ov9740_ops = {
-        .set_bus_param          = ov9740_set_bus_param,
-        .query_bus_param        = ov9740_query_bus_param,
-        .controls               = ov9740_controls,
-        .num_controls           = ARRAY_SIZE(ov9740_controls),
-};
+/* Request bus settings on camera side */
+static int ov9740_g_mbus_config(struct v4l2_subdev *sd,
+                                struct v4l2_mbus_config *cfg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+        cfg->flags = V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_MASTER |
+                V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+                V4L2_MBUS_DATA_ACTIVE_HIGH;
+        cfg->type = V4L2_MBUS_PARALLEL;
+        cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+        return 0;
+}
 
 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,
+        .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,
+        .g_mbus_config  = ov9740_g_mbus_config,
 };
 
 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
@@ -974,6 +923,10 @@ static struct v4l2_subdev_ops ov9740_subdev_ops = {
         .video                  = &ov9740_video_ops,
 };
 
+static const struct v4l2_ctrl_ops ov9740_ctrl_ops = {
+        .s_ctrl = ov9740_s_ctrl,
+};
+
 /*
  * i2c_driver function
  */
@@ -981,16 +934,9 @@ static int ov9740_probe(struct i2c_client *client,
                         const struct i2c_device_id *did)
 {
         struct ov9740_priv *priv;
-        struct soc_camera_device *icd   = client->dev.platform_data;
-        struct soc_camera_link *icl;
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
         int ret;
 
-        if (!icd) {
-                dev_err(&client->dev, "Missing soc-camera data!\n");
-                return -EINVAL;
-        }
-
-        icl = to_soc_camera_link(icd);
         if (!icl) {
                 dev_err(&client->dev, "Missing platform_data for driver\n");
                 return -EINVAL;
@@ -1003,12 +949,24 @@ static int ov9740_probe(struct i2c_client *client,
         }
 
         v4l2_i2c_subdev_init(&priv->subdev, client, &ov9740_subdev_ops);
+        v4l2_ctrl_handler_init(&priv->hdl, 13);
+        v4l2_ctrl_new_std(&priv->hdl, &ov9740_ctrl_ops,
+                        V4L2_CID_VFLIP, 0, 1, 1, 0);
+        v4l2_ctrl_new_std(&priv->hdl, &ov9740_ctrl_ops,
+                        V4L2_CID_HFLIP, 0, 1, 1, 0);
+        priv->subdev.ctrl_handler = &priv->hdl;
+        if (priv->hdl.error) {
+                int err = priv->hdl.error;
 
-        icd->ops = &ov9740_ops;
+                kfree(priv);
+                return err;
+        }
 
-        ret = ov9740_video_probe(icd, client);
+        ret = ov9740_video_probe(client);
+        if (!ret)
+                ret = v4l2_ctrl_handler_setup(&priv->hdl);
         if (ret < 0) {
-                icd->ops = NULL;
+                v4l2_ctrl_handler_free(&priv->hdl);
                 kfree(priv);
         }
 
@@ -1019,8 +977,9 @@ static int ov9740_remove(struct i2c_client *client)
 {
         struct ov9740_priv *priv = i2c_get_clientdata(client);
 
+        v4l2_device_unregister_subdev(&priv->subdev);
+        v4l2_ctrl_handler_free(&priv->hdl);
         kfree(priv);
-
         return 0;
 }
 
diff --git a/drivers/media/video/pwc/pwc-if.c b/drivers/media/video/pwc/pwc-if.c
index 360be226718d..01ff643e682d 100644
--- a/drivers/media/video/pwc/pwc-if.c
+++ b/drivers/media/video/pwc/pwc-if.c
@@ -744,9 +744,9 @@ static int pwc_video_mmap(struct file *file, struct vm_area_struct *vma)
 /***************************************************************************/
 /* Videobuf2 operations */
 
-static int queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
-                                unsigned int *nplanes, unsigned int sizes[],
-                                void *alloc_ctxs[])
+static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
+                                unsigned int *nbuffers, unsigned int *nplanes,
+                                unsigned int sizes[], void *alloc_ctxs[])
 {
         struct pwc_device *pdev = vb2_get_drv_priv(vq);
 
diff --git a/drivers/media/video/pxa_camera.c b/drivers/media/video/pxa_camera.c
index d07df22a5ec6..79fb22c89ae9 100644
--- a/drivers/media/video/pxa_camera.c
+++ b/drivers/media/video/pxa_camera.c
@@ -214,6 +214,7 @@ struct pxa_camera_dev {
         unsigned long           ciclk;
         unsigned long           mclk;
         u32                     mclk_divisor;
+        u16                     width_flags;    /* max 10 bits */
 
         struct list_head        capture;
 
@@ -1020,37 +1021,20 @@ static int test_platform_param(struct pxa_camera_dev *pcdev,
          * quick capture interface supports both.
          */
         *flags = (pcdev->platform_flags & PXA_CAMERA_MASTER ?
-                  SOCAM_MASTER : SOCAM_SLAVE) |
-                SOCAM_HSYNC_ACTIVE_HIGH |
-                SOCAM_HSYNC_ACTIVE_LOW |
-                SOCAM_VSYNC_ACTIVE_HIGH |
-                SOCAM_VSYNC_ACTIVE_LOW |
-                SOCAM_DATA_ACTIVE_HIGH |
-                SOCAM_PCLK_SAMPLE_RISING |
-                SOCAM_PCLK_SAMPLE_FALLING;
+                  V4L2_MBUS_MASTER : V4L2_MBUS_SLAVE) |
+                V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+                V4L2_MBUS_HSYNC_ACTIVE_LOW |
+                V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+                V4L2_MBUS_VSYNC_ACTIVE_LOW |
+                V4L2_MBUS_DATA_ACTIVE_HIGH |
+                V4L2_MBUS_PCLK_SAMPLE_RISING |
+                V4L2_MBUS_PCLK_SAMPLE_FALLING;
 
         /* If requested data width is supported by the platform, use it */
-        switch (buswidth) {
-        case 10:
-                if (!(pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_10))
-                        return -EINVAL;
-                *flags |= SOCAM_DATAWIDTH_10;
-                break;
-        case 9:
-                if (!(pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_9))
-                        return -EINVAL;
-                *flags |= SOCAM_DATAWIDTH_9;
-                break;
-        case 8:
-                if (!(pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_8))
-                        return -EINVAL;
-                *flags |= SOCAM_DATAWIDTH_8;
-                break;
-        default:
-                return -EINVAL;
-        }
+        if ((1 << (buswidth - 1)) & pcdev->width_flags)
+                return 0;
 
-        return 0;
+        return -EINVAL;
 }
 
 static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
@@ -1070,12 +1054,12 @@ static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
          * Datawidth is now guaranteed to be equal to one of the three values.
          * We fix bit-per-pixel equal to data-width...
          */
-        switch (flags & SOCAM_DATAWIDTH_MASK) {
-        case SOCAM_DATAWIDTH_10:
+        switch (icd->current_fmt->host_fmt->bits_per_sample) {
+        case 10:
                 dw = 4;
                 bpp = 0x40;
                 break;
-        case SOCAM_DATAWIDTH_9:
+        case 9:
                 dw = 3;
                 bpp = 0x20;
                 break;
@@ -1084,7 +1068,7 @@ static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
                  * Actually it can only be 8 now,
                  * default is just to silence compiler warnings
                  */
-        case SOCAM_DATAWIDTH_8:
+        case 8:
                 dw = 2;
                 bpp = 0;
         }
@@ -1093,11 +1077,11 @@ static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
                 cicr4 |= CICR4_PCLK_EN;
         if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
                 cicr4 |= CICR4_MCLK_EN;
-        if (flags & SOCAM_PCLK_SAMPLE_FALLING)
+        if (flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
                 cicr4 |= CICR4_PCP;
-        if (flags & SOCAM_HSYNC_ACTIVE_LOW)
+        if (flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
                 cicr4 |= CICR4_HSP;
-        if (flags & SOCAM_VSYNC_ACTIVE_LOW)
+        if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
                 cicr4 |= CICR4_VSP;
 
         cicr0 = __raw_readl(pcdev->base + CICR0);
@@ -1151,9 +1135,11 @@ 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 v4l2_subdev *sd = soc_camera_to_subdev(icd);
         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;
+        struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+        unsigned long bus_flags, common_flags;
         int ret;
         struct pxa_cam *cam = icd->host_priv;
 
@@ -1162,44 +1148,58 @@ static int pxa_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
         if (ret < 0)
                 return ret;
 
-        camera_flags = icd->ops->query_bus_param(icd);
-
-        common_flags = soc_camera_bus_param_compatible(camera_flags, bus_flags);
-        if (!common_flags)
-                return -EINVAL;
+        ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+        if (!ret) {
+                common_flags = soc_mbus_config_compatible(&cfg,
+                                                          bus_flags);
+                if (!common_flags) {
+                        dev_warn(icd->parent,
+                                 "Flags incompatible: camera 0x%x, host 0x%lx\n",
+                                 cfg.flags, bus_flags);
+                        return -EINVAL;
+                }
+        } else if (ret != -ENOIOCTLCMD) {
+                return ret;
+        } else {
+                common_flags = bus_flags;
+        }
 
         pcdev->channels = 1;
 
         /* Make choises, based on platform preferences */
-        if ((common_flags & SOCAM_HSYNC_ACTIVE_HIGH) &&
-            (common_flags & SOCAM_HSYNC_ACTIVE_LOW)) {
+        if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
+            (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
                 if (pcdev->platform_flags & PXA_CAMERA_HSP)
-                        common_flags &= ~SOCAM_HSYNC_ACTIVE_HIGH;
+                        common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
                 else
-                        common_flags &= ~SOCAM_HSYNC_ACTIVE_LOW;
+                        common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
         }
 
-        if ((common_flags & SOCAM_VSYNC_ACTIVE_HIGH) &&
-            (common_flags & SOCAM_VSYNC_ACTIVE_LOW)) {
+        if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
+            (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
                 if (pcdev->platform_flags & PXA_CAMERA_VSP)
-                        common_flags &= ~SOCAM_VSYNC_ACTIVE_HIGH;
+                        common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
                 else
-                        common_flags &= ~SOCAM_VSYNC_ACTIVE_LOW;
+                        common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
         }
 
-        if ((common_flags & SOCAM_PCLK_SAMPLE_RISING) &&
-            (common_flags & SOCAM_PCLK_SAMPLE_FALLING)) {
+        if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
+            (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
                 if (pcdev->platform_flags & PXA_CAMERA_PCP)
-                        common_flags &= ~SOCAM_PCLK_SAMPLE_RISING;
+                        common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
                 else
-                        common_flags &= ~SOCAM_PCLK_SAMPLE_FALLING;
+                        common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
         }
 
-        cam->flags = common_flags;
-
-        ret = icd->ops->set_bus_param(icd, common_flags);
-        if (ret < 0)
+        cfg.flags = common_flags;
+        ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
+        if (ret < 0 && ret != -ENOIOCTLCMD) {
+                dev_dbg(icd->parent, "camera s_mbus_config(0x%lx) returned %d\n",
+                        common_flags, ret);
                 return ret;
+        }
+
+        cam->flags = common_flags;
 
         pxa_camera_setup_cicr(icd, common_flags, pixfmt);
 
@@ -1209,17 +1209,31 @@ 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 v4l2_subdev *sd = soc_camera_to_subdev(icd);
         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct pxa_camera_dev *pcdev = ici->priv;
-        unsigned long bus_flags, camera_flags;
+        struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+        unsigned long bus_flags, common_flags;
         int ret = test_platform_param(pcdev, buswidth, &bus_flags);
 
         if (ret < 0)
                 return ret;
 
-        camera_flags = icd->ops->query_bus_param(icd);
+        ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+        if (!ret) {
+                common_flags = soc_mbus_config_compatible(&cfg,
+                                                          bus_flags);
+                if (!common_flags) {
+                        dev_warn(icd->parent,
+                                 "Flags incompatible: camera 0x%x, host 0x%lx\n",
+                                 cfg.flags, bus_flags);
+                        return -EINVAL;
+                }
+        } else if (ret == -ENOIOCTLCMD) {
+                ret = 0;
+        }
 
-        return soc_camera_bus_param_compatible(camera_flags, bus_flags) ? 0 : -EINVAL;
+        return ret;
 }
 
 static const struct soc_mbus_pixelfmt pxa_camera_formats[] = {
@@ -1687,6 +1701,12 @@ static int __devinit pxa_camera_probe(struct platform_device *pdev)
                          "data widths, using default 10 bit\n");
                 pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_10;
         }
+        if (pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_8)
+                pcdev->width_flags = 1 << 7;
+        if (pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_9)
+                pcdev->width_flags |= 1 << 8;
+        if (pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_10)
+                pcdev->width_flags |= 1 << 9;
         pcdev->mclk = pcdev->pdata->mclk_10khz * 10000;
         if (!pcdev->mclk) {
                 dev_warn(&pdev->dev,
diff --git a/drivers/media/video/rj54n1cb0c.c b/drivers/media/video/rj54n1cb0c.c
index 847ccc067e87..6afc61689549 100644
--- a/drivers/media/video/rj54n1cb0c.c
+++ b/drivers/media/video/rj54n1cb0c.c
@@ -11,13 +11,14 @@
 #include <linux/delay.h>
 #include <linux/i2c.h>
 #include <linux/slab.h>
+#include <linux/v4l2-mediabus.h>
 #include <linux/videodev2.h>
 
 #include <media/rj54n1cb0c.h>
 #include <media/soc_camera.h>
-#include <media/soc_mediabus.h>
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
 
 #define RJ54N1_DEV_CODE                 0x0400
 #define RJ54N1_DEV_CODE2                0x0401
@@ -148,6 +149,7 @@ struct rj54n1_clock_div {
 
 struct rj54n1 {
         struct v4l2_subdev subdev;
+        struct v4l2_ctrl_handler hdl;
         struct rj54n1_clock_div clk_div;
         const struct rj54n1_datafmt *fmt;
         struct v4l2_rect rect;  /* Sensor window */
@@ -499,31 +501,6 @@ static int rj54n1_s_stream(struct v4l2_subdev *sd, int enable)
         return reg_set(client, RJ54N1_STILL_CONTROL, (!enable) << 7, 0x80);
 }
 
-static int rj54n1_set_bus_param(struct soc_camera_device *icd,
-                                unsigned long flags)
-{
-        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        struct i2c_client *client = v4l2_get_subdevdata(sd);
-        /* Figures 2.5-1 to 2.5-3 - default falling pixclk edge */
-
-        if (flags & SOCAM_PCLK_SAMPLE_RISING)
-                return reg_write(client, RJ54N1_OUT_SIGPO, 1 << 4);
-        else
-                return reg_write(client, RJ54N1_OUT_SIGPO, 0);
-}
-
-static unsigned long rj54n1_query_bus_param(struct soc_camera_device *icd)
-{
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-        const unsigned long flags =
-                SOCAM_PCLK_SAMPLE_RISING | SOCAM_PCLK_SAMPLE_FALLING |
-                SOCAM_MASTER | SOCAM_DATAWIDTH_8 |
-                SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_HIGH |
-                SOCAM_DATA_ACTIVE_HIGH;
-
-        return soc_camera_apply_sensor_flags(icl, flags);
-}
-
 static int rj54n1_set_rect(struct i2c_client *client,
                            u16 reg_x, u16 reg_y, u16 reg_xy,
                            u32 width, u32 height)
@@ -1202,134 +1179,51 @@ static int rj54n1_s_register(struct v4l2_subdev *sd,
 }
 #endif
 
-static const struct v4l2_queryctrl rj54n1_controls[] = {
-        {
-                .id             = V4L2_CID_VFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Vertically",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        }, {
-                .id             = V4L2_CID_HFLIP,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Flip Horizontally",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        }, {
-                .id             = V4L2_CID_GAIN,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Gain",
-                .minimum        = 0,
-                .maximum        = 127,
-                .step           = 1,
-                .default_value  = 66,
-                .flags          = V4L2_CTRL_FLAG_SLIDER,
-        }, {
-                .id             = V4L2_CID_AUTO_WHITE_BALANCE,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Auto white balance",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 1,
-        },
-};
-
-static struct soc_camera_ops rj54n1_ops = {
-        .set_bus_param          = rj54n1_set_bus_param,
-        .query_bus_param        = rj54n1_query_bus_param,
-        .controls               = rj54n1_controls,
-        .num_controls           = ARRAY_SIZE(rj54n1_controls),
-};
-
-static int rj54n1_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int rj54n1_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+        struct rj54n1 *rj54n1 = container_of(ctrl->handler, struct rj54n1, hdl);
+        struct v4l2_subdev *sd = &rj54n1->subdev;
         struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct rj54n1 *rj54n1 = to_rj54n1(client);
         int data;
 
         switch (ctrl->id) {
         case V4L2_CID_VFLIP:
-                data = reg_read(client, RJ54N1_MIRROR_STILL_MODE);
-                if (data < 0)
-                        return -EIO;
-                ctrl->value = !(data & 1);
-                break;
-        case V4L2_CID_HFLIP:
-                data = reg_read(client, RJ54N1_MIRROR_STILL_MODE);
-                if (data < 0)
-                        return -EIO;
-                ctrl->value = !(data & 2);
-                break;
-        case V4L2_CID_GAIN:
-                data = reg_read(client, RJ54N1_Y_GAIN);
-                if (data < 0)
-                        return -EIO;
-
-                ctrl->value = data / 2;
-                break;
-        case V4L2_CID_AUTO_WHITE_BALANCE:
-                ctrl->value = rj54n1->auto_wb;
-                break;
-        }
-
-        return 0;
-}
-
-static int rj54n1_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-        int data;
-        struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct rj54n1 *rj54n1 = to_rj54n1(client);
-        const struct v4l2_queryctrl *qctrl;
-
-        qctrl = soc_camera_find_qctrl(&rj54n1_ops, ctrl->id);
-        if (!qctrl)
-                return -EINVAL;
-
-        switch (ctrl->id) {
-        case V4L2_CID_VFLIP:
-                if (ctrl->value)
+                if (ctrl->val)
                         data = reg_set(client, RJ54N1_MIRROR_STILL_MODE, 0, 1);
                 else
                         data = reg_set(client, RJ54N1_MIRROR_STILL_MODE, 1, 1);
                 if (data < 0)
                         return -EIO;
-                break;
+                return 0;
         case V4L2_CID_HFLIP:
-                if (ctrl->value)
+                if (ctrl->val)
                         data = reg_set(client, RJ54N1_MIRROR_STILL_MODE, 0, 2);
                 else
                         data = reg_set(client, RJ54N1_MIRROR_STILL_MODE, 2, 2);
                 if (data < 0)
                         return -EIO;
-                break;
+                return 0;
         case V4L2_CID_GAIN:
-                if (ctrl->value > qctrl->maximum ||
-                    ctrl->value < qctrl->minimum)
-                        return -EINVAL;
-                else if (reg_write(client, RJ54N1_Y_GAIN, ctrl->value * 2) < 0)
+                if (reg_write(client, RJ54N1_Y_GAIN, ctrl->val * 2) < 0)
                         return -EIO;
-                break;
+                return 0;
         case V4L2_CID_AUTO_WHITE_BALANCE:
                 /* Auto WB area - whole image */
-                if (reg_set(client, RJ54N1_WB_SEL_WEIGHT_I, ctrl->value << 7,
+                if (reg_set(client, RJ54N1_WB_SEL_WEIGHT_I, ctrl->val << 7,
                             0x80) < 0)
                         return -EIO;
-                rj54n1->auto_wb = ctrl->value;
-                break;
+                rj54n1->auto_wb = ctrl->val;
+                return 0;
         }
 
-        return 0;
+        return -EINVAL;
 }
 
+static const struct v4l2_ctrl_ops rj54n1_ctrl_ops = {
+        .s_ctrl = rj54n1_s_ctrl,
+};
+
 static struct v4l2_subdev_core_ops rj54n1_subdev_core_ops = {
-        .g_ctrl         = rj54n1_g_ctrl,
-        .s_ctrl         = rj54n1_s_ctrl,
         .g_chip_ident   = rj54n1_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register     = rj54n1_g_register,
@@ -1337,6 +1231,36 @@ static struct v4l2_subdev_core_ops rj54n1_subdev_core_ops = {
 #endif
 };
 
+static int rj54n1_g_mbus_config(struct v4l2_subdev *sd,
+                                struct v4l2_mbus_config *cfg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+        cfg->flags =
+                V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING |
+                V4L2_MBUS_MASTER | V4L2_MBUS_DATA_ACTIVE_HIGH |
+                V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH;
+        cfg->type = V4L2_MBUS_PARALLEL;
+        cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+        return 0;
+}
+
+static int rj54n1_s_mbus_config(struct v4l2_subdev *sd,
+                                const struct v4l2_mbus_config *cfg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+        /* Figures 2.5-1 to 2.5-3 - default falling pixclk edge */
+        if (soc_camera_apply_board_flags(icl, cfg) &
+            V4L2_MBUS_PCLK_SAMPLE_RISING)
+                return reg_write(client, RJ54N1_OUT_SIGPO, 1 << 4);
+        else
+                return reg_write(client, RJ54N1_OUT_SIGPO, 0);
+}
+
 static struct v4l2_subdev_video_ops rj54n1_subdev_video_ops = {
         .s_stream       = rj54n1_s_stream,
         .s_mbus_fmt     = rj54n1_s_fmt,
@@ -1346,6 +1270,8 @@ static struct v4l2_subdev_video_ops rj54n1_subdev_video_ops = {
         .g_crop         = rj54n1_g_crop,
         .s_crop         = rj54n1_s_crop,
         .cropcap        = rj54n1_cropcap,
+        .g_mbus_config  = rj54n1_g_mbus_config,
+        .s_mbus_config  = rj54n1_s_mbus_config,
 };
 
 static struct v4l2_subdev_ops rj54n1_subdev_ops = {
@@ -1357,17 +1283,12 @@ static struct v4l2_subdev_ops rj54n1_subdev_ops = {
  * Interface active, can use i2c. If it fails, it can indeed mean, that
  * this wasn't our capture interface, so, we wait for the right one
  */
-static int rj54n1_video_probe(struct soc_camera_device *icd,
-                              struct i2c_client *client,
+static int rj54n1_video_probe(struct i2c_client *client,
                               struct rj54n1_pdata *priv)
 {
         int data1, data2;
         int ret;
 
-        /* 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);
         data2 = reg_read(client, RJ54N1_DEV_CODE2);
@@ -1395,18 +1316,11 @@ static int rj54n1_probe(struct i2c_client *client,
                         const struct i2c_device_id *did)
 {
         struct rj54n1 *rj54n1;
-        struct soc_camera_device *icd = client->dev.platform_data;
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
         struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
-        struct soc_camera_link *icl;
         struct rj54n1_pdata *rj54n1_priv;
         int ret;
 
-        if (!icd) {
-                dev_err(&client->dev, "RJ54N1CB0C: missing soc-camera data!\n");
-                return -EINVAL;
-        }
-
-        icl = to_soc_camera_link(icd);
         if (!icl || !icl->priv) {
                 dev_err(&client->dev, "RJ54N1CB0C: missing platform data!\n");
                 return -EINVAL;
@@ -1425,8 +1339,22 @@ static int rj54n1_probe(struct i2c_client *client,
                 return -ENOMEM;
 
         v4l2_i2c_subdev_init(&rj54n1->subdev, client, &rj54n1_subdev_ops);
+        v4l2_ctrl_handler_init(&rj54n1->hdl, 4);
+        v4l2_ctrl_new_std(&rj54n1->hdl, &rj54n1_ctrl_ops,
+                        V4L2_CID_VFLIP, 0, 1, 1, 0);
+        v4l2_ctrl_new_std(&rj54n1->hdl, &rj54n1_ctrl_ops,
+                        V4L2_CID_HFLIP, 0, 1, 1, 0);
+        v4l2_ctrl_new_std(&rj54n1->hdl, &rj54n1_ctrl_ops,
+                        V4L2_CID_GAIN, 0, 127, 1, 66);
+        v4l2_ctrl_new_std(&rj54n1->hdl, &rj54n1_ctrl_ops,
+                        V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
+        rj54n1->subdev.ctrl_handler = &rj54n1->hdl;
+        if (rj54n1->hdl.error) {
+                int err = rj54n1->hdl.error;
 
-        icd->ops                = &rj54n1_ops;
+                kfree(rj54n1);
+                return err;
+        }
 
         rj54n1->clk_div         = clk_div;
         rj54n1->rect.left       = RJ54N1_COLUMN_SKIP;
@@ -1440,25 +1368,24 @@ static int rj54n1_probe(struct i2c_client *client,
         rj54n1->tgclk_mhz       = (rj54n1_priv->mclk_freq / PLL_L * PLL_N) /
                 (clk_div.ratio_tg + 1) / (clk_div.ratio_t + 1);
 
-        ret = rj54n1_video_probe(icd, client, rj54n1_priv);
+        ret = rj54n1_video_probe(client, rj54n1_priv);
         if (ret < 0) {
-                icd->ops = NULL;
+                v4l2_ctrl_handler_free(&rj54n1->hdl);
                 kfree(rj54n1);
                 return ret;
         }
-
-        return ret;
+        return v4l2_ctrl_handler_setup(&rj54n1->hdl);
 }
 
 static int rj54n1_remove(struct i2c_client *client)
 {
         struct rj54n1 *rj54n1 = to_rj54n1(client);
-        struct soc_camera_device *icd = client->dev.platform_data;
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
 
-        icd->ops = NULL;
+        v4l2_device_unregister_subdev(&rj54n1->subdev);
         if (icl->free_bus)
                 icl->free_bus(icl);
+        v4l2_ctrl_handler_free(&rj54n1->hdl);
         kfree(rj54n1);
 
         return 0;
diff --git a/drivers/media/video/s5k6aa.c b/drivers/media/video/s5k6aa.c
new file mode 100644
index 000000000000..2446736b7871
--- /dev/null
+++ b/drivers/media/video/s5k6aa.c
@@ -0,0 +1,1680 @@
+/*
+ * Driver for Samsung S5K6AAFX SXGA 1/6" 1.3M CMOS Image Sensor
+ * with embedded SoC ISP.
+ *
+ * Copyright (C) 2011, Samsung Electronics Co., Ltd.
+ * Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * Based on a driver authored by Dongsoo Nathaniel Kim.
+ * Copyright (C) 2009, Dongsoo Nathaniel Kim <dongsoo45.kim@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/gpio.h>
+#include <linux/i2c.h>
+#include <linux/media.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-mediabus.h>
+#include <media/s5k6aa.h>
+
+static int debug;
+module_param(debug, int, 0644);
+
+#define DRIVER_NAME                     "S5K6AA"
+
+/* The token to indicate array termination */
+#define S5K6AA_TERM                     0xffff
+#define S5K6AA_OUT_WIDTH_DEF            640
+#define S5K6AA_OUT_HEIGHT_DEF           480
+#define S5K6AA_WIN_WIDTH_MAX            1280
+#define S5K6AA_WIN_HEIGHT_MAX           1024
+#define S5K6AA_WIN_WIDTH_MIN            8
+#define S5K6AA_WIN_HEIGHT_MIN           8
+
+/*
+ * H/W register Interface (0xD0000000 - 0xD0000FFF)
+ */
+#define AHB_MSB_ADDR_PTR                0xfcfc
+#define GEN_REG_OFFSH                   0xd000
+#define REG_CMDWR_ADDRH                 0x0028
+#define REG_CMDWR_ADDRL                 0x002a
+#define REG_CMDRD_ADDRH                 0x002c
+#define REG_CMDRD_ADDRL                 0x002e
+#define REG_CMDBUF0_ADDR                0x0f12
+#define REG_CMDBUF1_ADDR                0x0f10
+
+/*
+ * Host S/W Register interface (0x70000000 - 0x70002000)
+ * The value of the two most significant address bytes is 0x7000,
+ * (HOST_SWIF_OFFS_H). The register addresses below specify 2 LSBs.
+ */
+#define HOST_SWIF_OFFSH                 0x7000
+
+/* Initialization parameters */
+/* Master clock frequency in KHz */
+#define REG_I_INCLK_FREQ_L              0x01b8
+#define REG_I_INCLK_FREQ_H              0x01ba
+#define  MIN_MCLK_FREQ_KHZ              6000U
+#define  MAX_MCLK_FREQ_KHZ              27000U
+#define REG_I_USE_NPVI_CLOCKS           0x01c6
+#define REG_I_USE_NMIPI_CLOCKS          0x01c8
+
+/* Clock configurations, n = 0..2. REG_I_* frequency unit is 4 kHz. */
+#define REG_I_OPCLK_4KHZ(n)             ((n) * 6 + 0x01cc)
+#define REG_I_MIN_OUTRATE_4KHZ(n)       ((n) * 6 + 0x01ce)
+#define REG_I_MAX_OUTRATE_4KHZ(n)       ((n) * 6 + 0x01d0)
+#define  SYS_PLL_OUT_FREQ               (48000000 / 4000)
+#define  PCLK_FREQ_MIN                  (24000000 / 4000)
+#define  PCLK_FREQ_MAX                  (48000000 / 4000)
+#define REG_I_INIT_PARAMS_UPDATED       0x01e0
+#define REG_I_ERROR_INFO                0x01e2
+
+/* General purpose parameters */
+#define REG_USER_BRIGHTNESS             0x01e4
+#define REG_USER_CONTRAST               0x01e6
+#define REG_USER_SATURATION             0x01e8
+#define REG_USER_SHARPBLUR              0x01ea
+
+#define REG_G_SPEC_EFFECTS              0x01ee
+#define REG_G_ENABLE_PREV               0x01f0
+#define REG_G_ENABLE_PREV_CHG           0x01f2
+#define REG_G_NEW_CFG_SYNC              0x01f8
+#define REG_G_PREVZOOM_IN_WIDTH         0x020a
+#define REG_G_PREVZOOM_IN_HEIGHT        0x020c
+#define REG_G_PREVZOOM_IN_XOFFS         0x020e
+#define REG_G_PREVZOOM_IN_YOFFS         0x0210
+#define REG_G_INPUTS_CHANGE_REQ         0x021a
+#define REG_G_ACTIVE_PREV_CFG           0x021c
+#define REG_G_PREV_CFG_CHG              0x021e
+#define REG_G_PREV_OPEN_AFTER_CH        0x0220
+#define REG_G_PREV_CFG_ERROR            0x0222
+
+/* Preview control section. n = 0...4. */
+#define PREG(n, x)                      ((n) * 0x26 + x)
+#define REG_P_OUT_WIDTH(n)              PREG(n, 0x0242)
+#define REG_P_OUT_HEIGHT(n)             PREG(n, 0x0244)
+#define REG_P_FMT(n)                    PREG(n, 0x0246)
+#define REG_P_MAX_OUT_RATE(n)           PREG(n, 0x0248)
+#define REG_P_MIN_OUT_RATE(n)           PREG(n, 0x024a)
+#define REG_P_PVI_MASK(n)               PREG(n, 0x024c)
+#define REG_P_CLK_INDEX(n)              PREG(n, 0x024e)
+#define REG_P_FR_RATE_TYPE(n)           PREG(n, 0x0250)
+#define  FR_RATE_DYNAMIC                0
+#define  FR_RATE_FIXED                  1
+#define  FR_RATE_FIXED_ACCURATE         2
+#define REG_P_FR_RATE_Q_TYPE(n)         PREG(n, 0x0252)
+#define  FR_RATE_Q_BEST_FRRATE          1 /* Binning enabled */
+#define  FR_RATE_Q_BEST_QUALITY         2 /* Binning disabled */
+/* Frame period in 0.1 ms units */
+#define REG_P_MAX_FR_TIME(n)            PREG(n, 0x0254)
+#define REG_P_MIN_FR_TIME(n)            PREG(n, 0x0256)
+/* Conversion to REG_P_[MAX/MIN]_FR_TIME value; __t: time in us */
+#define  US_TO_FR_TIME(__t)             ((__t) / 100)
+#define  S5K6AA_MIN_FR_TIME             33300  /* us */
+#define  S5K6AA_MAX_FR_TIME             650000 /* us */
+#define  S5K6AA_MAX_HIGHRES_FR_TIME     666    /* x100 us */
+/* The below 5 registers are for "device correction" values */
+#define REG_P_COLORTEMP(n)              PREG(n, 0x025e)
+#define REG_P_PREV_MIRROR(n)            PREG(n, 0x0262)
+
+/* Extended image property controls */
+/* Exposure time in 10 us units */
+#define REG_SF_USR_EXPOSURE_L           0x03c6
+#define REG_SF_USR_EXPOSURE_H           0x03c8
+#define REG_SF_USR_EXPOSURE_CHG         0x03ca
+#define REG_SF_USR_TOT_GAIN             0x03cc
+#define REG_SF_USR_TOT_GAIN_CHG         0x03ce
+#define REG_SF_RGAIN                    0x03d0
+#define REG_SF_RGAIN_CHG                0x03d2
+#define REG_SF_GGAIN                    0x03d4
+#define REG_SF_GGAIN_CHG                0x03d6
+#define REG_SF_BGAIN                    0x03d8
+#define REG_SF_BGAIN_CHG                0x03da
+#define REG_SF_FLICKER_QUANT            0x03dc
+#define REG_SF_FLICKER_QUANT_CHG        0x03de
+
+/* Output interface (parallel/MIPI) setup */
+#define REG_OIF_EN_MIPI_LANES           0x03fa
+#define REG_OIF_EN_PACKETS              0x03fc
+#define REG_OIF_CFG_CHG                 0x03fe
+
+/* Auto-algorithms enable mask */
+#define REG_DBG_AUTOALG_EN              0x0400
+#define  AALG_ALL_EN_MASK               (1 << 0)
+#define  AALG_AE_EN_MASK                (1 << 1)
+#define  AALG_DIVLEI_EN_MASK            (1 << 2)
+#define  AALG_WB_EN_MASK                (1 << 3)
+#define  AALG_FLICKER_EN_MASK           (1 << 5)
+#define  AALG_FIT_EN_MASK               (1 << 6)
+#define  AALG_WRHW_EN_MASK              (1 << 7)
+
+/* Firmware revision information */
+#define REG_FW_APIVER                   0x012e
+#define  S5K6AAFX_FW_APIVER             0x0001
+#define REG_FW_REVISION                 0x0130
+
+/* For now we use only one user configuration register set */
+#define S5K6AA_MAX_PRESETS              1
+
+static const char * const s5k6aa_supply_names[] = {
+        "vdd_core",     /* Digital core supply 1.5V (1.4V to 1.6V) */
+        "vdda",         /* Analog power supply 2.8V (2.6V to 3.0V) */
+        "vdd_reg",      /* Regulator input power 1.8V (1.7V to 1.9V)
+                           or 2.8V (2.6V to 3.0) */
+        "vddio",        /* I/O supply 1.8V (1.65V to 1.95V)
+                           or 2.8V (2.5V to 3.1V) */
+};
+#define S5K6AA_NUM_SUPPLIES ARRAY_SIZE(s5k6aa_supply_names)
+
+enum s5k6aa_gpio_id {
+        STBY,
+        RST,
+        GPIO_NUM,
+};
+
+struct s5k6aa_regval {
+        u16 addr;
+        u16 val;
+};
+
+struct s5k6aa_pixfmt {
+        enum v4l2_mbus_pixelcode code;
+        u32 colorspace;
+        /* REG_P_FMT(x) register value */
+        u16 reg_p_fmt;
+};
+
+struct s5k6aa_preset {
+        /* output pixel format and resolution */
+        struct v4l2_mbus_framefmt mbus_fmt;
+        u8 clk_id;
+        u8 index;
+};
+
+struct s5k6aa_ctrls {
+        struct v4l2_ctrl_handler handler;
+        /* Auto / manual white balance cluster */
+        struct v4l2_ctrl *awb;
+        struct v4l2_ctrl *gain_red;
+        struct v4l2_ctrl *gain_blue;
+        struct v4l2_ctrl *gain_green;
+        /* Mirror cluster */
+        struct v4l2_ctrl *hflip;
+        struct v4l2_ctrl *vflip;
+        /* Auto exposure / manual exposure and gain cluster */
+        struct v4l2_ctrl *auto_exp;
+        struct v4l2_ctrl *exposure;
+        struct v4l2_ctrl *gain;
+};
+
+struct s5k6aa_interval {
+        u16 reg_fr_time;
+        struct v4l2_fract interval;
+        /* Maximum rectangle for the interval */
+        struct v4l2_frmsize_discrete size;
+};
+
+struct s5k6aa {
+        struct v4l2_subdev sd;
+        struct media_pad pad;
+
+        enum v4l2_mbus_type bus_type;
+        u8 mipi_lanes;
+
+        int (*s_power)(int enable);
+        struct regulator_bulk_data supplies[S5K6AA_NUM_SUPPLIES];
+        struct s5k6aa_gpio gpio[GPIO_NUM];
+
+        /* external master clock frequency */
+        unsigned long mclk_frequency;
+        /* ISP internal master clock frequency */
+        u16 clk_fop;
+        /* output pixel clock frequency range */
+        u16 pclk_fmin;
+        u16 pclk_fmax;
+
+        unsigned int inv_hflip:1;
+        unsigned int inv_vflip:1;
+
+        /* protects the struct members below */
+        struct mutex lock;
+
+        /* sensor matrix scan window */
+        struct v4l2_rect ccd_rect;
+
+        struct s5k6aa_ctrls ctrls;
+        struct s5k6aa_preset presets[S5K6AA_MAX_PRESETS];
+        struct s5k6aa_preset *preset;
+        const struct s5k6aa_interval *fiv;
+
+        unsigned int streaming:1;
+        unsigned int apply_cfg:1;
+        unsigned int apply_crop:1;
+        unsigned int power;
+};
+
+static struct s5k6aa_regval s5k6aa_analog_config[] = {
+        /* Analog settings */
+        { 0x112a, 0x0000 }, { 0x1132, 0x0000 },
+        { 0x113e, 0x0000 }, { 0x115c, 0x0000 },
+        { 0x1164, 0x0000 }, { 0x1174, 0x0000 },
+        { 0x1178, 0x0000 }, { 0x077a, 0x0000 },
+        { 0x077c, 0x0000 }, { 0x077e, 0x0000 },
+        { 0x0780, 0x0000 }, { 0x0782, 0x0000 },
+        { 0x0784, 0x0000 }, { 0x0786, 0x0000 },
+        { 0x0788, 0x0000 }, { 0x07a2, 0x0000 },
+        { 0x07a4, 0x0000 }, { 0x07a6, 0x0000 },
+        { 0x07a8, 0x0000 }, { 0x07b6, 0x0000 },
+        { 0x07b8, 0x0002 }, { 0x07ba, 0x0004 },
+        { 0x07bc, 0x0004 }, { 0x07be, 0x0005 },
+        { 0x07c0, 0x0005 }, { S5K6AA_TERM, 0 },
+};
+
+/* TODO: Add RGB888 and Bayer format */
+static const struct s5k6aa_pixfmt s5k6aa_formats[] = {
+        { V4L2_MBUS_FMT_YUYV8_2X8,      V4L2_COLORSPACE_JPEG,   5 },
+        /* range 16-240 */
+        { V4L2_MBUS_FMT_YUYV8_2X8,      V4L2_COLORSPACE_REC709, 6 },
+        { V4L2_MBUS_FMT_RGB565_2X8_BE,  V4L2_COLORSPACE_JPEG,   0 },
+};
+
+static const struct s5k6aa_interval s5k6aa_intervals[] = {
+        { 1000, {10000, 1000000}, {1280, 1024} }, /* 10 fps */
+        { 666,  {15000, 1000000}, {1280, 1024} }, /* 15 fps */
+        { 500,  {20000, 1000000}, {1280, 720} },  /* 20 fps */
+        { 400,  {25000, 1000000}, {640, 480} },   /* 25 fps */
+        { 333,  {33300, 1000000}, {640, 480} },   /* 30 fps */
+};
+
+#define S5K6AA_INTERVAL_DEF_INDEX 1 /* 15 fps */
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+        return &container_of(ctrl->handler, struct s5k6aa, ctrls.handler)->sd;
+}
+
+static inline struct s5k6aa *to_s5k6aa(struct v4l2_subdev *sd)
+{
+        return container_of(sd, struct s5k6aa, sd);
+}
+
+/* Set initial values for all preview presets */
+static void s5k6aa_presets_data_init(struct s5k6aa *s5k6aa)
+{
+        struct s5k6aa_preset *preset = &s5k6aa->presets[0];
+        int i;
+
+        for (i = 0; i < S5K6AA_MAX_PRESETS; i++) {
+                preset->mbus_fmt.width  = S5K6AA_OUT_WIDTH_DEF;
+                preset->mbus_fmt.height = S5K6AA_OUT_HEIGHT_DEF;
+                preset->mbus_fmt.code   = s5k6aa_formats[0].code;
+                preset->index           = i;
+                preset->clk_id          = 0;
+                preset++;
+        }
+
+        s5k6aa->fiv = &s5k6aa_intervals[S5K6AA_INTERVAL_DEF_INDEX];
+        s5k6aa->preset = &s5k6aa->presets[0];
+}
+
+static int s5k6aa_i2c_read(struct i2c_client *client, u16 addr, u16 *val)
+{
+        u8 wbuf[2] = {addr >> 8, addr & 0xFF};
+        struct i2c_msg msg[2];
+        u8 rbuf[2];
+        int ret;
+
+        msg[0].addr = client->addr;
+        msg[0].flags = 0;
+        msg[0].len = 2;
+        msg[0].buf = wbuf;
+
+        msg[1].addr = client->addr;
+        msg[1].flags = I2C_M_RD;
+        msg[1].len = 2;
+        msg[1].buf = rbuf;
+
+        ret = i2c_transfer(client->adapter, msg, 2);
+        *val = be16_to_cpu(*((u16 *)rbuf));
+
+        v4l2_dbg(3, debug, client, "i2c_read: 0x%04X : 0x%04x\n", addr, *val);
+
+        return ret == 2 ? 0 : ret;
+}
+
+static int s5k6aa_i2c_write(struct i2c_client *client, u16 addr, u16 val)
+{
+        u8 buf[4] = {addr >> 8, addr & 0xFF, val >> 8, val & 0xFF};
+
+        int ret = i2c_master_send(client, buf, 4);
+        v4l2_dbg(3, debug, client, "i2c_write: 0x%04X : 0x%04x\n", addr, val);
+
+        return ret == 4 ? 0 : ret;
+}
+
+/* The command register write, assumes Command_Wr_addH = 0x7000. */
+static int s5k6aa_write(struct i2c_client *c, u16 addr, u16 val)
+{
+        int ret = s5k6aa_i2c_write(c, REG_CMDWR_ADDRL, addr);
+        if (ret)
+                return ret;
+        return s5k6aa_i2c_write(c, REG_CMDBUF0_ADDR, val);
+}
+
+/* The command register read, assumes Command_Rd_addH = 0x7000. */
+static int s5k6aa_read(struct i2c_client *client, u16 addr, u16 *val)
+{
+        int ret = s5k6aa_i2c_write(client, REG_CMDRD_ADDRL, addr);
+        if (ret)
+                return ret;
+        return s5k6aa_i2c_read(client, REG_CMDBUF0_ADDR, val);
+}
+
+static int s5k6aa_write_array(struct v4l2_subdev *sd,
+                              const struct s5k6aa_regval *msg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        u16 addr_incr = 0;
+        int ret = 0;
+
+        while (msg->addr != S5K6AA_TERM) {
+                if (addr_incr != 2)
+                        ret = s5k6aa_i2c_write(client, REG_CMDWR_ADDRL,
+                                               msg->addr);
+                if (ret)
+                        break;
+                ret = s5k6aa_i2c_write(client, REG_CMDBUF0_ADDR, msg->val);
+                if (ret)
+                        break;
+                /* Assume that msg->addr is always less than 0xfffc */
+                addr_incr = (msg + 1)->addr - msg->addr;
+                msg++;
+        }
+
+        return ret;
+}
+
+/* Configure the AHB high address bytes for GTG registers access */
+static int s5k6aa_set_ahb_address(struct i2c_client *client)
+{
+        int ret = s5k6aa_i2c_write(client, AHB_MSB_ADDR_PTR, GEN_REG_OFFSH);
+        if (ret)
+                return ret;
+        ret = s5k6aa_i2c_write(client, REG_CMDRD_ADDRH, HOST_SWIF_OFFSH);
+        if (ret)
+                return ret;
+        return s5k6aa_i2c_write(client, REG_CMDWR_ADDRH, HOST_SWIF_OFFSH);
+}
+
+/**
+ * s5k6aa_configure_pixel_clock - apply ISP main clock/PLL configuration
+ *
+ * Configure the internal ISP PLL for the required output frequency.
+ * Locking: called with s5k6aa.lock mutex held.
+ */
+static int s5k6aa_configure_pixel_clocks(struct s5k6aa *s5k6aa)
+{
+        struct i2c_client *c = v4l2_get_subdevdata(&s5k6aa->sd);
+        unsigned long fmclk = s5k6aa->mclk_frequency / 1000;
+        u16 status;
+        int ret;
+
+        if (WARN(fmclk < MIN_MCLK_FREQ_KHZ || fmclk > MAX_MCLK_FREQ_KHZ,
+                 "Invalid clock frequency: %ld\n", fmclk))
+                return -EINVAL;
+
+        s5k6aa->pclk_fmin = PCLK_FREQ_MIN;
+        s5k6aa->pclk_fmax = PCLK_FREQ_MAX;
+        s5k6aa->clk_fop = SYS_PLL_OUT_FREQ;
+
+        /* External input clock frequency in kHz */
+        ret = s5k6aa_write(c, REG_I_INCLK_FREQ_H, fmclk >> 16);
+        if (!ret)
+                ret = s5k6aa_write(c, REG_I_INCLK_FREQ_L, fmclk & 0xFFFF);
+        if (!ret)
+                ret = s5k6aa_write(c, REG_I_USE_NPVI_CLOCKS, 1);
+        /* Internal PLL frequency */
+        if (!ret)
+                ret = s5k6aa_write(c, REG_I_OPCLK_4KHZ(0), s5k6aa->clk_fop);
+        if (!ret)
+                ret = s5k6aa_write(c, REG_I_MIN_OUTRATE_4KHZ(0),
+                                   s5k6aa->pclk_fmin);
+        if (!ret)
+                ret = s5k6aa_write(c, REG_I_MAX_OUTRATE_4KHZ(0),
+                                   s5k6aa->pclk_fmax);
+        if (!ret)
+                ret = s5k6aa_write(c, REG_I_INIT_PARAMS_UPDATED, 1);
+        if (!ret)
+                ret = s5k6aa_read(c, REG_I_ERROR_INFO, &status);
+
+        return ret ? ret : (status ? -EINVAL : 0);
+}
+
+/* Set horizontal and vertical image flipping */
+static int s5k6aa_set_mirror(struct s5k6aa *s5k6aa, int horiz_flip)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
+        int index = s5k6aa->preset->index;
+
+        unsigned int vflip = s5k6aa->ctrls.vflip->val ^ s5k6aa->inv_vflip;
+        unsigned int flip = (horiz_flip ^ s5k6aa->inv_hflip) | (vflip << 1);
+
+        return s5k6aa_write(client, REG_P_PREV_MIRROR(index), flip);
+}
+
+/* Configure auto/manual white balance and R/G/B gains */
+static int s5k6aa_set_awb(struct s5k6aa *s5k6aa, int awb)
+{
+        struct i2c_client *c = v4l2_get_subdevdata(&s5k6aa->sd);
+        struct s5k6aa_ctrls *ctrls = &s5k6aa->ctrls;
+        u16 reg;
+
+        int ret = s5k6aa_read(c, REG_DBG_AUTOALG_EN, &reg);
+
+        if (!ret && !awb) {
+                ret = s5k6aa_write(c, REG_SF_RGAIN, ctrls->gain_red->val);
+                if (!ret)
+                        ret = s5k6aa_write(c, REG_SF_RGAIN_CHG, 1);
+                if (ret)
+                        return ret;
+
+                ret = s5k6aa_write(c, REG_SF_GGAIN, ctrls->gain_green->val);
+                if (!ret)
+                        ret = s5k6aa_write(c, REG_SF_GGAIN_CHG, 1);
+                if (ret)
+                        return ret;
+
+                ret = s5k6aa_write(c, REG_SF_BGAIN, ctrls->gain_blue->val);
+                if (!ret)
+                        ret = s5k6aa_write(c, REG_SF_BGAIN_CHG, 1);
+        }
+        if (!ret) {
+                reg = awb ? reg | AALG_WB_EN_MASK : reg & ~AALG_WB_EN_MASK;
+                ret = s5k6aa_write(c, REG_DBG_AUTOALG_EN, reg);
+        }
+
+        return ret;
+}
+
+/* Program FW with exposure time, 'exposure' in us units */
+static int s5k6aa_set_user_exposure(struct i2c_client *client, int exposure)
+{
+        unsigned int time = exposure / 10;
+
+        int ret = s5k6aa_write(client, REG_SF_USR_EXPOSURE_L, time & 0xffff);
+        if (!ret)
+                ret = s5k6aa_write(client, REG_SF_USR_EXPOSURE_H, time >> 16);
+        if (ret)
+                return ret;
+        return s5k6aa_write(client, REG_SF_USR_EXPOSURE_CHG, 1);
+}
+
+static int s5k6aa_set_user_gain(struct i2c_client *client, int gain)
+{
+        int ret = s5k6aa_write(client, REG_SF_USR_TOT_GAIN, gain);
+        if (ret)
+                return ret;
+        return s5k6aa_write(client, REG_SF_USR_TOT_GAIN_CHG, 1);
+}
+
+/* Set auto/manual exposure and total gain */
+static int s5k6aa_set_auto_exposure(struct s5k6aa *s5k6aa, int value)
+{
+        struct i2c_client *c = v4l2_get_subdevdata(&s5k6aa->sd);
+        unsigned int exp_time = s5k6aa->ctrls.exposure->val;
+        u16 auto_alg;
+
+        int ret = s5k6aa_read(c, REG_DBG_AUTOALG_EN, &auto_alg);
+        if (ret)
+                return ret;
+
+        v4l2_dbg(1, debug, c, "man_exp: %d, auto_exp: %d, a_alg: 0x%x\n",
+                 exp_time, value, auto_alg);
+
+        if (value == V4L2_EXPOSURE_AUTO) {
+                auto_alg |= AALG_AE_EN_MASK | AALG_DIVLEI_EN_MASK;
+        } else {
+                ret = s5k6aa_set_user_exposure(c, exp_time);
+                if (ret)
+                        return ret;
+                ret = s5k6aa_set_user_gain(c, s5k6aa->ctrls.gain->val);
+                if (ret)
+                        return ret;
+                auto_alg &= ~(AALG_AE_EN_MASK | AALG_DIVLEI_EN_MASK);
+        }
+
+        return s5k6aa_write(c, REG_DBG_AUTOALG_EN, auto_alg);
+}
+
+static int s5k6aa_set_anti_flicker(struct s5k6aa *s5k6aa, int value)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
+        u16 auto_alg;
+        int ret;
+
+        ret = s5k6aa_read(client, REG_DBG_AUTOALG_EN, &auto_alg);
+        if (ret)
+                return ret;
+
+        if (value == V4L2_CID_POWER_LINE_FREQUENCY_AUTO) {
+                auto_alg |= AALG_FLICKER_EN_MASK;
+        } else {
+                auto_alg &= ~AALG_FLICKER_EN_MASK;
+                /* The V4L2_CID_LINE_FREQUENCY control values match
+                 * the register values */
+                ret = s5k6aa_write(client, REG_SF_FLICKER_QUANT, value);
+                if (ret)
+                        return ret;
+                ret = s5k6aa_write(client, REG_SF_FLICKER_QUANT_CHG, 1);
+                if (ret)
+                        return ret;
+        }
+
+        return s5k6aa_write(client, REG_DBG_AUTOALG_EN, auto_alg);
+}
+
+static int s5k6aa_set_colorfx(struct s5k6aa *s5k6aa, int val)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
+        static const struct v4l2_control colorfx[] = {
+                { V4L2_COLORFX_NONE,     0 },
+                { V4L2_COLORFX_BW,       1 },
+                { V4L2_COLORFX_NEGATIVE, 2 },
+                { V4L2_COLORFX_SEPIA,    3 },
+                { V4L2_COLORFX_SKY_BLUE, 4 },
+                { V4L2_COLORFX_SKETCH,   5 },
+        };
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(colorfx); i++) {
+                if (colorfx[i].id == val)
+                        return s5k6aa_write(client, REG_G_SPEC_EFFECTS,
+                                            colorfx[i].value);
+        }
+        return -EINVAL;
+}
+
+static int s5k6aa_preview_config_status(struct i2c_client *client)
+{
+        u16 error = 0;
+        int ret = s5k6aa_read(client, REG_G_PREV_CFG_ERROR, &error);
+
+        v4l2_dbg(1, debug, client, "error: 0x%x (%d)\n", error, ret);
+        return ret ? ret : (error ? -EINVAL : 0);
+}
+
+static int s5k6aa_get_pixfmt_index(struct s5k6aa *s5k6aa,
+                                   struct v4l2_mbus_framefmt *mf)
+{
+        unsigned int i;
+
+        for (i = 0; i < ARRAY_SIZE(s5k6aa_formats); i++)
+                if (mf->colorspace == s5k6aa_formats[i].colorspace &&
+                    mf->code == s5k6aa_formats[i].code)
+                        return i;
+        return 0;
+}
+
+static int s5k6aa_set_output_framefmt(struct s5k6aa *s5k6aa,
+                                      struct s5k6aa_preset *preset)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
+        int fmt_index = s5k6aa_get_pixfmt_index(s5k6aa, &preset->mbus_fmt);
+        int ret;
+
+        ret = s5k6aa_write(client, REG_P_OUT_WIDTH(preset->index),
+                           preset->mbus_fmt.width);
+        if (!ret)
+                ret = s5k6aa_write(client, REG_P_OUT_HEIGHT(preset->index),
+                                   preset->mbus_fmt.height);
+        if (!ret)
+                ret = s5k6aa_write(client, REG_P_FMT(preset->index),
+                                   s5k6aa_formats[fmt_index].reg_p_fmt);
+        return ret;
+}
+
+static int s5k6aa_set_input_params(struct s5k6aa *s5k6aa)
+{
+        struct i2c_client *c = v4l2_get_subdevdata(&s5k6aa->sd);
+        struct v4l2_rect *r = &s5k6aa->ccd_rect;
+        int ret;
+
+        ret = s5k6aa_write(c, REG_G_PREVZOOM_IN_WIDTH, r->width);
+        if (!ret)
+                ret = s5k6aa_write(c, REG_G_PREVZOOM_IN_HEIGHT, r->height);
+        if (!ret)
+                ret = s5k6aa_write(c, REG_G_PREVZOOM_IN_XOFFS, r->left);
+        if (!ret)
+                ret = s5k6aa_write(c, REG_G_PREVZOOM_IN_YOFFS, r->top);
+        if (!ret)
+                ret = s5k6aa_write(c, REG_G_INPUTS_CHANGE_REQ, 1);
+        if (!ret)
+                s5k6aa->apply_crop = 0;
+
+        return ret;
+}
+
+/**
+ * s5k6aa_configure_video_bus - configure the video output interface
+ * @bus_type: video bus type: parallel or MIPI-CSI
+ * @nlanes: number of MIPI lanes to be used (MIPI-CSI only)
+ *
+ * Note: Only parallel bus operation has been tested.
+ */
+static int s5k6aa_configure_video_bus(struct s5k6aa *s5k6aa,
+                                      enum v4l2_mbus_type bus_type, int nlanes)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
+        u16 cfg = 0;
+        int ret;
+
+        /*
+         * TODO: The sensor is supposed to support BT.601 and BT.656
+         * but there is nothing indicating how to switch between both
+         * in the datasheet. For now default BT.601 interface is assumed.
+         */
+        if (bus_type == V4L2_MBUS_CSI2)
+                cfg = nlanes;
+        else if (bus_type != V4L2_MBUS_PARALLEL)
+                return -EINVAL;
+
+        ret = s5k6aa_write(client, REG_OIF_EN_MIPI_LANES, cfg);
+        if (ret)
+                return ret;
+        return s5k6aa_write(client, REG_OIF_CFG_CHG, 1);
+}
+
+/* This function should be called when switching to new user configuration set*/
+static int s5k6aa_new_config_sync(struct i2c_client *client, int timeout,
+                                  int cid)
+{
+        unsigned long end = jiffies + msecs_to_jiffies(timeout);
+        u16 reg = 1;
+        int ret;
+
+        ret = s5k6aa_write(client, REG_G_ACTIVE_PREV_CFG, cid);
+        if (!ret)
+                ret = s5k6aa_write(client, REG_G_PREV_CFG_CHG, 1);
+        if (!ret)
+                ret = s5k6aa_write(client, REG_G_NEW_CFG_SYNC, 1);
+        if (timeout == 0)
+                return ret;
+
+        while (ret >= 0 && time_is_after_jiffies(end)) {
+                ret = s5k6aa_read(client, REG_G_NEW_CFG_SYNC, &reg);
+                if (!reg)
+                        return 0;
+                usleep_range(1000, 5000);
+        }
+        return ret ? ret : -ETIMEDOUT;
+}
+
+/**
+ * s5k6aa_set_prev_config - write user preview register set
+ *
+ * Configure output resolution and color fromat, pixel clock
+ * frequency range, device frame rate type and frame period range.
+ */
+static int s5k6aa_set_prev_config(struct s5k6aa *s5k6aa,
+                                  struct s5k6aa_preset *preset)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
+        int idx = preset->index;
+        u16 frame_rate_q;
+        int ret;
+
+        if (s5k6aa->fiv->reg_fr_time >= S5K6AA_MAX_HIGHRES_FR_TIME)
+                frame_rate_q = FR_RATE_Q_BEST_FRRATE;
+        else
+                frame_rate_q = FR_RATE_Q_BEST_QUALITY;
+
+        ret = s5k6aa_set_output_framefmt(s5k6aa, preset);
+        if (!ret)
+                ret = s5k6aa_write(client, REG_P_MAX_OUT_RATE(idx),
+                                   s5k6aa->pclk_fmax);
+        if (!ret)
+                ret = s5k6aa_write(client, REG_P_MIN_OUT_RATE(idx),
+                                   s5k6aa->pclk_fmin);
+        if (!ret)
+                ret = s5k6aa_write(client, REG_P_CLK_INDEX(idx),
+                                   preset->clk_id);
+        if (!ret)
+                ret = s5k6aa_write(client, REG_P_FR_RATE_TYPE(idx),
+                                   FR_RATE_DYNAMIC);
+        if (!ret)
+                ret = s5k6aa_write(client, REG_P_FR_RATE_Q_TYPE(idx),
+                                   frame_rate_q);
+        if (!ret)
+                ret = s5k6aa_write(client, REG_P_MAX_FR_TIME(idx),
+                                   s5k6aa->fiv->reg_fr_time + 33);
+        if (!ret)
+                ret = s5k6aa_write(client, REG_P_MIN_FR_TIME(idx),
+                                   s5k6aa->fiv->reg_fr_time - 33);
+        if (!ret)
+                ret = s5k6aa_new_config_sync(client, 250, idx);
+        if (!ret)
+                ret = s5k6aa_preview_config_status(client);
+        if (!ret)
+                s5k6aa->apply_cfg = 0;
+
+        v4l2_dbg(1, debug, client, "Frame interval: %d +/- 3.3ms. (%d)\n",
+                 s5k6aa->fiv->reg_fr_time, ret);
+        return ret;
+}
+
+/**
+ * s5k6aa_initialize_isp - basic ISP MCU initialization
+ *
+ * Configure AHB addresses for registers read/write; configure PLLs for
+ * required output pixel clock. The ISP power supply needs to be already
+ * enabled, with an optional H/W reset.
+ * Locking: called with s5k6aa.lock mutex held.
+ */
+static int s5k6aa_initialize_isp(struct v4l2_subdev *sd)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+        int ret;
+
+        s5k6aa->apply_crop = 1;
+        s5k6aa->apply_cfg = 1;
+        msleep(100);
+
+        ret = s5k6aa_set_ahb_address(client);
+        if (ret)
+                return ret;
+        ret = s5k6aa_configure_video_bus(s5k6aa, s5k6aa->bus_type,
+                                         s5k6aa->mipi_lanes);
+        if (ret)
+                return ret;
+        ret = s5k6aa_write_array(sd, s5k6aa_analog_config);
+        if (ret)
+                return ret;
+        msleep(20);
+
+        return s5k6aa_configure_pixel_clocks(s5k6aa);
+}
+
+static int s5k6aa_gpio_set_value(struct s5k6aa *priv, int id, u32 val)
+{
+        if (!gpio_is_valid(priv->gpio[id].gpio))
+                return 0;
+        gpio_set_value(priv->gpio[id].gpio, !!val);
+        return 1;
+}
+
+static int s5k6aa_gpio_assert(struct s5k6aa *priv, int id)
+{
+        return s5k6aa_gpio_set_value(priv, id, priv->gpio[id].level);
+}
+
+static int s5k6aa_gpio_deassert(struct s5k6aa *priv, int id)
+{
+        return s5k6aa_gpio_set_value(priv, id, !priv->gpio[id].level);
+}
+
+static int __s5k6aa_power_on(struct s5k6aa *s5k6aa)
+{
+        int ret;
+
+        ret = regulator_bulk_enable(S5K6AA_NUM_SUPPLIES, s5k6aa->supplies);
+        if (ret)
+                return ret;
+        if (s5k6aa_gpio_deassert(s5k6aa, STBY))
+                usleep_range(150, 200);
+
+        if (s5k6aa->s_power)
+                ret = s5k6aa->s_power(1);
+        usleep_range(4000, 4000);
+
+        if (s5k6aa_gpio_deassert(s5k6aa, RST))
+                msleep(20);
+
+        return ret;
+}
+
+static int __s5k6aa_power_off(struct s5k6aa *s5k6aa)
+{
+        int ret;
+
+        if (s5k6aa_gpio_assert(s5k6aa, RST))
+                usleep_range(100, 150);
+
+        if (s5k6aa->s_power) {
+                ret = s5k6aa->s_power(0);
+                if (ret)
+                        return ret;
+        }
+        if (s5k6aa_gpio_assert(s5k6aa, STBY))
+                usleep_range(50, 100);
+        s5k6aa->streaming = 0;
+
+        return regulator_bulk_disable(S5K6AA_NUM_SUPPLIES, s5k6aa->supplies);
+}
+
+/*
+ * V4L2 subdev core and video operations
+ */
+static int s5k6aa_set_power(struct v4l2_subdev *sd, int on)
+{
+        struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+        int ret = 0;
+
+        mutex_lock(&s5k6aa->lock);
+
+        if (!on == s5k6aa->power) {
+                if (on) {
+                        ret = __s5k6aa_power_on(s5k6aa);
+                        if (!ret)
+                                ret = s5k6aa_initialize_isp(sd);
+                } else {
+                        ret = __s5k6aa_power_off(s5k6aa);
+                }
+
+                if (!ret)
+                        s5k6aa->power += on ? 1 : -1;
+        }
+
+        mutex_unlock(&s5k6aa->lock);
+
+        if (!on || ret || s5k6aa->power != 1)
+                return ret;
+
+        return v4l2_ctrl_handler_setup(sd->ctrl_handler);
+}
+
+static int __s5k6aa_stream(struct s5k6aa *s5k6aa, int enable)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
+        int ret = 0;
+
+        ret = s5k6aa_write(client, REG_G_ENABLE_PREV, enable);
+        if (!ret)
+                ret = s5k6aa_write(client, REG_G_ENABLE_PREV_CHG, 1);
+        if (!ret)
+                s5k6aa->streaming = enable;
+
+        return ret;
+}
+
+static int s5k6aa_s_stream(struct v4l2_subdev *sd, int on)
+{
+        struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+        int ret = 0;
+
+        mutex_lock(&s5k6aa->lock);
+
+        if (s5k6aa->streaming == !on) {
+                if (!ret && s5k6aa->apply_cfg)
+                        ret = s5k6aa_set_prev_config(s5k6aa, s5k6aa->preset);
+                if (s5k6aa->apply_crop)
+                        ret = s5k6aa_set_input_params(s5k6aa);
+                if (!ret)
+                        ret = __s5k6aa_stream(s5k6aa, !!on);
+        }
+        mutex_unlock(&s5k6aa->lock);
+
+        return ret;
+}
+
+static int s5k6aa_g_frame_interval(struct v4l2_subdev *sd,
+                                   struct v4l2_subdev_frame_interval *fi)
+{
+        struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+
+        mutex_lock(&s5k6aa->lock);
+        fi->interval = s5k6aa->fiv->interval;
+        mutex_unlock(&s5k6aa->lock);
+
+        return 0;
+}
+
+static int __s5k6aa_set_frame_interval(struct s5k6aa *s5k6aa,
+                                       struct v4l2_subdev_frame_interval *fi)
+{
+        struct v4l2_mbus_framefmt *mbus_fmt = &s5k6aa->preset->mbus_fmt;
+        const struct s5k6aa_interval *fiv = &s5k6aa_intervals[0];
+        unsigned int err, min_err = UINT_MAX;
+        unsigned int i, fr_time;
+
+        if (fi->interval.denominator == 0)
+                return -EINVAL;
+
+        fr_time = fi->interval.numerator * 10000 / fi->interval.denominator;
+
+        for (i = 0; i < ARRAY_SIZE(s5k6aa_intervals); i++) {
+                const struct s5k6aa_interval *iv = &s5k6aa_intervals[i];
+
+                if (mbus_fmt->width > iv->size.width ||
+                    mbus_fmt->height > iv->size.height)
+                        continue;
+
+                err = abs(iv->reg_fr_time - fr_time);
+                if (err < min_err) {
+                        fiv = iv;
+                        min_err = err;
+                }
+        }
+        s5k6aa->fiv = fiv;
+
+        v4l2_dbg(1, debug, &s5k6aa->sd, "Changed frame interval to %d us\n",
+                 fiv->reg_fr_time * 100);
+        return 0;
+}
+
+static int s5k6aa_s_frame_interval(struct v4l2_subdev *sd,
+                                   struct v4l2_subdev_frame_interval *fi)
+{
+        struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+        int ret;
+
+        v4l2_dbg(1, debug, sd, "Setting %d/%d frame interval\n",
+                 fi->interval.numerator, fi->interval.denominator);
+
+        mutex_lock(&s5k6aa->lock);
+        ret = __s5k6aa_set_frame_interval(s5k6aa, fi);
+        s5k6aa->apply_cfg = 1;
+
+        mutex_unlock(&s5k6aa->lock);
+        return ret;
+}
+
+/*
+ * V4L2 subdev pad level and video operations
+ */
+static int s5k6aa_enum_frame_interval(struct v4l2_subdev *sd,
+                              struct v4l2_subdev_fh *fh,
+                              struct v4l2_subdev_frame_interval_enum *fie)
+{
+        struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+        const struct s5k6aa_interval *fi;
+        int ret = 0;
+
+        if (fie->index > ARRAY_SIZE(s5k6aa_intervals))
+                return -EINVAL;
+
+        v4l_bound_align_image(&fie->width, S5K6AA_WIN_WIDTH_MIN,
+                              S5K6AA_WIN_WIDTH_MAX, 1,
+                              &fie->height, S5K6AA_WIN_HEIGHT_MIN,
+                              S5K6AA_WIN_HEIGHT_MAX, 1, 0);
+
+        mutex_lock(&s5k6aa->lock);
+        fi = &s5k6aa_intervals[fie->index];
+        if (fie->width > fi->size.width || fie->height > fi->size.height)
+                ret = -EINVAL;
+        else
+                fie->interval = fi->interval;
+        mutex_unlock(&s5k6aa->lock);
+
+        return ret;
+}
+
+static int s5k6aa_enum_mbus_code(struct v4l2_subdev *sd,
+                                 struct v4l2_subdev_fh *fh,
+                                 struct v4l2_subdev_mbus_code_enum *code)
+{
+        if (code->index >= ARRAY_SIZE(s5k6aa_formats))
+                return -EINVAL;
+
+        code->code = s5k6aa_formats[code->index].code;
+        return 0;
+}
+
+static int s5k6aa_enum_frame_size(struct v4l2_subdev *sd,
+                                  struct v4l2_subdev_fh *fh,
+                                  struct v4l2_subdev_frame_size_enum *fse)
+{
+        int i = ARRAY_SIZE(s5k6aa_formats);
+
+        if (fse->index > 0)
+                return -EINVAL;
+
+        while (--i)
+                if (fse->code == s5k6aa_formats[i].code)
+                        break;
+
+        fse->code = s5k6aa_formats[i].code;
+        fse->min_width  = S5K6AA_WIN_WIDTH_MIN;
+        fse->max_width  = S5K6AA_WIN_WIDTH_MAX;
+        fse->max_height = S5K6AA_WIN_HEIGHT_MIN;
+        fse->min_height = S5K6AA_WIN_HEIGHT_MAX;
+
+        return 0;
+}
+
+static struct v4l2_rect *
+__s5k6aa_get_crop_rect(struct s5k6aa *s5k6aa, struct v4l2_subdev_fh *fh,
+                       enum v4l2_subdev_format_whence which)
+{
+        if (which == V4L2_SUBDEV_FORMAT_ACTIVE)
+                return &s5k6aa->ccd_rect;
+        if (which == V4L2_SUBDEV_FORMAT_TRY)
+                return v4l2_subdev_get_try_crop(fh, 0);
+
+        return NULL;
+}
+
+static void s5k6aa_try_format(struct s5k6aa *s5k6aa,
+                              struct v4l2_mbus_framefmt *mf)
+{
+        unsigned int index;
+
+        v4l_bound_align_image(&mf->width, S5K6AA_WIN_WIDTH_MIN,
+                              S5K6AA_WIN_WIDTH_MAX, 1,
+                              &mf->height, S5K6AA_WIN_HEIGHT_MIN,
+                              S5K6AA_WIN_HEIGHT_MAX, 1, 0);
+
+        if (mf->colorspace != V4L2_COLORSPACE_JPEG &&
+            mf->colorspace != V4L2_COLORSPACE_REC709)
+                mf->colorspace = V4L2_COLORSPACE_JPEG;
+
+        index = s5k6aa_get_pixfmt_index(s5k6aa, mf);
+
+        mf->colorspace  = s5k6aa_formats[index].colorspace;
+        mf->code        = s5k6aa_formats[index].code;
+        mf->field       = V4L2_FIELD_NONE;
+}
+
+static int s5k6aa_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+                          struct v4l2_subdev_format *fmt)
+{
+        struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+        struct v4l2_mbus_framefmt *mf;
+
+        memset(fmt->reserved, 0, sizeof(fmt->reserved));
+
+        if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+                mf = v4l2_subdev_get_try_format(fh, 0);
+                fmt->format = *mf;
+                return 0;
+        }
+
+        mutex_lock(&s5k6aa->lock);
+        fmt->format = s5k6aa->preset->mbus_fmt;
+        mutex_unlock(&s5k6aa->lock);
+
+        return 0;
+}
+
+static int s5k6aa_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+                          struct v4l2_subdev_format *fmt)
+{
+        struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+        struct s5k6aa_preset *preset = s5k6aa->preset;
+        struct v4l2_mbus_framefmt *mf;
+        struct v4l2_rect *crop;
+        int ret = 0;
+
+        mutex_lock(&s5k6aa->lock);
+        s5k6aa_try_format(s5k6aa, &fmt->format);
+
+        if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+                mf = v4l2_subdev_get_try_format(fh, fmt->pad);
+                crop = v4l2_subdev_get_try_crop(fh, 0);
+        } else {
+                if (s5k6aa->streaming) {
+                        ret = -EBUSY;
+                } else {
+                        mf = &preset->mbus_fmt;
+                        crop = &s5k6aa->ccd_rect;
+                        s5k6aa->apply_cfg = 1;
+                }
+        }
+
+        if (ret == 0) {
+                struct v4l2_subdev_frame_interval fiv = {
+                        .interval = {0, 1}
+                };
+
+                *mf = fmt->format;
+                /*
+                 * Make sure the crop window is valid, i.e. its size is
+                 * greater than the output window, as the ISP supports
+                 * only down-scaling.
+                 */
+                crop->width = clamp_t(unsigned int, crop->width, mf->width,
+                                      S5K6AA_WIN_WIDTH_MAX);
+                crop->height = clamp_t(unsigned int, crop->height, mf->height,
+                                       S5K6AA_WIN_HEIGHT_MAX);
+                crop->left = clamp_t(unsigned int, crop->left, 0,
+                                     S5K6AA_WIN_WIDTH_MAX - crop->width);
+                crop->top  = clamp_t(unsigned int, crop->top, 0,
+                                     S5K6AA_WIN_HEIGHT_MAX - crop->height);
+
+                /* Reset to minimum possible frame interval */
+                ret = __s5k6aa_set_frame_interval(s5k6aa, &fiv);
+        }
+        mutex_unlock(&s5k6aa->lock);
+
+        return ret;
+}
+
+static int s5k6aa_get_crop(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+                           struct v4l2_subdev_crop *crop)
+{
+        struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+        struct v4l2_rect *rect;
+
+        memset(crop->reserved, 0, sizeof(crop->reserved));
+        mutex_lock(&s5k6aa->lock);
+
+        rect = __s5k6aa_get_crop_rect(s5k6aa, fh, crop->which);
+        if (rect)
+                crop->rect = *rect;
+
+        mutex_unlock(&s5k6aa->lock);
+
+        v4l2_dbg(1, debug, sd, "Current crop rectangle: (%d,%d)/%dx%d\n",
+                 rect->left, rect->top, rect->width, rect->height);
+
+        return 0;
+}
+
+static int s5k6aa_set_crop(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+                           struct v4l2_subdev_crop *crop)
+{
+        struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+        struct v4l2_mbus_framefmt *mf;
+        unsigned int max_x, max_y;
+        struct v4l2_rect *crop_r;
+
+        mutex_lock(&s5k6aa->lock);
+        crop_r = __s5k6aa_get_crop_rect(s5k6aa, fh, crop->which);
+
+        if (crop->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+                mf = &s5k6aa->preset->mbus_fmt;
+                s5k6aa->apply_crop = 1;
+        } else {
+                mf = v4l2_subdev_get_try_format(fh, 0);
+        }
+        v4l_bound_align_image(&crop->rect.width, mf->width,
+                              S5K6AA_WIN_WIDTH_MAX, 1,
+                              &crop->rect.height, mf->height,
+                              S5K6AA_WIN_HEIGHT_MAX, 1, 0);
+
+        max_x = (S5K6AA_WIN_WIDTH_MAX - crop->rect.width) & ~1;
+        max_y = (S5K6AA_WIN_HEIGHT_MAX - crop->rect.height) & ~1;
+
+        crop->rect.left = clamp_t(unsigned int, crop->rect.left, 0, max_x);
+        crop->rect.top  = clamp_t(unsigned int, crop->rect.top, 0, max_y);
+
+        *crop_r = crop->rect;
+
+        mutex_unlock(&s5k6aa->lock);
+
+        v4l2_dbg(1, debug, sd, "Set crop rectangle: (%d,%d)/%dx%d\n",
+                 crop_r->left, crop_r->top, crop_r->width, crop_r->height);
+
+        return 0;
+}
+
+static const struct v4l2_subdev_pad_ops s5k6aa_pad_ops = {
+        .enum_mbus_code         = s5k6aa_enum_mbus_code,
+        .enum_frame_size        = s5k6aa_enum_frame_size,
+        .enum_frame_interval    = s5k6aa_enum_frame_interval,
+        .get_fmt                = s5k6aa_get_fmt,
+        .set_fmt                = s5k6aa_set_fmt,
+        .get_crop               = s5k6aa_get_crop,
+        .set_crop               = s5k6aa_set_crop,
+};
+
+static const struct v4l2_subdev_video_ops s5k6aa_video_ops = {
+        .g_frame_interval       = s5k6aa_g_frame_interval,
+        .s_frame_interval       = s5k6aa_s_frame_interval,
+        .s_stream               = s5k6aa_s_stream,
+};
+
+/*
+ * V4L2 subdev controls
+ */
+
+static int s5k6aa_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+        int idx, err = 0;
+
+        v4l2_dbg(1, debug, sd, "ctrl: 0x%x, value: %d\n", ctrl->id, ctrl->val);
+
+        mutex_lock(&s5k6aa->lock);
+        /*
+         * If the device is not powered up by the host driver do
+         * not apply any controls to H/W at this time. Instead
+         * the controls will be restored right after power-up.
+         */
+        if (s5k6aa->power == 0)
+                goto unlock;
+        idx = s5k6aa->preset->index;
+
+        switch (ctrl->id) {
+        case V4L2_CID_AUTO_WHITE_BALANCE:
+                err = s5k6aa_set_awb(s5k6aa, ctrl->val);
+                break;
+
+        case V4L2_CID_BRIGHTNESS:
+                err = s5k6aa_write(client, REG_USER_BRIGHTNESS, ctrl->val);
+                break;
+
+        case V4L2_CID_COLORFX:
+                err = s5k6aa_set_colorfx(s5k6aa, ctrl->val);
+                break;
+
+        case V4L2_CID_CONTRAST:
+                err = s5k6aa_write(client, REG_USER_CONTRAST, ctrl->val);
+                break;
+
+        case V4L2_CID_EXPOSURE_AUTO:
+                err = s5k6aa_set_auto_exposure(s5k6aa, ctrl->val);
+                break;
+
+        case V4L2_CID_HFLIP:
+                err = s5k6aa_set_mirror(s5k6aa, ctrl->val);
+                if (err)
+                        break;
+                err = s5k6aa_write(client, REG_G_PREV_CFG_CHG, 1);
+                break;
+
+        case V4L2_CID_POWER_LINE_FREQUENCY:
+                err = s5k6aa_set_anti_flicker(s5k6aa, ctrl->val);
+                break;
+
+        case V4L2_CID_SATURATION:
+                err = s5k6aa_write(client, REG_USER_SATURATION, ctrl->val);
+                break;
+
+        case V4L2_CID_SHARPNESS:
+                err = s5k6aa_write(client, REG_USER_SHARPBLUR, ctrl->val);
+                break;
+
+        case V4L2_CID_WHITE_BALANCE_TEMPERATURE:
+                err = s5k6aa_write(client, REG_P_COLORTEMP(idx), ctrl->val);
+                if (err)
+                        break;
+                err = s5k6aa_write(client, REG_G_PREV_CFG_CHG, 1);
+                break;
+        }
+unlock:
+        mutex_unlock(&s5k6aa->lock);
+        return err;
+}
+
+static const struct v4l2_ctrl_ops s5k6aa_ctrl_ops = {
+        .s_ctrl = s5k6aa_s_ctrl,
+};
+
+static int s5k6aa_log_status(struct v4l2_subdev *sd)
+{
+        v4l2_ctrl_handler_log_status(sd->ctrl_handler, sd->name);
+        return 0;
+}
+
+#define V4L2_CID_RED_GAIN       (V4L2_CTRL_CLASS_CAMERA | 0x1001)
+#define V4L2_CID_GREEN_GAIN     (V4L2_CTRL_CLASS_CAMERA | 0x1002)
+#define V4L2_CID_BLUE_GAIN      (V4L2_CTRL_CLASS_CAMERA | 0x1003)
+
+static const struct v4l2_ctrl_config s5k6aa_ctrls[] = {
+        {
+                .ops    = &s5k6aa_ctrl_ops,
+                .id     = V4L2_CID_RED_GAIN,
+                .type   = V4L2_CTRL_TYPE_INTEGER,
+                .name   = "Gain, Red",
+                .min    = 0,
+                .max    = 256,
+                .def    = 127,
+                .step   = 1,
+        }, {
+                .ops    = &s5k6aa_ctrl_ops,
+                .id     = V4L2_CID_GREEN_GAIN,
+                .type   = V4L2_CTRL_TYPE_INTEGER,
+                .name   = "Gain, Green",
+                .min    = 0,
+                .max    = 256,
+                .def    = 127,
+                .step   = 1,
+        }, {
+                .ops    = &s5k6aa_ctrl_ops,
+                .id     = V4L2_CID_BLUE_GAIN,
+                .type   = V4L2_CTRL_TYPE_INTEGER,
+                .name   = "Gain, Blue",
+                .min    = 0,
+                .max    = 256,
+                .def    = 127,
+                .step   = 1,
+        },
+};
+
+static int s5k6aa_initialize_ctrls(struct s5k6aa *s5k6aa)
+{
+        const struct v4l2_ctrl_ops *ops = &s5k6aa_ctrl_ops;
+        struct s5k6aa_ctrls *ctrls = &s5k6aa->ctrls;
+        struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+
+        int ret = v4l2_ctrl_handler_init(hdl, 16);
+        if (ret)
+                return ret;
+        /* Auto white balance cluster */
+        ctrls->awb = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTO_WHITE_BALANCE,
+                                       0, 1, 1, 1);
+        ctrls->gain_red = v4l2_ctrl_new_custom(hdl, &s5k6aa_ctrls[0], NULL);
+        ctrls->gain_green = v4l2_ctrl_new_custom(hdl, &s5k6aa_ctrls[1], NULL);
+        ctrls->gain_blue = v4l2_ctrl_new_custom(hdl, &s5k6aa_ctrls[2], NULL);
+        v4l2_ctrl_auto_cluster(4, &ctrls->awb, 0, false);
+
+        ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+        ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+        v4l2_ctrl_cluster(2, &ctrls->hflip);
+
+        ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
+                                V4L2_CID_EXPOSURE_AUTO,
+                                V4L2_EXPOSURE_MANUAL, 0, V4L2_EXPOSURE_AUTO);
+        /* Exposure time: x 1 us */
+        ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
+                                            0, 6000000U, 1, 100000U);
+        /* Total gain: 256 <=> 1x */
+        ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
+                                        0, 256, 1, 256);
+        v4l2_ctrl_auto_cluster(3, &ctrls->auto_exp, 0, false);
+
+        v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_POWER_LINE_FREQUENCY,
+                               V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
+                               V4L2_CID_POWER_LINE_FREQUENCY_AUTO);
+
+        v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_COLORFX,
+                               V4L2_COLORFX_SKY_BLUE, ~0x6f, V4L2_COLORFX_NONE);
+
+        v4l2_ctrl_new_std(hdl, ops, V4L2_CID_WHITE_BALANCE_TEMPERATURE,
+                          0, 256, 1, 0);
+
+        v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION, -127, 127, 1, 0);
+        v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BRIGHTNESS, -127, 127, 1, 0);
+        v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST, -127, 127, 1, 0);
+        v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SHARPNESS, -127, 127, 1, 0);
+
+        if (hdl->error) {
+                ret = hdl->error;
+                v4l2_ctrl_handler_free(hdl);
+                return ret;
+        }
+
+        s5k6aa->sd.ctrl_handler = hdl;
+        return 0;
+}
+
+/*
+ * V4L2 subdev internal operations
+ */
+static int s5k6aa_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+        struct v4l2_mbus_framefmt *format = v4l2_subdev_get_try_format(fh, 0);
+        struct v4l2_rect *crop = v4l2_subdev_get_try_crop(fh, 0);
+
+        format->colorspace = s5k6aa_formats[0].colorspace;
+        format->code = s5k6aa_formats[0].code;
+        format->width = S5K6AA_OUT_WIDTH_DEF;
+        format->height = S5K6AA_OUT_HEIGHT_DEF;
+        format->field = V4L2_FIELD_NONE;
+
+        crop->width = S5K6AA_WIN_WIDTH_MAX;
+        crop->height = S5K6AA_WIN_HEIGHT_MAX;
+        crop->left = 0;
+        crop->top = 0;
+
+        return 0;
+}
+
+int s5k6aa_check_fw_revision(struct s5k6aa *s5k6aa)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
+        u16 api_ver = 0, fw_rev = 0;
+
+        int ret = s5k6aa_set_ahb_address(client);
+
+        if (!ret)
+                ret = s5k6aa_read(client, REG_FW_APIVER, &api_ver);
+        if (!ret)
+                ret = s5k6aa_read(client, REG_FW_REVISION, &fw_rev);
+        if (ret) {
+                v4l2_err(&s5k6aa->sd, "FW revision check failed!\n");
+                return ret;
+        }
+
+        v4l2_info(&s5k6aa->sd, "FW API ver.: 0x%X, FW rev.: 0x%X\n",
+                  api_ver, fw_rev);
+
+        return api_ver == S5K6AAFX_FW_APIVER ? 0 : -ENODEV;
+}
+
+static int s5k6aa_registered(struct v4l2_subdev *sd)
+{
+        struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+        int ret;
+
+        mutex_lock(&s5k6aa->lock);
+        ret = __s5k6aa_power_on(s5k6aa);
+        if (!ret) {
+                msleep(100);
+                ret = s5k6aa_check_fw_revision(s5k6aa);
+                __s5k6aa_power_off(s5k6aa);
+        }
+        mutex_unlock(&s5k6aa->lock);
+
+        return ret;
+}
+
+static const struct v4l2_subdev_internal_ops s5k6aa_subdev_internal_ops = {
+        .registered = s5k6aa_registered,
+        .open = s5k6aa_open,
+};
+
+static const struct v4l2_subdev_core_ops s5k6aa_core_ops = {
+        .s_power = s5k6aa_set_power,
+        .log_status = s5k6aa_log_status,
+};
+
+static const struct v4l2_subdev_ops s5k6aa_subdev_ops = {
+        .core = &s5k6aa_core_ops,
+        .pad = &s5k6aa_pad_ops,
+        .video = &s5k6aa_video_ops,
+};
+
+/*
+ * GPIO setup
+ */
+static int s5k6aa_configure_gpio(int nr, int val, const char *name)
+{
+        unsigned long flags = val ? GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW;
+        int ret;
+
+        if (!gpio_is_valid(nr))
+                return 0;
+        ret = gpio_request_one(nr, flags, name);
+        if (!ret)
+                gpio_export(nr, 0);
+        return ret;
+}
+
+static void s5k6aa_free_gpios(struct s5k6aa *s5k6aa)
+{
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(s5k6aa->gpio); i++) {
+                if (!gpio_is_valid(s5k6aa->gpio[i].gpio))
+                        continue;
+                gpio_free(s5k6aa->gpio[i].gpio);
+                s5k6aa->gpio[i].gpio = -EINVAL;
+        }
+}
+
+static int s5k6aa_configure_gpios(struct s5k6aa *s5k6aa,
+                                  const struct s5k6aa_platform_data *pdata)
+{
+        const struct s5k6aa_gpio *gpio = &pdata->gpio_stby;
+        int ret;
+
+        s5k6aa->gpio[STBY].gpio = -EINVAL;
+        s5k6aa->gpio[RST].gpio  = -EINVAL;
+
+        ret = s5k6aa_configure_gpio(gpio->gpio, gpio->level, "S5K6AA_STBY");
+        if (ret) {
+                s5k6aa_free_gpios(s5k6aa);
+                return ret;
+        }
+        s5k6aa->gpio[STBY] = *gpio;
+        if (gpio_is_valid(gpio->gpio))
+                gpio_set_value(gpio->gpio, 0);
+
+        gpio = &pdata->gpio_reset;
+        ret = s5k6aa_configure_gpio(gpio->gpio, gpio->level, "S5K6AA_RST");
+        if (ret) {
+                s5k6aa_free_gpios(s5k6aa);
+                return ret;
+        }
+        s5k6aa->gpio[RST] = *gpio;
+        if (gpio_is_valid(gpio->gpio))
+                gpio_set_value(gpio->gpio, 0);
+
+        return 0;
+}
+
+static int s5k6aa_probe(struct i2c_client *client,
+                        const struct i2c_device_id *id)
+{
+        const struct s5k6aa_platform_data *pdata = client->dev.platform_data;
+        struct v4l2_subdev *sd;
+        struct s5k6aa *s5k6aa;
+        int i, ret;
+
+        if (pdata == NULL) {
+                dev_err(&client->dev, "Platform data not specified\n");
+                return -EINVAL;
+        }
+
+        if (pdata->mclk_frequency == 0) {
+                dev_err(&client->dev, "MCLK frequency not specified\n");
+                return -EINVAL;
+        }
+
+        s5k6aa = kzalloc(sizeof(*s5k6aa), GFP_KERNEL);
+        if (!s5k6aa)
+                return -ENOMEM;
+
+        mutex_init(&s5k6aa->lock);
+
+        s5k6aa->mclk_frequency = pdata->mclk_frequency;
+        s5k6aa->bus_type = pdata->bus_type;
+        s5k6aa->mipi_lanes = pdata->nlanes;
+        s5k6aa->s_power = pdata->set_power;
+        s5k6aa->inv_hflip = pdata->horiz_flip;
+        s5k6aa->inv_vflip = pdata->vert_flip;
+
+        sd = &s5k6aa->sd;
+        strlcpy(sd->name, DRIVER_NAME, sizeof(sd->name));
+        v4l2_i2c_subdev_init(sd, client, &s5k6aa_subdev_ops);
+
+        sd->internal_ops = &s5k6aa_subdev_internal_ops;
+        sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+        s5k6aa->pad.flags = MEDIA_PAD_FL_SOURCE;
+        sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+        ret = media_entity_init(&sd->entity, 1, &s5k6aa->pad, 0);
+        if (ret)
+                goto out_err1;
+
+        ret = s5k6aa_configure_gpios(s5k6aa, pdata);
+        if (ret)
+                goto out_err2;
+
+        for (i = 0; i < S5K6AA_NUM_SUPPLIES; i++)
+                s5k6aa->supplies[i].supply = s5k6aa_supply_names[i];
+
+        ret = regulator_bulk_get(&client->dev, S5K6AA_NUM_SUPPLIES,
+                                 s5k6aa->supplies);
+        if (ret) {
+                dev_err(&client->dev, "Failed to get regulators\n");
+                goto out_err3;
+        }
+
+        ret = s5k6aa_initialize_ctrls(s5k6aa);
+        if (ret)
+                goto out_err4;
+
+        s5k6aa_presets_data_init(s5k6aa);
+
+        s5k6aa->ccd_rect.width = S5K6AA_WIN_WIDTH_MAX;
+        s5k6aa->ccd_rect.height = S5K6AA_WIN_HEIGHT_MAX;
+        s5k6aa->ccd_rect.left = 0;
+        s5k6aa->ccd_rect.top = 0;
+
+        return 0;
+
+out_err4:
+        regulator_bulk_free(S5K6AA_NUM_SUPPLIES, s5k6aa->supplies);
+out_err3:
+        s5k6aa_free_gpios(s5k6aa);
+out_err2:
+        media_entity_cleanup(&s5k6aa->sd.entity);
+out_err1:
+        kfree(s5k6aa);
+        return ret;
+}
+
+static int s5k6aa_remove(struct i2c_client *client)
+{
+        struct v4l2_subdev *sd = i2c_get_clientdata(client);
+        struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+
+        v4l2_device_unregister_subdev(sd);
+        v4l2_ctrl_handler_free(sd->ctrl_handler);
+        media_entity_cleanup(&sd->entity);
+        regulator_bulk_free(S5K6AA_NUM_SUPPLIES, s5k6aa->supplies);
+        s5k6aa_free_gpios(s5k6aa);
+        kfree(s5k6aa);
+
+        return 0;
+}
+
+static const struct i2c_device_id s5k6aa_id[] = {
+        { DRIVER_NAME, 0 },
+        { },
+};
+MODULE_DEVICE_TABLE(i2c, s5k6aa_id);
+
+
+static struct i2c_driver s5k6aa_i2c_driver = {
+        .driver = {
+                .name = DRIVER_NAME
+        },
+        .probe          = s5k6aa_probe,
+        .remove         = s5k6aa_remove,
+        .id_table       = s5k6aa_id,
+};
+
+static int __init s5k6aa_init(void)
+{
+        return i2c_add_driver(&s5k6aa_i2c_driver);
+}
+
+static void __exit s5k6aa_exit(void)
+{
+        i2c_del_driver(&s5k6aa_i2c_driver);
+}
+
+module_init(s5k6aa_init);
+module_exit(s5k6aa_exit);
+
+MODULE_DESCRIPTION("Samsung S5K6AA(FX) SXGA camera driver");
+MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/s5p-fimc/fimc-capture.c b/drivers/media/video/s5p-fimc/fimc-capture.c
index 931f469604b0..c8d91b0cd9bd 100644
--- a/drivers/media/video/s5p-fimc/fimc-capture.c
+++ b/drivers/media/video/s5p-fimc/fimc-capture.c
@@ -246,9 +246,9 @@ static unsigned int get_plane_size(struct fimc_frame *fr, unsigned int plane)
         return fr->f_width * fr->f_height * fr->fmt->depth[plane] / 8;
 }
 
-static int queue_setup(struct vb2_queue *vq, unsigned int *num_buffers,
-                       unsigned int *num_planes, unsigned int sizes[],
-                       void *allocators[])
+static int queue_setup(struct vb2_queue *vq,  const struct v4l2_format *pfmt,
+                       unsigned int *num_buffers, unsigned int *num_planes,
+                       unsigned int sizes[], void *allocators[])
 {
         struct fimc_ctx *ctx = vq->drv_priv;
         struct fimc_fmt *fmt = ctx->d_frame.fmt;
diff --git a/drivers/media/video/s5p-fimc/fimc-core.c b/drivers/media/video/s5p-fimc/fimc-core.c
index 6c1c9cb55378..19ca6db38b2f 100644
--- a/drivers/media/video/s5p-fimc/fimc-core.c
+++ b/drivers/media/video/s5p-fimc/fimc-core.c
@@ -670,9 +670,9 @@ static void fimc_job_abort(void *priv)
         fimc_m2m_shutdown(priv);
 }
 
-static int fimc_queue_setup(struct vb2_queue *vq, unsigned int *num_buffers,
-                            unsigned int *num_planes, unsigned int sizes[],
-                            void *allocators[])
+static int fimc_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
+                            unsigned int *num_buffers, unsigned int *num_planes,
+                            unsigned int sizes[], void *allocators[])
 {
         struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
         struct fimc_frame *f;
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.c b/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.c
index 5f4da80051bb..f2481a85e0a2 100644
--- a/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.c
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.c
@@ -38,7 +38,7 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev)
          * into kernel. */
         mfc_debug_enter();
         err = request_firmware((const struct firmware **)&fw_blob,
-                                     "s5pc110-mfc.fw", dev->v4l2_dev.dev);
+                                     "s5p-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;
@@ -116,7 +116,7 @@ int s5p_mfc_reload_firmware(struct s5p_mfc_dev *dev)
          * into kernel. */
         mfc_debug_enter();
         err = request_firmware((const struct firmware **)&fw_blob,
-                                     "s5pc110-mfc.fw", dev->v4l2_dev.dev);
+                                     "s5p-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;
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_dec.c b/drivers/media/video/s5p-mfc/s5p_mfc_dec.c
index bfbe08432050..725634d9736d 100644
--- a/drivers/media/video/s5p-mfc/s5p_mfc_dec.c
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_dec.c
@@ -744,9 +744,10 @@ static const struct v4l2_ioctl_ops s5p_mfc_dec_ioctl_ops = {
         .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 int psize[],
-                               void *allocators[])
+static int s5p_mfc_queue_setup(struct vb2_queue *vq,
+                        const struct v4l2_format *fmt, unsigned int *buf_count,
+                        unsigned int *plane_count, unsigned int psize[],
+                        void *allocators[])
 {
         struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
 
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_enc.c b/drivers/media/video/s5p-mfc/s5p_mfc_enc.c
index 4c90e53bd964..ecef127dbc66 100644
--- a/drivers/media/video/s5p-mfc/s5p_mfc_enc.c
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_enc.c
@@ -1513,8 +1513,9 @@ static int check_vb_with_fmt(struct s5p_mfc_fmt *fmt, struct vb2_buffer *vb)
 }
 
 static int s5p_mfc_queue_setup(struct vb2_queue *vq,
-                       unsigned int *buf_count, unsigned int *plane_count,
-                       unsigned int psize[], void *allocators[])
+                        const struct v4l2_format *fmt,
+                        unsigned int *buf_count, unsigned int *plane_count,
+                        unsigned int psize[], void *allocators[])
 {
         struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
 
diff --git a/drivers/media/video/s5p-tv/mixer_video.c b/drivers/media/video/s5p-tv/mixer_video.c
index 4917e2c2b321..e16d3a4bc1dc 100644
--- a/drivers/media/video/s5p-tv/mixer_video.c
+++ b/drivers/media/video/s5p-tv/mixer_video.c
@@ -727,8 +727,8 @@ static const struct v4l2_file_operations mxr_fops = {
         .unlocked_ioctl = video_ioctl2,
 };
 
-static int queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
-        unsigned int *nplanes, unsigned int sizes[],
+static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *pfmt,
+        unsigned int *nbuffers, unsigned int *nplanes, unsigned int sizes[],
         void *alloc_ctxs[])
 {
         struct mxr_layer *layer = vb2_get_drv_priv(vq);
diff --git a/drivers/media/video/saa7134/saa7134.h b/drivers/media/video/saa7134/saa7134.h
index bc8d6bba8ee5..9b550687213a 100644
--- a/drivers/media/video/saa7134/saa7134.h
+++ b/drivers/media/video/saa7134/saa7134.h
@@ -843,10 +843,10 @@ void saa7134_probe_i2c_ir(struct saa7134_dev *dev);
 int saa7134_ir_start(struct saa7134_dev *dev);
 void saa7134_ir_stop(struct saa7134_dev *dev);
 #else
-#define saa7134_input_init1(dev)        (0)
-#define saa7134_input_fini(dev)         (0)
-#define saa7134_input_irq(dev)          (0)
-#define saa7134_probe_i2c_ir(dev)       (0)
-#define saa7134_ir_start(dev)           (0)
-#define saa7134_ir_stop(dev)            (0)
+#define saa7134_input_init1(dev)        ((void)0)
+#define saa7134_input_fini(dev)         ((void)0)
+#define saa7134_input_irq(dev)          ((void)0)
+#define saa7134_probe_i2c_ir(dev)       ((void)0)
+#define saa7134_ir_start(dev)           ((void)0)
+#define saa7134_ir_stop(dev)            ((void)0)
 #endif
diff --git a/drivers/media/video/sh_mobile_ceu_camera.c b/drivers/media/video/sh_mobile_ceu_camera.c
index 8615fb81775f..f390682629cf 100644
--- a/drivers/media/video/sh_mobile_ceu_camera.c
+++ b/drivers/media/video/sh_mobile_ceu_camera.c
@@ -90,7 +90,6 @@
 struct sh_mobile_ceu_buffer {
         struct vb2_buffer vb; /* v4l buffer must be first */
         struct list_head queue;
-        enum v4l2_mbus_pixelcode code;
 };
 
 struct sh_mobile_ceu_dev {
@@ -100,7 +99,8 @@ struct sh_mobile_ceu_dev {
 
         unsigned int irq;
         void __iomem *base;
-        unsigned long video_limit;
+        size_t video_limit;
+        size_t buf_total;
 
         spinlock_t lock;                /* Protects video buffer lists */
         struct list_head capture;
@@ -121,7 +121,7 @@ struct sh_mobile_ceu_dev {
 };
 
 struct sh_mobile_ceu_cam {
-        /* CEU offsets within scaled by the CEU camera output */
+        /* CEU offsets within the camera output, before the CEU scaler */
         unsigned int ceu_left;
         unsigned int ceu_top;
         /* Client output, as seen by the CEU */
@@ -144,30 +144,6 @@ static struct sh_mobile_ceu_buffer *to_ceu_vb(struct vb2_buffer *vb)
         return container_of(vb, struct sh_mobile_ceu_buffer, vb);
 }
 
-static unsigned long make_bus_param(struct sh_mobile_ceu_dev *pcdev)
-{
-        unsigned long flags;
-
-        flags = SOCAM_MASTER |
-                SOCAM_PCLK_SAMPLE_RISING |
-                SOCAM_HSYNC_ACTIVE_HIGH |
-                SOCAM_HSYNC_ACTIVE_LOW |
-                SOCAM_VSYNC_ACTIVE_HIGH |
-                SOCAM_VSYNC_ACTIVE_LOW |
-                SOCAM_DATA_ACTIVE_HIGH;
-
-        if (pcdev->pdata->flags & SH_CEU_FLAG_USE_8BIT_BUS)
-                flags |= SOCAM_DATAWIDTH_8;
-
-        if (pcdev->pdata->flags & SH_CEU_FLAG_USE_16BIT_BUS)
-                flags |= SOCAM_DATAWIDTH_16;
-
-        if (flags & SOCAM_DATAWIDTH_MASK)
-                return flags;
-
-        return 0;
-}
-
 static void ceu_write(struct sh_mobile_ceu_dev *priv,
                       unsigned long reg_offs, u32 data)
 {
@@ -216,33 +192,61 @@ static int sh_mobile_ceu_soft_reset(struct sh_mobile_ceu_dev *pcdev)
 /*
  *  Videobuf operations
  */
+
+/*
+ * .queue_setup() is called to check, whether the driver can accept the
+ *                requested number of buffers and to fill in plane sizes
+ *                for the current frame format if required
+ */
 static int sh_mobile_ceu_videobuf_setup(struct vb2_queue *vq,
+                        const struct v4l2_format *fmt,
                         unsigned int *count, unsigned int *num_planes,
                         unsigned int 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->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);
+        int bytes_per_line;
+        unsigned int height;
 
+        if (fmt) {
+                const struct soc_camera_format_xlate *xlate = soc_camera_xlate_by_fourcc(icd,
+                                                                fmt->fmt.pix.pixelformat);
+                if (!xlate)
+                        return -EINVAL;
+                bytes_per_line = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
+                                                         xlate->host_fmt);
+                height = fmt->fmt.pix.height;
+        } else {
+                /* Called from VIDIOC_REQBUFS or in compatibility mode */
+                bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+                                                icd->current_fmt->host_fmt);
+                height = icd->user_height;
+        }
         if (bytes_per_line < 0)
                 return bytes_per_line;
 
-        *num_planes = 1;
+        sizes[0] = bytes_per_line * height;
 
-        pcdev->sequence = 0;
-        sizes[0] = bytes_per_line * icd->user_height;
         alloc_ctxs[0] = pcdev->alloc_ctx;
 
+        if (!vq->num_buffers)
+                pcdev->sequence = 0;
+
         if (!*count)
                 *count = 2;
 
-        if (pcdev->video_limit) {
-                if (PAGE_ALIGN(sizes[0]) * *count > pcdev->video_limit)
-                        *count = pcdev->video_limit / PAGE_ALIGN(sizes[0]);
+        /* If *num_planes != 0, we have already verified *count. */
+        if (pcdev->video_limit && !*num_planes) {
+                size_t size = PAGE_ALIGN(sizes[0]) * *count;
+
+                if (size + pcdev->buf_total > pcdev->video_limit)
+                        *count = (pcdev->video_limit - pcdev->buf_total) /
+                                PAGE_ALIGN(sizes[0]);
         }
 
+        *num_planes = 1;
+
         dev_dbg(icd->parent, "count=%d, size=%u\n", *count, sizes[0]);
 
         return 0;
@@ -267,6 +271,7 @@ static int sh_mobile_ceu_capture(struct sh_mobile_ceu_dev *pcdev)
         unsigned long top1, top2;
         unsigned long bottom1, bottom2;
         u32 status;
+        bool planar;
         int ret = 0;
 
         /*
@@ -314,17 +319,29 @@ static int sh_mobile_ceu_capture(struct sh_mobile_ceu_dev *pcdev)
 
         phys_addr_top = vb2_dma_contig_plane_dma_addr(pcdev->active, 0);
 
-        ceu_write(pcdev, top1, phys_addr_top);
-        if (V4L2_FIELD_NONE != pcdev->field) {
-                phys_addr_bottom = phys_addr_top + icd->user_width;
-                ceu_write(pcdev, bottom1, phys_addr_bottom);
-        }
-
         switch (icd->current_fmt->host_fmt->fourcc) {
         case V4L2_PIX_FMT_NV12:
         case V4L2_PIX_FMT_NV21:
         case V4L2_PIX_FMT_NV16:
         case V4L2_PIX_FMT_NV61:
+                planar = true;
+                break;
+        default:
+                planar = false;
+        }
+
+        ceu_write(pcdev, top1, phys_addr_top);
+        if (V4L2_FIELD_NONE != pcdev->field) {
+                if (planar)
+                        phys_addr_bottom = phys_addr_top + icd->user_width;
+                else
+                        phys_addr_bottom = phys_addr_top +
+                                soc_mbus_bytes_per_line(icd->user_width,
+                                                        icd->current_fmt->host_fmt);
+                ceu_write(pcdev, bottom1, phys_addr_bottom);
+        }
+
+        if (planar) {
                 phys_addr_top += icd->user_width *
                         icd->user_height;
                 ceu_write(pcdev, top2, phys_addr_top);
@@ -341,23 +358,40 @@ static int sh_mobile_ceu_capture(struct sh_mobile_ceu_dev *pcdev)
 
 static int sh_mobile_ceu_videobuf_prepare(struct vb2_buffer *vb)
 {
+        struct sh_mobile_ceu_buffer *buf = to_ceu_vb(vb);
+
+        /* Added list head initialization on alloc */
+        WARN(!list_empty(&buf->queue), "Buffer %p on queue!\n", vb);
+
+        return 0;
+}
+
+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 sh_mobile_ceu_buffer *buf;
+        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);
+        unsigned long size;
         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
                                                 icd->current_fmt->host_fmt);
-        unsigned long size;
 
         if (bytes_per_line < 0)
-                return bytes_per_line;
+                goto error;
 
-        buf = to_ceu_vb(vb);
+        size = icd->user_height * bytes_per_line;
+
+        if (vb2_plane_size(vb, 0) < size) {
+                dev_err(icd->parent, "Buffer #%d too small (%lu < %lu)\n",
+                        vb->v4l2_buf.index, vb2_plane_size(vb, 0), size);
+                goto error;
+        }
+
+        vb2_set_plane_payload(vb, 0, size);
 
         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 */
-        WARN(!list_empty(&buf->queue), "Buffer %p on queue!\n", vb);
-
 #ifdef DEBUG
         /*
          * This can be useful if you want to see if we actually fill
@@ -367,31 +401,6 @@ static int sh_mobile_ceu_videobuf_prepare(struct vb2_buffer *vb)
                 memset(vb2_plane_vaddr(vb, 0), 0xaa, vb2_get_plane_payload(vb, 0));
 #endif
 
-        BUG_ON(NULL == icd->current_fmt);
-
-        size = icd->user_height * bytes_per_line;
-
-        if (vb2_plane_size(vb, 0) < size) {
-                dev_err(icd->parent, "Buffer too small (%lu < %lu)\n",
-                        vb2_plane_size(vb, 0), size);
-                return -ENOBUFS;
-        }
-
-        vb2_set_plane_payload(vb, 0, size);
-
-        return 0;
-}
-
-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->parent);
-        struct sh_mobile_ceu_dev *pcdev = ici->priv;
-        struct sh_mobile_ceu_buffer *buf = to_ceu_vb(vb);
-
-        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);
         list_add_tail(&buf->queue, &pcdev->capture);
 
@@ -405,6 +414,11 @@ static void sh_mobile_ceu_videobuf_queue(struct vb2_buffer *vb)
                 sh_mobile_ceu_capture(pcdev);
         }
         spin_unlock_irq(&pcdev->lock);
+
+        return;
+
+error:
+        vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
 }
 
 static void sh_mobile_ceu_videobuf_release(struct vb2_buffer *vb)
@@ -429,11 +443,23 @@ static void sh_mobile_ceu_videobuf_release(struct vb2_buffer *vb)
         if (buf->queue.next)
                 list_del_init(&buf->queue);
 
+        pcdev->buf_total -= PAGE_ALIGN(vb2_plane_size(vb, 0));
+        dev_dbg(icd->parent, "%s() %zu bytes buffers\n", __func__,
+                pcdev->buf_total);
+
         spin_unlock_irq(&pcdev->lock);
 }
 
 static int sh_mobile_ceu_videobuf_init(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->parent);
+        struct sh_mobile_ceu_dev *pcdev = ici->priv;
+
+        pcdev->buf_total += PAGE_ALIGN(vb2_plane_size(vb, 0));
+        dev_dbg(icd->parent, "%s() %zu bytes buffers\n", __func__,
+                pcdev->buf_total);
+
         /* This is for locking debugging only */
         INIT_LIST_HEAD(&to_ceu_vb(vb)->queue);
         return 0;
@@ -535,19 +561,29 @@ static int sh_mobile_ceu_add_device(struct soc_camera_device *icd)
 
         pm_runtime_get_sync(ici->v4l2_dev.dev);
 
+        pcdev->buf_total = 0;
+
         ret = sh_mobile_ceu_soft_reset(pcdev);
 
         csi2_sd = find_csi2(pcdev);
+        if (csi2_sd)
+                csi2_sd->grp_id = (long)icd;
 
         ret = v4l2_subdev_call(csi2_sd, core, s_power, 1);
-        if (ret != -ENODEV && ret != -ENOIOCTLCMD && ret < 0) {
+        if (ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV) {
                 pm_runtime_put_sync(ici->v4l2_dev.dev);
-        } else {
-                pcdev->icd = icd;
-                ret = 0;
+                return ret;
         }
 
-        return ret;
+        /*
+         * -ENODEV is special: either csi2_sd == NULL or the CSI-2 driver
+         * has not found this soc-camera device among its clients
+         */
+        if (ret == -ENODEV && csi2_sd)
+                csi2_sd->grp_id = 0;
+        pcdev->icd = icd;
+
+        return 0;
 }
 
 /* Called with .video_lock held */
@@ -560,6 +596,8 @@ static void sh_mobile_ceu_remove_device(struct soc_camera_device *icd)
         BUG_ON(icd != pcdev->icd);
 
         v4l2_subdev_call(csi2_sd, core, s_power, 0);
+        if (csi2_sd)
+                csi2_sd->grp_id = 0;
         /* disable capture, disable interrupts */
         ceu_write(pcdev, CEIER, 0);
         sh_mobile_ceu_soft_reset(pcdev);
@@ -628,22 +666,22 @@ static void sh_mobile_ceu_set_rect(struct soc_camera_device *icd)
         left_offset     = cam->ceu_left;
         top_offset      = cam->ceu_top;
 
-        /* CEU cropping (CFSZR) is applied _after_ the scaling filter (CFLCR) */
+        WARN_ON(icd->user_width & 3 || icd->user_height & 3);
+
+        width = icd->user_width;
+
         if (pcdev->image_mode) {
                 in_width = cam->width;
                 if (!pcdev->is_16bit) {
                         in_width *= 2;
                         left_offset *= 2;
                 }
-                width = icd->user_width;
-                cdwdr_width = icd->user_width;
+                cdwdr_width = width;
         } else {
-                int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+                int bytes_per_line = soc_mbus_bytes_per_line(width,
                                                 icd->current_fmt->host_fmt);
                 unsigned int w_factor;
 
-                width = icd->user_width;
-
                 switch (icd->current_fmt->host_fmt->packing) {
                 case SOC_MBUS_PACKING_2X8_PADHI:
                         w_factor = 2;
@@ -653,10 +691,10 @@ static void sh_mobile_ceu_set_rect(struct soc_camera_device *icd)
                 }
 
                 in_width = cam->width * w_factor;
-                left_offset = left_offset * w_factor;
+                left_offset *= w_factor;
 
                 if (bytes_per_line < 0)
-                        cdwdr_width = icd->user_width;
+                        cdwdr_width = width;
                 else
                         cdwdr_width = bytes_per_line;
         }
@@ -664,7 +702,7 @@ static void sh_mobile_ceu_set_rect(struct soc_camera_device *icd)
         height = icd->user_height;
         in_height = cam->height;
         if (V4L2_FIELD_NONE != pcdev->field) {
-                height /= 2;
+                height = (height / 2) & ~3;
                 in_height /= 2;
                 top_offset /= 2;
                 cdwdr_width *= 2;
@@ -686,6 +724,7 @@ static void sh_mobile_ceu_set_rect(struct soc_camera_device *icd)
 
         ceu_write(pcdev, CAMOR, camor);
         ceu_write(pcdev, CAPWR, (in_height << 16) | in_width);
+        /* CFSZR clipping is applied _after_ the scaling filter (CFLCR) */
         ceu_write(pcdev, CFSZR, (height << 16) | width);
         ceu_write(pcdev, CDWDR, cdwdr_width);
 }
@@ -723,66 +762,93 @@ static void capture_restore(struct sh_mobile_ceu_dev *pcdev, u32 capsr)
                 ceu_write(pcdev, CAPSR, capsr);
 }
 
+/* Find the bus subdevice driver, e.g., CSI2 */
+static struct v4l2_subdev *find_bus_subdev(struct sh_mobile_ceu_dev *pcdev,
+                                           struct soc_camera_device *icd)
+{
+        if (pcdev->csi2_pdev) {
+                struct v4l2_subdev *csi2_sd = find_csi2(pcdev);
+                if (csi2_sd && csi2_sd->grp_id == (u32)icd)
+                        return csi2_sd;
+        }
+
+        return soc_camera_to_subdev(icd);
+}
+
+#define CEU_BUS_FLAGS (V4L2_MBUS_MASTER |       \
+                V4L2_MBUS_PCLK_SAMPLE_RISING |  \
+                V4L2_MBUS_HSYNC_ACTIVE_HIGH |   \
+                V4L2_MBUS_HSYNC_ACTIVE_LOW |    \
+                V4L2_MBUS_VSYNC_ACTIVE_HIGH |   \
+                V4L2_MBUS_VSYNC_ACTIVE_LOW |    \
+                V4L2_MBUS_DATA_ACTIVE_HIGH)
+
 /* Capture is not running, no interrupts, no locking needed */
 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->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
-        int ret;
-        unsigned long camera_flags, common_flags, value;
-        int yuv_lineskip;
+        struct v4l2_subdev *sd = find_bus_subdev(pcdev, icd);
         struct sh_mobile_ceu_cam *cam = icd->host_priv;
+        struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+        unsigned long value, common_flags = CEU_BUS_FLAGS;
         u32 capsr = capture_save_reset(pcdev);
+        unsigned int yuv_lineskip;
+        int ret;
 
-        camera_flags = icd->ops->query_bus_param(icd);
-        common_flags = soc_camera_bus_param_compatible(camera_flags,
-                                                       make_bus_param(pcdev));
-        if (!common_flags)
-                return -EINVAL;
+        /*
+         * If the client doesn't implement g_mbus_config, we just use our
+         * platform data
+         */
+        ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+        if (!ret) {
+                common_flags = soc_mbus_config_compatible(&cfg,
+                                                          common_flags);
+                if (!common_flags)
+                        return -EINVAL;
+        } else if (ret != -ENOIOCTLCMD) {
+                return ret;
+        }
 
         /* Make choises, based on platform preferences */
-        if ((common_flags & SOCAM_HSYNC_ACTIVE_HIGH) &&
-            (common_flags & SOCAM_HSYNC_ACTIVE_LOW)) {
+        if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
+            (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
                 if (pcdev->pdata->flags & SH_CEU_FLAG_HSYNC_LOW)
-                        common_flags &= ~SOCAM_HSYNC_ACTIVE_HIGH;
+                        common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
                 else
-                        common_flags &= ~SOCAM_HSYNC_ACTIVE_LOW;
+                        common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
         }
 
-        if ((common_flags & SOCAM_VSYNC_ACTIVE_HIGH) &&
-            (common_flags & SOCAM_VSYNC_ACTIVE_LOW)) {
+        if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
+            (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
                 if (pcdev->pdata->flags & SH_CEU_FLAG_VSYNC_LOW)
-                        common_flags &= ~SOCAM_VSYNC_ACTIVE_HIGH;
+                        common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
                 else
-                        common_flags &= ~SOCAM_VSYNC_ACTIVE_LOW;
+                        common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
         }
 
-        ret = icd->ops->set_bus_param(icd, common_flags);
-        if (ret < 0)
+        cfg.flags = common_flags;
+        ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
+        if (ret < 0 && ret != -ENOIOCTLCMD)
                 return ret;
 
-        switch (common_flags & SOCAM_DATAWIDTH_MASK) {
-        case SOCAM_DATAWIDTH_8:
-                pcdev->is_16bit = 0;
-                break;
-        case SOCAM_DATAWIDTH_16:
+        if (icd->current_fmt->host_fmt->bits_per_sample > 8)
                 pcdev->is_16bit = 1;
-                break;
-        default:
-                return -EINVAL;
-        }
+        else
+                pcdev->is_16bit = 0;
 
         ceu_write(pcdev, CRCNTR, 0);
         ceu_write(pcdev, CRCMPR, 0);
 
         value = 0x00000010; /* data fetch by default */
-        yuv_lineskip = 0;
+        yuv_lineskip = 0x10;
 
         switch (icd->current_fmt->host_fmt->fourcc) {
         case V4L2_PIX_FMT_NV12:
         case V4L2_PIX_FMT_NV21:
-                yuv_lineskip = 1; /* skip for NV12/21, no skip for NV16/61 */
+                /* convert 4:2:2 -> 4:2:0 */
+                yuv_lineskip = 0; /* skip for NV12/21, no skip for NV16/61 */
                 /* fall-through */
         case V4L2_PIX_FMT_NV16:
         case V4L2_PIX_FMT_NV61:
@@ -808,8 +874,8 @@ static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd,
             icd->current_fmt->host_fmt->fourcc == V4L2_PIX_FMT_NV61)
                 value ^= 0x00000100; /* swap U, V to change from NV1x->NVx1 */
 
-        value |= common_flags & SOCAM_VSYNC_ACTIVE_LOW ? 1 << 1 : 0;
-        value |= common_flags & SOCAM_HSYNC_ACTIVE_LOW ? 1 << 0 : 0;
+        value |= common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW ? 1 << 1 : 0;
+        value |= common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW ? 1 << 0 : 0;
         value |= pcdev->is_16bit ? 1 << 12 : 0;
 
         /* CSI2 mode */
@@ -852,9 +918,7 @@ static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd,
          * using 7 we swap the data bytes to match the incoming order:
          * D0, D1, D2, D3, D4, D5, D6, D7
          */
-        value = 0x00000017;
-        if (yuv_lineskip)
-                value &= ~0x00000010; /* convert 4:2:2 -> 4:2:0 */
+        value = 0x00000007 | yuv_lineskip;
 
         ceu_write(pcdev, CDOCR, value);
         ceu_write(pcdev, CFWCR, 0); /* keep "datafetch firewall" disabled */
@@ -875,13 +939,19 @@ static int sh_mobile_ceu_try_bus_param(struct soc_camera_device *icd,
 {
         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;
+        struct v4l2_subdev *sd = find_bus_subdev(pcdev, icd);
+        unsigned long common_flags = CEU_BUS_FLAGS;
+        struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+        int ret;
 
-        camera_flags = icd->ops->query_bus_param(icd);
-        common_flags = soc_camera_bus_param_compatible(camera_flags,
-                                                       make_bus_param(pcdev));
-        if (!common_flags || buswidth > 16 ||
-            (buswidth > 8 && !(common_flags & SOCAM_DATAWIDTH_16)))
+        ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+        if (!ret)
+                common_flags = soc_mbus_config_compatible(&cfg,
+                                                          common_flags);
+        else if (ret != -ENOIOCTLCMD)
+                return ret;
+
+        if (!common_flags || buswidth > 16)
                 return -EINVAL;
 
         return 0;
@@ -891,26 +961,26 @@ static const struct soc_mbus_pixelfmt sh_mobile_ceu_formats[] = {
         {
                 .fourcc                 = V4L2_PIX_FMT_NV12,
                 .name                   = "NV12",
-                .bits_per_sample        = 12,
-                .packing                = SOC_MBUS_PACKING_NONE,
+                .bits_per_sample        = 8,
+                .packing                = SOC_MBUS_PACKING_1_5X8,
                 .order                  = SOC_MBUS_ORDER_LE,
         }, {
                 .fourcc                 = V4L2_PIX_FMT_NV21,
                 .name                   = "NV21",
-                .bits_per_sample        = 12,
-                .packing                = SOC_MBUS_PACKING_NONE,
+                .bits_per_sample        = 8,
+                .packing                = SOC_MBUS_PACKING_1_5X8,
                 .order                  = SOC_MBUS_ORDER_LE,
         }, {
                 .fourcc                 = V4L2_PIX_FMT_NV16,
                 .name                   = "NV16",
-                .bits_per_sample        = 16,
-                .packing                = SOC_MBUS_PACKING_NONE,
+                .bits_per_sample        = 8,
+                .packing                = SOC_MBUS_PACKING_2X8_PADHI,
                 .order                  = SOC_MBUS_ORDER_LE,
         }, {
                 .fourcc                 = V4L2_PIX_FMT_NV61,
                 .name                   = "NV61",
-                .bits_per_sample        = 16,
-                .packing                = SOC_MBUS_PACKING_NONE,
+                .bits_per_sample        = 8,
+                .packing                = SOC_MBUS_PACKING_2X8_PADHI,
                 .order                  = SOC_MBUS_ORDER_LE,
         },
 };
@@ -920,6 +990,8 @@ static bool sh_mobile_ceu_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_1_5X8) ||
+                (fmt->bits_per_sample == 8 &&
                  fmt->packing == SOC_MBUS_PACKING_2X8_PADHI) ||
                 (fmt->bits_per_sample > 8 &&
                  fmt->packing == SOC_MBUS_PACKING_EXTEND16);
@@ -927,6 +999,38 @@ static bool sh_mobile_ceu_packing_supported(const struct soc_mbus_pixelfmt *fmt)
 
 static int client_g_rect(struct v4l2_subdev *sd, struct v4l2_rect *rect);
 
+static struct soc_camera_device *ctrl_to_icd(struct v4l2_ctrl *ctrl)
+{
+        return container_of(ctrl->handler, struct soc_camera_device,
+                                                        ctrl_handler);
+}
+
+static int sh_mobile_ceu_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct soc_camera_device *icd = ctrl_to_icd(ctrl);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+        struct sh_mobile_ceu_dev *pcdev = ici->priv;
+
+        switch (ctrl->id) {
+        case V4L2_CID_SHARPNESS:
+                switch (icd->current_fmt->host_fmt->fourcc) {
+                case V4L2_PIX_FMT_NV12:
+                case V4L2_PIX_FMT_NV21:
+                case V4L2_PIX_FMT_NV16:
+                case V4L2_PIX_FMT_NV61:
+                        ceu_write(pcdev, CLFCR, !ctrl->val);
+                        return 0;
+                }
+                break;
+        }
+
+        return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops sh_mobile_ceu_ctrl_ops = {
+        .s_ctrl = sh_mobile_ceu_s_ctrl,
+};
+
 static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int idx,
                                      struct soc_camera_format_xlate *xlate)
 {
@@ -952,6 +1056,7 @@ static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int
         }
 
         if (!pcdev->pdata->csi2) {
+                /* Are there any restrictions in the CSI-2 case? */
                 ret = sh_mobile_ceu_try_bus_param(icd, fmt->bits_per_sample);
                 if (ret < 0)
                         return 0;
@@ -962,6 +1067,12 @@ static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int
                 struct v4l2_rect rect;
                 int shift = 0;
 
+                /* Add our control */
+                v4l2_ctrl_new_std(&icd->ctrl_handler, &sh_mobile_ceu_ctrl_ops,
+                                  V4L2_CID_SHARPNESS, 0, 1, 1, 0);
+                if (icd->ctrl_handler.error)
+                        return icd->ctrl_handler.error;
+
                 /* FIXME: subwindow is lost between close / open */
 
                 /* Cache current client geometry */
@@ -1004,9 +1115,6 @@ static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int
                 cam->width      = mf.width;
                 cam->height     = mf.height;
 
-                cam->width      = mf.width;
-                cam->height     = mf.height;
-
                 icd->host_priv = cam;
         } else {
                 cam = icd->host_priv;
@@ -1278,6 +1386,7 @@ static int client_s_fmt(struct soc_camera_device *icd,
         unsigned int width = mf->width, height = mf->height, tmp_w, tmp_h;
         unsigned int max_width, max_height;
         struct v4l2_cropcap cap;
+        bool ceu_1to1;
         int ret;
 
         ret = v4l2_device_call_until_err(sd->v4l2_dev, (long)icd, video,
@@ -1287,7 +1396,14 @@ static int client_s_fmt(struct soc_camera_device *icd,
 
         dev_geo(dev, "camera scaled to %ux%u\n", mf->width, mf->height);
 
-        if ((width == mf->width && height == mf->height) || !ceu_can_scale)
+        if (width == mf->width && height == mf->height) {
+                /* Perfect! The client has done it all. */
+                ceu_1to1 = true;
+                goto update_cache;
+        }
+
+        ceu_1to1 = false;
+        if (!ceu_can_scale)
                 goto update_cache;
 
         cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
@@ -1327,7 +1443,10 @@ update_cache:
         if (ret < 0)
                 return ret;
 
-        update_subrect(cam);
+        if (ceu_1to1)
+                cam->subrect = cam->rect;
+        else
+                update_subrect(cam);
 
         return 0;
 }
@@ -1414,7 +1533,10 @@ static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
         capsr = capture_save_reset(pcdev);
         dev_dbg(dev, "CAPSR 0x%x, CFLCR 0x%x\n", capsr, pcdev->cflcr);
 
-        /* 1. - 2. Apply iterative camera S_CROP for new input window. */
+        /*
+         * 1. - 2. Apply iterative camera S_CROP for new input window, read back
+         * actual camera rectangle.
+         */
         ret = client_s_crop(icd, a, &cam_crop);
         if (ret < 0)
                 return ret;
@@ -1498,8 +1620,9 @@ static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
                 ceu_write(pcdev, CFLCR, cflcr);
         }
 
-        icd->user_width  = out_width;
-        icd->user_height = out_height;
+        icd->user_width  = out_width & ~3;
+        icd->user_height = out_height & ~3;
+        /* Offsets are applied at the CEU scaling filter input */
         cam->ceu_left    = scale_down(rect->left - cam_rect->left, scale_cam_h) & ~1;
         cam->ceu_top     = scale_down(rect->top - cam_rect->top, scale_cam_v) & ~1;
 
@@ -1538,7 +1661,7 @@ static int sh_mobile_ceu_get_crop(struct soc_camera_device *icd,
  * CEU crop, mapped backed onto the client input (subrect).
  */
 static void calculate_client_output(struct soc_camera_device *icd,
-                struct v4l2_pix_format *pix, struct v4l2_mbus_framefmt *mf)
+                const struct v4l2_pix_format *pix, struct v4l2_mbus_framefmt *mf)
 {
         struct sh_mobile_ceu_cam *cam = icd->host_priv;
         struct device *dev = icd->parent;
@@ -1574,8 +1697,8 @@ static void calculate_client_output(struct soc_camera_device *icd,
         dev_geo(dev, "3: scales %u:%u\n", scale_h, scale_v);
 
         /*
-         * 4. Calculate client output window by applying combined scales to real
-         *    input window.
+         * 4. Calculate desired client output window by applying combined scales
+         *    to client (real) input window.
          */
         mf->width       = scale_down(cam->rect.width, scale_h);
         mf->height      = scale_down(cam->rect.height, scale_v);
@@ -1600,8 +1723,6 @@ static int sh_mobile_ceu_set_fmt(struct soc_camera_device *icd,
         bool image_mode;
         enum v4l2_field field;
 
-        dev_geo(dev, "S_FMT(pix=0x%x, %ux%u)\n", pixfmt, pix->width, pix->height);
-
         switch (pix->field) {
         default:
                 pix->field = V4L2_FIELD_NONE;
@@ -1622,8 +1743,8 @@ static int sh_mobile_ceu_set_fmt(struct soc_camera_device *icd,
                 return -EINVAL;
         }
 
-        /* 1.-4. Calculate client output geometry */
-        calculate_client_output(icd, &f->fmt.pix, &mf);
+        /* 1.-4. Calculate desired client output geometry */
+        calculate_client_output(icd, pix, &mf);
         mf.field        = pix->field;
         mf.colorspace   = pix->colorspace;
         mf.code         = xlate->code;
@@ -1639,6 +1760,9 @@ static int sh_mobile_ceu_set_fmt(struct soc_camera_device *icd,
                 image_mode = false;
         }
 
+        dev_geo(dev, "S_FMT(pix=0x%x, fld 0x%x, code 0x%x, %ux%u)\n", pixfmt, mf.field, mf.code,
+                pix->width, pix->height);
+
         dev_geo(dev, "4: request camera output %ux%u\n", mf.width, mf.height);
 
         /* 5. - 9. */
@@ -1700,6 +1824,10 @@ static int sh_mobile_ceu_set_fmt(struct soc_camera_device *icd,
         pcdev->field = field;
         pcdev->image_mode = image_mode;
 
+        /* CFSZR requirement */
+        pix->width      &= ~3;
+        pix->height     &= ~3;
+
         return 0;
 }
 
@@ -1725,7 +1853,8 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
 
         /* FIXME: calculate using depth and bus width */
 
-        v4l_bound_align_image(&pix->width, 2, 2560, 1,
+        /* CFSZR requires height and width to be 4-pixel aligned */
+        v4l_bound_align_image(&pix->width, 2, 2560, 2,
                               &pix->height, 4, 1920, 2, 0);
 
         width = pix->width;
@@ -1778,6 +1907,9 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
                         pix->height = height;
         }
 
+        pix->width      &= ~3;
+        pix->height     &= ~3;
+
         dev_geo(icd->parent, "%s(): return %d, fmt 0x%x, %ux%u\n",
                 __func__, ret, pix->pixelformat, pix->width, pix->height);
 
@@ -1824,8 +1956,8 @@ static int sh_mobile_ceu_set_livecrop(struct soc_camera_device *icd,
                              out_height != f.fmt.pix.height))
                         ret = -EINVAL;
                 if (!ret) {
-                        icd->user_width         = out_width;
-                        icd->user_height        = out_height;
+                        icd->user_width         = out_width & ~3;
+                        icd->user_height        = out_height & ~3;
                         ret = sh_mobile_ceu_set_bus_param(icd,
                                         icd->current_fmt->host_fmt->fourcc);
                 }
@@ -1869,55 +2001,6 @@ static int sh_mobile_ceu_init_videobuf(struct vb2_queue *q,
         return vb2_queue_init(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->parent);
-        struct sh_mobile_ceu_dev *pcdev = ici->priv;
-        u32 val;
-
-        switch (ctrl->id) {
-        case V4L2_CID_SHARPNESS:
-                val = ceu_read(pcdev, CLFCR);
-                ctrl->value = val ^ 1;
-                return 0;
-        }
-        return -ENOIOCTLCMD;
-}
-
-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->parent);
-        struct sh_mobile_ceu_dev *pcdev = ici->priv;
-
-        switch (ctrl->id) {
-        case V4L2_CID_SHARPNESS:
-                switch (icd->current_fmt->host_fmt->fourcc) {
-                case V4L2_PIX_FMT_NV12:
-                case V4L2_PIX_FMT_NV21:
-                case V4L2_PIX_FMT_NV16:
-                case V4L2_PIX_FMT_NV61:
-                        ceu_write(pcdev, CLFCR, !ctrl->value);
-                        return 0;
-                }
-                return -EINVAL;
-        }
-        return -ENOIOCTLCMD;
-}
-
-static const struct v4l2_queryctrl sh_mobile_ceu_controls[] = {
-        {
-                .id             = V4L2_CID_SHARPNESS,
-                .type           = V4L2_CTRL_TYPE_BOOLEAN,
-                .name           = "Low-pass filter",
-                .minimum        = 0,
-                .maximum        = 1,
-                .step           = 1,
-                .default_value  = 0,
-        },
-};
-
 static struct soc_camera_host_ops sh_mobile_ceu_host_ops = {
         .owner          = THIS_MODULE,
         .add            = sh_mobile_ceu_add_device,
@@ -1929,14 +2012,10 @@ static struct soc_camera_host_ops sh_mobile_ceu_host_ops = {
         .set_livecrop   = sh_mobile_ceu_set_livecrop,
         .set_fmt        = sh_mobile_ceu_set_fmt,
         .try_fmt        = sh_mobile_ceu_try_fmt,
-        .set_ctrl       = sh_mobile_ceu_set_ctrl,
-        .get_ctrl       = sh_mobile_ceu_get_ctrl,
         .poll           = sh_mobile_ceu_poll,
         .querycap       = sh_mobile_ceu_querycap,
         .set_bus_param  = sh_mobile_ceu_set_bus_param,
         .init_videobuf2 = sh_mobile_ceu_init_videobuf,
-        .controls       = sh_mobile_ceu_controls,
-        .num_controls   = ARRAY_SIZE(sh_mobile_ceu_controls),
 };
 
 struct bus_wait {
diff --git a/drivers/media/video/sh_mobile_csi2.c b/drivers/media/video/sh_mobile_csi2.c
index 2893a0134c7e..37706eb81f25 100644
--- a/drivers/media/video/sh_mobile_csi2.c
+++ b/drivers/media/video/sh_mobile_csi2.c
@@ -19,6 +19,7 @@
 #include <media/sh_mobile_ceu.h>
 #include <media/sh_mobile_csi2.h>
 #include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-dev.h>
 #include <media/v4l2-device.h>
@@ -35,11 +36,10 @@ struct sh_csi2 {
         struct v4l2_subdev              subdev;
         struct list_head                list;
         unsigned int                    irq;
+        unsigned long                   mipi_flags;
         void __iomem                    *base;
         struct platform_device          *pdev;
         struct sh_csi2_client_config    *client;
-        unsigned long (*query_bus_param)(struct soc_camera_device *);
-        int (*set_bus_param)(struct soc_camera_device *, unsigned long);
 };
 
 static int sh_csi2_try_fmt(struct v4l2_subdev *sd,
@@ -127,9 +127,34 @@ static int sh_csi2_s_fmt(struct v4l2_subdev *sd,
         return 0;
 }
 
+static int sh_csi2_g_mbus_config(struct v4l2_subdev *sd,
+                                 struct v4l2_mbus_config *cfg)
+{
+        cfg->flags = V4L2_MBUS_PCLK_SAMPLE_RISING |
+                V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+                V4L2_MBUS_MASTER | V4L2_MBUS_DATA_ACTIVE_HIGH;
+        cfg->type = V4L2_MBUS_PARALLEL;
+
+        return 0;
+}
+
+static int sh_csi2_s_mbus_config(struct v4l2_subdev *sd,
+                                 const struct v4l2_mbus_config *cfg)
+{
+        struct sh_csi2 *priv = container_of(sd, struct sh_csi2, subdev);
+        struct soc_camera_device *icd = (struct soc_camera_device *)sd->grp_id;
+        struct v4l2_subdev *client_sd = soc_camera_to_subdev(icd);
+        struct v4l2_mbus_config client_cfg = {.type = V4L2_MBUS_CSI2,
+                                              .flags = priv->mipi_flags};
+
+        return v4l2_subdev_call(client_sd, video, s_mbus_config, &client_cfg);
+}
+
 static struct v4l2_subdev_video_ops sh_csi2_subdev_video_ops = {
         .s_mbus_fmt     = sh_csi2_s_fmt,
         .try_mbus_fmt   = sh_csi2_try_fmt,
+        .g_mbus_config  = sh_csi2_g_mbus_config,
+        .s_mbus_config  = sh_csi2_s_mbus_config,
 };
 
 static void sh_csi2_hwinit(struct sh_csi2 *priv)
@@ -144,11 +169,21 @@ static void sh_csi2_hwinit(struct sh_csi2 *priv)
         udelay(5);
         iowrite32(0x00000000, priv->base + SH_CSI2_SRST);
 
-        if (priv->client->lanes & 3)
-                tmp |= priv->client->lanes & 3;
-        else
-                /* Default - both lanes */
-                tmp |= 3;
+        switch (pdata->type) {
+        case SH_CSI2C:
+                if (priv->client->lanes == 1)
+                        tmp |= 1;
+                else
+                        /* Default - both lanes */
+                        tmp |= 3;
+                break;
+        case SH_CSI2I:
+                if (!priv->client->lanes || priv->client->lanes > 4)
+                        /* Default - all 4 lanes */
+                        tmp |= 0xf;
+                else
+                        tmp |= (1 << priv->client->lanes) - 1;
+        }
 
         if (priv->client->phy == SH_CSI2_PHY_MAIN)
                 tmp |= 0x8000;
@@ -163,38 +198,18 @@ static void sh_csi2_hwinit(struct sh_csi2 *priv)
         iowrite32(tmp, priv->base + SH_CSI2_CHKSUM);
 }
 
-static int sh_csi2_set_bus_param(struct soc_camera_device *icd,
-                                 unsigned long flags)
-{
-        return 0;
-}
-
-static unsigned long sh_csi2_query_bus_param(struct soc_camera_device *icd)
-{
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-        const unsigned long flags = SOCAM_PCLK_SAMPLE_RISING |
-                SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_HIGH |
-                SOCAM_MASTER | SOCAM_DATAWIDTH_8 | SOCAM_DATA_ACTIVE_HIGH;
-
-        return soc_camera_apply_sensor_flags(icl, flags);
-}
-
 static int sh_csi2_client_connect(struct sh_csi2 *priv)
 {
         struct sh_csi2_pdata *pdata = priv->pdev->dev.platform_data;
-        struct v4l2_subdev *sd, *csi2_sd = &priv->subdev;
-        struct soc_camera_device *icd = NULL;
+        struct soc_camera_device *icd = (struct soc_camera_device *)priv->subdev.grp_id;
+        struct v4l2_subdev *client_sd = soc_camera_to_subdev(icd);
         struct device *dev = v4l2_get_subdevdata(&priv->subdev);
-        int i;
+        struct v4l2_mbus_config cfg;
+        unsigned long common_flags, csi2_flags;
+        int i, ret;
 
-        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;
+        if (priv->client)
+                return -EBUSY;
 
         for (i = 0; i < pdata->num_clients; i++)
                 if (&pdata->clients[i].pdev->dev == icd->pdev)
@@ -205,14 +220,41 @@ static int sh_csi2_client_connect(struct sh_csi2 *priv)
         if (i == pdata->num_clients)
                 return -ENODEV;
 
-        priv->client = pdata->clients + i;
+        /* Check if we can support this camera */
+        csi2_flags = V4L2_MBUS_CSI2_CONTINUOUS_CLOCK | V4L2_MBUS_CSI2_1_LANE;
+
+        switch (pdata->type) {
+        case SH_CSI2C:
+                if (pdata->clients[i].lanes != 1)
+                        csi2_flags |= V4L2_MBUS_CSI2_2_LANE;
+                break;
+        case SH_CSI2I:
+                switch (pdata->clients[i].lanes) {
+                default:
+                        csi2_flags |= V4L2_MBUS_CSI2_4_LANE;
+                case 3:
+                        csi2_flags |= V4L2_MBUS_CSI2_3_LANE;
+                case 2:
+                        csi2_flags |= V4L2_MBUS_CSI2_2_LANE;
+                }
+        }
 
-        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;
+        cfg.type = V4L2_MBUS_CSI2;
+        ret = v4l2_subdev_call(client_sd, video, g_mbus_config, &cfg);
+        if (ret == -ENOIOCTLCMD)
+                common_flags = csi2_flags;
+        else if (!ret)
+                common_flags = soc_mbus_config_compatible(&cfg,
+                                                          csi2_flags);
+        else
+                common_flags = 0;
 
-        csi2_sd->grp_id = (long)icd;
+        if (!common_flags)
+                return -EINVAL;
+
+        /* All good: camera MIPI configuration supported */
+        priv->mipi_flags = common_flags;
+        priv->client = pdata->clients + i;
 
         pm_runtime_get_sync(dev);
 
@@ -223,16 +265,10 @@ static int sh_csi2_client_connect(struct sh_csi2 *priv)
 
 static void sh_csi2_client_disconnect(struct sh_csi2 *priv)
 {
-        struct soc_camera_device *icd = (struct soc_camera_device *)priv->subdev.grp_id;
+        if (!priv->client)
+                return;
 
         priv->client = NULL;
-        priv->subdev.grp_id = 0;
-
-        /* 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));
 }
diff --git a/drivers/media/video/soc_camera.c b/drivers/media/video/soc_camera.c
index 5bdfe7e16bc1..b72580c38957 100644
--- a/drivers/media/video/soc_camera.c
+++ b/drivers/media/video/soc_camera.c
@@ -50,49 +50,65 @@ static LIST_HEAD(hosts);
 static LIST_HEAD(devices);
 static DEFINE_MUTEX(list_lock);         /* Protects the list of hosts */
 
-static int soc_camera_power_set(struct soc_camera_device *icd,
-                                struct soc_camera_link *icl,
-                                int power_on)
+static int soc_camera_power_on(struct soc_camera_device *icd,
+                               struct soc_camera_link *icl)
 {
-        int ret;
-
-        if (power_on) {
-                ret = regulator_bulk_enable(icl->num_regulators,
-                                            icl->regulators);
-                if (ret < 0) {
-                        dev_err(icd->pdev, "Cannot enable regulators\n");
-                        return ret;
-                }
+        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+        int ret = regulator_bulk_enable(icl->num_regulators,
+                                        icl->regulators);
+        if (ret < 0) {
+                dev_err(icd->pdev, "Cannot enable regulators\n");
+                return ret;
+        }
 
-                if (icl->power)
-                        ret = icl->power(icd->pdev, power_on);
+        if (icl->power) {
+                ret = icl->power(icd->pdev, 1);
                 if (ret < 0) {
                         dev_err(icd->pdev,
                                 "Platform failed to power-on the camera.\n");
-
-                        regulator_bulk_disable(icl->num_regulators,
-                                               icl->regulators);
-                        return ret;
+                        goto elinkpwr;
                 }
-        } else {
-                ret = 0;
-                if (icl->power)
-                        ret = icl->power(icd->pdev, 0);
+        }
+
+        ret = v4l2_subdev_call(sd, core, s_power, 1);
+        if (ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV)
+                goto esdpwr;
+
+        return 0;
+
+esdpwr:
+        if (icl->power)
+                icl->power(icd->pdev, 0);
+elinkpwr:
+        regulator_bulk_disable(icl->num_regulators,
+                               icl->regulators);
+        return ret;
+}
+
+static int soc_camera_power_off(struct soc_camera_device *icd,
+                                struct soc_camera_link *icl)
+{
+        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+        int ret = v4l2_subdev_call(sd, core, s_power, 0);
+
+        if (ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV)
+                return ret;
+
+        if (icl->power) {
+                ret = icl->power(icd->pdev, 0);
                 if (ret < 0) {
                         dev_err(icd->pdev,
                                 "Platform failed to power-off the camera.\n");
                         return ret;
                 }
-
-                ret = regulator_bulk_disable(icl->num_regulators,
-                                             icl->regulators);
-                if (ret < 0) {
-                        dev_err(icd->pdev, "Cannot disable regulators\n");
-                        return ret;
-                }
         }
 
-        return 0;
+        ret = regulator_bulk_disable(icl->num_regulators,
+                                     icl->regulators);
+        if (ret < 0)
+                dev_err(icd->pdev, "Cannot disable regulators\n");
+
+        return ret;
 }
 
 const struct soc_camera_format_xlate *soc_camera_xlate_by_fourcc(
@@ -108,38 +124,38 @@ const struct soc_camera_format_xlate *soc_camera_xlate_by_fourcc(
 EXPORT_SYMBOL(soc_camera_xlate_by_fourcc);
 
 /**
- * soc_camera_apply_sensor_flags() - apply platform SOCAM_SENSOR_INVERT_* flags
+ * soc_camera_apply_board_flags() - apply platform SOCAM_SENSOR_INVERT_* flags
  * @icl:        camera platform parameters
- * @flags:      flags to be inverted according to platform configuration
+ * @cfg:        media bus configuration
  * @return:     resulting flags
  */
-unsigned long soc_camera_apply_sensor_flags(struct soc_camera_link *icl,
-                                            unsigned long flags)
+unsigned long soc_camera_apply_board_flags(struct soc_camera_link *icl,
+                                           const struct v4l2_mbus_config *cfg)
 {
-        unsigned long f;
+        unsigned long f, flags = cfg->flags;
 
         /* If only one of the two polarities is supported, switch to the opposite */
         if (icl->flags & SOCAM_SENSOR_INVERT_HSYNC) {
-                f = flags & (SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_LOW);
-                if (f == SOCAM_HSYNC_ACTIVE_HIGH || f == SOCAM_HSYNC_ACTIVE_LOW)
-                        flags ^= SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_LOW;
+                f = flags & (V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_LOW);
+                if (f == V4L2_MBUS_HSYNC_ACTIVE_HIGH || f == V4L2_MBUS_HSYNC_ACTIVE_LOW)
+                        flags ^= V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_LOW;
         }
 
         if (icl->flags & SOCAM_SENSOR_INVERT_VSYNC) {
-                f = flags & (SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_LOW);
-                if (f == SOCAM_VSYNC_ACTIVE_HIGH || f == SOCAM_VSYNC_ACTIVE_LOW)
-                        flags ^= SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_LOW;
+                f = flags & (V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_LOW);
+                if (f == V4L2_MBUS_VSYNC_ACTIVE_HIGH || f == V4L2_MBUS_VSYNC_ACTIVE_LOW)
+                        flags ^= V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_LOW;
         }
 
         if (icl->flags & SOCAM_SENSOR_INVERT_PCLK) {
-                f = flags & (SOCAM_PCLK_SAMPLE_RISING | SOCAM_PCLK_SAMPLE_FALLING);
-                if (f == SOCAM_PCLK_SAMPLE_RISING || f == SOCAM_PCLK_SAMPLE_FALLING)
-                        flags ^= SOCAM_PCLK_SAMPLE_RISING | SOCAM_PCLK_SAMPLE_FALLING;
+                f = flags & (V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING);
+                if (f == V4L2_MBUS_PCLK_SAMPLE_RISING || f == V4L2_MBUS_PCLK_SAMPLE_FALLING)
+                        flags ^= V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING;
         }
 
         return flags;
 }
-EXPORT_SYMBOL(soc_camera_apply_sensor_flags);
+EXPORT_SYMBOL(soc_camera_apply_board_flags);
 
 #define pixfmtstr(x) (x) & 0xff, ((x) >> 8) & 0xff, ((x) >> 16) & 0xff, \
         ((x) >> 24) & 0xff
@@ -233,6 +249,14 @@ static int soc_camera_s_std(struct file *file, void *priv, v4l2_std_id *a)
         return v4l2_subdev_call(sd, core, s_std, *a);
 }
 
+static int soc_camera_g_std(struct file *file, void *priv, v4l2_std_id *a)
+{
+        struct soc_camera_device *icd = file->private_data;
+        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+
+        return v4l2_subdev_call(sd, core, g_std, a);
+}
+
 static int soc_camera_enum_fsizes(struct file *file, void *fh,
                                          struct v4l2_frmsizeenum *fsize)
 {
@@ -318,6 +342,32 @@ static int soc_camera_dqbuf(struct file *file, void *priv,
                 return vb2_dqbuf(&icd->vb2_vidq, p, file->f_flags & O_NONBLOCK);
 }
 
+static int soc_camera_create_bufs(struct file *file, void *priv,
+                            struct v4l2_create_buffers *create)
+{
+        struct soc_camera_device *icd = file->private_data;
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+        /* videobuf2 only */
+        if (ici->ops->init_videobuf)
+                return -EINVAL;
+        else
+                return vb2_create_bufs(&icd->vb2_vidq, create);
+}
+
+static int soc_camera_prepare_buf(struct file *file, void *priv,
+                                  struct v4l2_buffer *b)
+{
+        struct soc_camera_device *icd = file->private_data;
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+        /* videobuf2 only */
+        if (ici->ops->init_videobuf)
+                return -EINVAL;
+        else
+                return vb2_prepare_buf(&icd->vb2_vidq, b);
+}
+
 /* Always entered with .video_lock held */
 static int soc_camera_init_user_formats(struct soc_camera_device *icd)
 {
@@ -448,7 +498,7 @@ static int soc_camera_open(struct file *file)
         struct soc_camera_host *ici;
         int ret;
 
-        if (!icd->ops)
+        if (!to_soc_camera_control(icd))
                 /* No device driver attached */
                 return -ENODEV;
 
@@ -476,7 +526,7 @@ static int soc_camera_open(struct file *file)
                         },
                 };
 
-                ret = soc_camera_power_set(icd, icl, 1);
+                ret = soc_camera_power_on(icd, icl);
                 if (ret < 0)
                         goto epower;
 
@@ -512,6 +562,7 @@ static int soc_camera_open(struct file *file)
                         if (ret < 0)
                                 goto einitvb;
                 }
+                v4l2_ctrl_handler_setup(&icd->ctrl_handler);
         }
 
         file->private_data = icd;
@@ -529,7 +580,7 @@ esfmt:
 eresume:
         ici->ops->remove(icd);
 eiciadd:
-        soc_camera_power_set(icd, icl, 0);
+        soc_camera_power_off(icd, icl);
 epower:
         icd->use_count--;
         module_put(ici->ops->owner);
@@ -553,7 +604,7 @@ static int soc_camera_close(struct file *file)
                 if (ici->ops->init_videobuf2)
                         vb2_queue_release(&icd->vb2_vidq);
 
-                soc_camera_power_set(icd, icl, 0);
+                soc_camera_power_off(icd, icl);
         }
 
         if (icd->streamer == file)
@@ -781,75 +832,6 @@ static int soc_camera_streamoff(struct file *file, void *priv,
         return 0;
 }
 
-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->parent);
-        int i;
-
-        WARN_ON(priv != file->private_data);
-
-        if (!qc->id)
-                return -EINVAL;
-
-        /* First check host controls */
-        for (i = 0; i < ici->ops->num_controls; i++)
-                if (qc->id == ici->ops->controls[i].id) {
-                        memcpy(qc, &(ici->ops->controls[i]),
-                                sizeof(*qc));
-                        return 0;
-                }
-
-        /* Then device controls */
-        for (i = 0; i < icd->ops->num_controls; i++)
-                if (qc->id == icd->ops->controls[i].id) {
-                        memcpy(qc, &(icd->ops->controls[i]),
-                                sizeof(*qc));
-                        return 0;
-                }
-
-        return -EINVAL;
-}
-
-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->parent);
-        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        int ret;
-
-        WARN_ON(priv != file->private_data);
-
-        if (ici->ops->get_ctrl) {
-                ret = ici->ops->get_ctrl(icd, ctrl);
-                if (ret != -ENOIOCTLCMD)
-                        return ret;
-        }
-
-        return v4l2_subdev_call(sd, core, g_ctrl, ctrl);
-}
-
-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->parent);
-        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        int ret;
-
-        WARN_ON(priv != file->private_data);
-
-        if (ici->ops->set_ctrl) {
-                ret = ici->ops->set_ctrl(icd, ctrl);
-                if (ret != -ENOIOCTLCMD)
-                        return ret;
-        }
-
-        return v4l2_subdev_call(sd, core, s_ctrl, ctrl);
-}
-
 static int soc_camera_cropcap(struct file *file, void *fh,
                               struct v4l2_cropcap *a)
 {
@@ -1003,7 +985,7 @@ static int soc_camera_init_i2c(struct soc_camera_device *icd,
                 goto ei2cga;
         }
 
-        icl->board_info->platform_data = icd;
+        icl->board_info->platform_data = icl;
 
         subdev = v4l2_i2c_new_subdev_board(&ici->v4l2_dev, adap,
                                 icl->board_info, NULL);
@@ -1052,12 +1034,29 @@ static int soc_camera_probe(struct soc_camera_device *icd)
 
         dev_info(icd->pdev, "Probing %s\n", dev_name(icd->pdev));
 
+        /*
+         * Currently the subdev with the largest number of controls (13) is
+         * ov6550. So let's pick 16 as a hint for the control handler. Note
+         * that this is a hint only: too large and you waste some memory, too
+         * small and there is a (very) small performance hit when looking up
+         * controls in the internal hash.
+         */
+        ret = v4l2_ctrl_handler_init(&icd->ctrl_handler, 16);
+        if (ret < 0)
+                return ret;
+
         ret = regulator_bulk_get(icd->pdev, icl->num_regulators,
                                  icl->regulators);
         if (ret < 0)
                 goto ereg;
 
-        ret = soc_camera_power_set(icd, icl, 1);
+        /*
+         * This will not yet call v4l2_subdev_core_ops::s_power(1), because the
+         * subdevice has not been initialised yet. We'll have to call it once
+         * again after initialisation, even though it shouldn't be needed, we
+         * don't do any IO here.
+         */
+        ret = soc_camera_power_on(icd, icl);
         if (ret < 0)
                 goto epower;
 
@@ -1098,6 +1097,7 @@ static int soc_camera_probe(struct soc_camera_device *icd)
                 if (!control || !control->driver || !dev_get_drvdata(control) ||
                     !try_module_get(control->driver->owner)) {
                         icl->del_device(icd);
+                        ret = -ENODEV;
                         goto enodrv;
                 }
         }
@@ -1105,6 +1105,9 @@ static int soc_camera_probe(struct soc_camera_device *icd)
         sd = soc_camera_to_subdev(icd);
         sd->grp_id = (long)icd;
 
+        if (v4l2_ctrl_add_handler(&icd->ctrl_handler, sd->ctrl_handler))
+                goto ectrl;
+
         /* At this point client .probe() should have run already */
         ret = soc_camera_init_user_formats(icd);
         if (ret < 0)
@@ -1123,6 +1126,10 @@ static int soc_camera_probe(struct soc_camera_device *icd)
         if (ret < 0)
                 goto evidstart;
 
+        ret = v4l2_subdev_call(sd, core, s_power, 1);
+        if (ret < 0 && ret != -ENOIOCTLCMD)
+                goto esdpwr;
+
         /* Try to improve our guess of a reasonable window format */
         if (!v4l2_subdev_call(sd, video, g_mbus_fmt, &mf)) {
                 icd->user_width         = mf.width;
@@ -1133,16 +1140,19 @@ static int soc_camera_probe(struct soc_camera_device *icd)
 
         ici->ops->remove(icd);
 
-        soc_camera_power_set(icd, icl, 0);
+        soc_camera_power_off(icd, icl);
 
         mutex_unlock(&icd->video_lock);
 
         return 0;
 
+esdpwr:
+        video_unregister_device(icd->vdev);
 evidstart:
         mutex_unlock(&icd->video_lock);
         soc_camera_free_user_formats(icd);
 eiufmt:
+ectrl:
         if (icl->board_info) {
                 soc_camera_free_i2c(icd);
         } else {
@@ -1152,13 +1162,15 @@ eiufmt:
 enodrv:
 eadddev:
         video_device_release(icd->vdev);
+        icd->vdev = NULL;
 evdc:
         ici->ops->remove(icd);
 eadd:
-        soc_camera_power_set(icd, icl, 0);
+        soc_camera_power_off(icd, icl);
 epower:
         regulator_bulk_free(icl->num_regulators, icl->regulators);
 ereg:
+        v4l2_ctrl_handler_free(&icd->ctrl_handler);
         return ret;
 }
 
@@ -1173,6 +1185,7 @@ static int soc_camera_remove(struct soc_camera_device *icd)
 
         BUG_ON(!icd->parent);
 
+        v4l2_ctrl_handler_free(&icd->ctrl_handler);
         if (vdev) {
                 video_unregister_device(vdev);
                 icd->vdev = NULL;
@@ -1363,24 +1376,24 @@ static int soc_camera_device_register(struct soc_camera_device *icd)
 
 static const struct v4l2_ioctl_ops soc_camera_ioctl_ops = {
         .vidioc_querycap         = soc_camera_querycap,
+        .vidioc_try_fmt_vid_cap  = soc_camera_try_fmt_vid_cap,
         .vidioc_g_fmt_vid_cap    = soc_camera_g_fmt_vid_cap,
-        .vidioc_enum_fmt_vid_cap = soc_camera_enum_fmt_vid_cap,
         .vidioc_s_fmt_vid_cap    = soc_camera_s_fmt_vid_cap,
+        .vidioc_enum_fmt_vid_cap = soc_camera_enum_fmt_vid_cap,
         .vidioc_enum_input       = soc_camera_enum_input,
         .vidioc_g_input          = soc_camera_g_input,
         .vidioc_s_input          = soc_camera_s_input,
         .vidioc_s_std            = soc_camera_s_std,
+        .vidioc_g_std            = soc_camera_g_std,
         .vidioc_enum_framesizes  = soc_camera_enum_fsizes,
         .vidioc_reqbufs          = soc_camera_reqbufs,
-        .vidioc_try_fmt_vid_cap  = soc_camera_try_fmt_vid_cap,
         .vidioc_querybuf         = soc_camera_querybuf,
         .vidioc_qbuf             = soc_camera_qbuf,
         .vidioc_dqbuf            = soc_camera_dqbuf,
+        .vidioc_create_bufs      = soc_camera_create_bufs,
+        .vidioc_prepare_buf      = soc_camera_prepare_buf,
         .vidioc_streamon         = soc_camera_streamon,
         .vidioc_streamoff        = soc_camera_streamoff,
-        .vidioc_queryctrl        = soc_camera_queryctrl,
-        .vidioc_g_ctrl           = soc_camera_g_ctrl,
-        .vidioc_s_ctrl           = soc_camera_s_ctrl,
         .vidioc_cropcap          = soc_camera_cropcap,
         .vidioc_g_crop           = soc_camera_g_crop,
         .vidioc_s_crop           = soc_camera_s_crop,
@@ -1409,6 +1422,7 @@ static int video_dev_create(struct soc_camera_device *icd)
         vdev->ioctl_ops         = &soc_camera_ioctl_ops;
         vdev->release           = video_device_release;
         vdev->tvnorms           = V4L2_STD_UNKNOWN;
+        vdev->ctrl_handler      = &icd->ctrl_handler;
         vdev->lock              = &icd->video_lock;
 
         icd->vdev = vdev;
@@ -1427,11 +1441,6 @@ static int soc_camera_video_start(struct soc_camera_device *icd)
         if (!icd->parent)
                 return -ENODEV;
 
-        if (!icd->ops ||
-            !icd->ops->query_bus_param ||
-            !icd->ops->set_bus_param)
-                return -EINVAL;
-
         ret = video_register_device(icd->vdev, VFL_TYPE_GRABBER, -1);
         if (ret < 0) {
                 dev_err(icd->pdev, "video_register_device failed: %d\n", ret);
diff --git a/drivers/media/video/soc_camera_platform.c b/drivers/media/video/soc_camera_platform.c
index 8069cd6bc5e8..4402a8a74f7a 100644
--- a/drivers/media/video/soc_camera_platform.c
+++ b/drivers/media/video/soc_camera_platform.c
@@ -30,32 +30,12 @@ static struct soc_camera_platform_priv *get_priv(struct platform_device *pdev)
         return container_of(subdev, struct soc_camera_platform_priv, subdev);
 }
 
-static struct soc_camera_platform_info *get_info(struct soc_camera_device *icd)
-{
-        struct platform_device *pdev =
-                to_platform_device(to_soc_camera_control(icd));
-        return pdev->dev.platform_data;
-}
-
 static int soc_camera_platform_s_stream(struct v4l2_subdev *sd, int enable)
 {
         struct soc_camera_platform_info *p = v4l2_get_subdevdata(sd);
         return p->set_capture(p, enable);
 }
 
-static int soc_camera_platform_set_bus_param(struct soc_camera_device *icd,
-                                             unsigned long flags)
-{
-        return 0;
-}
-
-static unsigned long
-soc_camera_platform_query_bus_param(struct soc_camera_device *icd)
-{
-        struct soc_camera_platform_info *p = get_info(icd);
-        return p->bus_param;
-}
-
 static int soc_camera_platform_fill_fmt(struct v4l2_subdev *sd,
                                         struct v4l2_mbus_framefmt *mf)
 {
@@ -115,6 +95,17 @@ static int soc_camera_platform_cropcap(struct v4l2_subdev *sd,
         return 0;
 }
 
+static int soc_camera_platform_g_mbus_config(struct v4l2_subdev *sd,
+                                             struct v4l2_mbus_config *cfg)
+{
+        struct soc_camera_platform_info *p = v4l2_get_subdevdata(sd);
+
+        cfg->flags = p->mbus_param;
+        cfg->type = p->mbus_type;
+
+        return 0;
+}
+
 static struct v4l2_subdev_video_ops platform_subdev_video_ops = {
         .s_stream       = soc_camera_platform_s_stream,
         .enum_mbus_fmt  = soc_camera_platform_enum_fmt,
@@ -123,6 +114,7 @@ static struct v4l2_subdev_video_ops platform_subdev_video_ops = {
         .try_mbus_fmt   = soc_camera_platform_fill_fmt,
         .g_mbus_fmt     = soc_camera_platform_fill_fmt,
         .s_mbus_fmt     = soc_camera_platform_fill_fmt,
+        .g_mbus_config  = soc_camera_platform_g_mbus_config,
 };
 
 static struct v4l2_subdev_ops platform_subdev_ops = {
@@ -130,11 +122,6 @@ static struct v4l2_subdev_ops platform_subdev_ops = {
         .video  = &platform_subdev_video_ops,
 };
 
-static struct soc_camera_ops soc_camera_platform_ops = {
-        .set_bus_param          = soc_camera_platform_set_bus_param,
-        .query_bus_param        = soc_camera_platform_query_bus_param,
-};
-
 static int soc_camera_platform_probe(struct platform_device *pdev)
 {
         struct soc_camera_host *ici;
@@ -163,8 +150,6 @@ static int soc_camera_platform_probe(struct platform_device *pdev)
         /* Set the control device reference */
         icd->control = &pdev->dev;
 
-        icd->ops = &soc_camera_platform_ops;
-
         ici = to_soc_camera_host(icd->parent);
 
         v4l2_subdev_init(&priv->subdev, &platform_subdev_ops);
@@ -178,7 +163,6 @@ static int soc_camera_platform_probe(struct platform_device *pdev)
         return ret;
 
 evdrs:
-        icd->ops = NULL;
         platform_set_drvdata(pdev, NULL);
         kfree(priv);
         return ret;
@@ -187,11 +171,10 @@ evdrs:
 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 = p->icd;
+        struct soc_camera_platform_info *p = v4l2_get_subdevdata(&priv->subdev);
 
+        p->icd->control = NULL;
         v4l2_device_unregister_subdev(&priv->subdev);
-        icd->ops = NULL;
         platform_set_drvdata(pdev, NULL);
         kfree(priv);
         return 0;
diff --git a/drivers/media/video/soc_mediabus.c b/drivers/media/video/soc_mediabus.c
index bea7c9cf4f88..cf7f2194ded4 100644
--- a/drivers/media/video/soc_mediabus.c
+++ b/drivers/media/video/soc_mediabus.c
@@ -383,6 +383,39 @@ const struct soc_mbus_pixelfmt *soc_mbus_get_fmtdesc(
 }
 EXPORT_SYMBOL(soc_mbus_get_fmtdesc);
 
+unsigned int soc_mbus_config_compatible(const struct v4l2_mbus_config *cfg,
+                                        unsigned int flags)
+{
+        unsigned long common_flags;
+        bool hsync = true, vsync = true, pclk, data, mode;
+        bool mipi_lanes, mipi_clock;
+
+        common_flags = cfg->flags & flags;
+
+        switch (cfg->type) {
+        case V4L2_MBUS_PARALLEL:
+                hsync = common_flags & (V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+                                        V4L2_MBUS_HSYNC_ACTIVE_LOW);
+                vsync = common_flags & (V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+                                        V4L2_MBUS_VSYNC_ACTIVE_LOW);
+        case V4L2_MBUS_BT656:
+                pclk = common_flags & (V4L2_MBUS_PCLK_SAMPLE_RISING |
+                                       V4L2_MBUS_PCLK_SAMPLE_FALLING);
+                data = common_flags & (V4L2_MBUS_DATA_ACTIVE_HIGH |
+                                       V4L2_MBUS_DATA_ACTIVE_LOW);
+                mode = common_flags & (V4L2_MBUS_MASTER | V4L2_MBUS_SLAVE);
+                return (!hsync || !vsync || !pclk || !data || !mode) ?
+                        0 : common_flags;
+        case V4L2_MBUS_CSI2:
+                mipi_lanes = common_flags & V4L2_MBUS_CSI2_LANES;
+                mipi_clock = common_flags & (V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK |
+                                             V4L2_MBUS_CSI2_CONTINUOUS_CLOCK);
+                return (!mipi_lanes || !mipi_clock) ? 0 : common_flags;
+        }
+        return 0;
+}
+EXPORT_SYMBOL(soc_mbus_config_compatible);
+
 static int __init soc_mbus_init(void)
 {
         return 0;
diff --git a/drivers/media/video/tw9910.c b/drivers/media/video/tw9910.c
index 742482e30011..a514fa61116c 100644
--- a/drivers/media/video/tw9910.c
+++ b/drivers/media/video/tw9910.c
@@ -22,11 +22,13 @@
 #include <linux/slab.h>
 #include <linux/kernel.h>
 #include <linux/delay.h>
+#include <linux/v4l2-mediabus.h>
 #include <linux/videodev2.h>
-#include <media/v4l2-chip-ident.h>
-#include <media/v4l2-subdev.h>
+
 #include <media/soc_camera.h>
 #include <media/tw9910.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-subdev.h>
 
 #define GET_ID(val)  ((val & 0xF8) >> 3)
 #define GET_REV(val) (val & 0x07)
@@ -203,6 +205,10 @@
 #define RTSEL_FIELD 0x06 /* 0110 = FIELD */
 #define RTSEL_RTCO  0x07 /* 0111 = RTCO ( Real Time Control ) */
 
+/* HSYNC start and end are constant for now */
+#define HSYNC_START     0x0260
+#define HSYNC_END       0x0300
+
 /*
  * structure
  */
@@ -220,22 +226,11 @@ struct tw9910_scale_ctrl {
         u16             vscale;
 };
 
-struct tw9910_cropping_ctrl {
-        u16 vdelay;
-        u16 vactive;
-        u16 hdelay;
-        u16 hactive;
-};
-
-struct tw9910_hsync_ctrl {
-        u16 start;
-        u16 end;
-};
-
 struct tw9910_priv {
         struct v4l2_subdev              subdev;
         struct tw9910_video_info        *info;
         const struct tw9910_scale_ctrl  *scale;
+        v4l2_std_id                     norm;
         u32                             revision;
 };
 
@@ -329,11 +324,6 @@ static const struct tw9910_scale_ctrl tw9910_pal_scales[] = {
         },
 };
 
-static const struct tw9910_hsync_ctrl tw9910_hsync_ctrl = {
-        .start = 0x0260,
-        .end   = 0x0300,
-};
-
 /*
  * general function
  */
@@ -378,21 +368,20 @@ static int tw9910_set_scale(struct i2c_client *client,
         return ret;
 }
 
-static int tw9910_set_hsync(struct i2c_client *client,
-                            const struct tw9910_hsync_ctrl *hsync)
+static int tw9910_set_hsync(struct i2c_client *client)
 {
         struct tw9910_priv *priv = to_tw9910(client);
         int ret;
 
         /* bit 10 - 3 */
         ret = i2c_smbus_write_byte_data(client, HSBEGIN,
-                                        (hsync->start & 0x07F8) >> 3);
+                                        (HSYNC_START & 0x07F8) >> 3);
         if (ret < 0)
                 return ret;
 
         /* bit 10 - 3 */
         ret = i2c_smbus_write_byte_data(client, HSEND,
-                                        (hsync->end & 0x07F8) >> 3);
+                                        (HSYNC_END & 0x07F8) >> 3);
         if (ret < 0)
                 return ret;
 
@@ -400,8 +389,8 @@ static int tw9910_set_hsync(struct i2c_client *client,
         /* bit 2 - 0 */
         if (1 == priv->revision)
                 ret = tw9910_mask_set(client, HSLOWCTL, 0x77,
-                                      (hsync->start & 0x0007) << 4 |
-                                      (hsync->end   & 0x0007));
+                                      (HSYNC_START & 0x0007) << 4 |
+                                      (HSYNC_END   & 0x0007));
 
         return ret;
 }
@@ -433,12 +422,11 @@ static int tw9910_power(struct i2c_client *client, int enable)
         return tw9910_mask_set(client, ACNTL2, ACNTL2_PDN_MASK, acntl2);
 }
 
-static const struct tw9910_scale_ctrl*
-tw9910_select_norm(struct soc_camera_device *icd, u32 width, u32 height)
+static const struct tw9910_scale_ctrl *tw9910_select_norm(v4l2_std_id norm,
+                                                          u32 width, u32 height)
 {
         const struct tw9910_scale_ctrl *scale;
         const struct tw9910_scale_ctrl *ret = NULL;
-        v4l2_std_id norm = icd->vdev->current_norm;
         __u32 diff = 0xffffffff, tmp;
         int size, i;
 
@@ -465,7 +453,7 @@ tw9910_select_norm(struct soc_camera_device *icd, u32 width, u32 height)
 }
 
 /*
- * soc_camera_ops function
+ * subdevice operations
  */
 static int tw9910_s_stream(struct v4l2_subdev *sd, int enable)
 {
@@ -507,49 +495,27 @@ static int tw9910_s_stream(struct v4l2_subdev *sd, int enable)
         return tw9910_power(client, enable);
 }
 
-static int tw9910_set_bus_param(struct soc_camera_device *icd,
-                                unsigned long flags)
+static int tw9910_g_std(struct v4l2_subdev *sd, v4l2_std_id *norm)
 {
-        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
         struct i2c_client *client = v4l2_get_subdevdata(sd);
-        u8 val = VSSL_VVALID | HSSL_DVALID;
+        struct tw9910_priv *priv = to_tw9910(client);
 
-        /*
-         * set OUTCTR1
-         *
-         * We use VVALID and DVALID signals to control VSYNC and HSYNC
-         * outputs, in this mode their polarity is inverted.
-         */
-        if (flags & SOCAM_HSYNC_ACTIVE_LOW)
-                val |= HSP_HI;
+        *norm = priv->norm;
 
-        if (flags & SOCAM_VSYNC_ACTIVE_LOW)
-                val |= VSP_HI;
-
-        return i2c_smbus_write_byte_data(client, OUTCTR1, val);
+        return 0;
 }
 
-static unsigned long tw9910_query_bus_param(struct soc_camera_device *icd)
+static int tw9910_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
 {
-        struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
         struct tw9910_priv *priv = to_tw9910(client);
-        struct soc_camera_link *icl = to_soc_camera_link(icd);
-        unsigned long flags = SOCAM_PCLK_SAMPLE_RISING | SOCAM_MASTER |
-                SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_HIGH |
-                SOCAM_VSYNC_ACTIVE_LOW  | SOCAM_HSYNC_ACTIVE_LOW  |
-                SOCAM_DATA_ACTIVE_HIGH | priv->info->buswidth;
 
-        return soc_camera_apply_sensor_flags(icl, flags);
-}
-
-static int tw9910_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
-{
-        int ret = -EINVAL;
+        if (!(norm & (V4L2_STD_NTSC | V4L2_STD_PAL)))
+                return -EINVAL;
 
-        if (norm & (V4L2_STD_NTSC | V4L2_STD_PAL))
-                ret = 0;
+        priv->norm = norm;
 
-        return ret;
+        return 0;
 }
 
 static int tw9910_g_chip_ident(struct v4l2_subdev *sd,
@@ -600,19 +566,17 @@ static int tw9910_s_register(struct v4l2_subdev *sd,
 }
 #endif
 
-static int tw9910_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+static int tw9910_set_frame(struct v4l2_subdev *sd, u32 *width, u32 *height)
 {
-        struct v4l2_rect *rect = &a->c;
         struct i2c_client *client = v4l2_get_subdevdata(sd);
         struct tw9910_priv *priv = to_tw9910(client);
-        struct soc_camera_device *icd = client->dev.platform_data;
-        int                 ret  = -EINVAL;
-        u8                  val;
+        int ret = -EINVAL;
+        u8 val;
 
         /*
          * select suitable norm
          */
-        priv->scale = tw9910_select_norm(icd, rect->width, rect->height);
+        priv->scale = tw9910_select_norm(priv->norm, *width, *height);
         if (!priv->scale)
                 goto tw9910_set_fmt_error;
 
@@ -670,14 +634,12 @@ static int tw9910_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
         /*
          * set hsync
          */
-        ret = tw9910_set_hsync(client, &tw9910_hsync_ctrl);
+        ret = tw9910_set_hsync(client);
         if (ret < 0)
                 goto tw9910_set_fmt_error;
 
-        rect->width = priv->scale->width;
-        rect->height = priv->scale->height;
-        rect->left = 0;
-        rect->top = 0;
+        *width = priv->scale->width;
+        *height = priv->scale->height;
 
         return ret;
 
@@ -694,25 +656,15 @@ static int tw9910_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
         struct i2c_client *client = v4l2_get_subdevdata(sd);
         struct tw9910_priv *priv = to_tw9910(client);
 
-        if (!priv->scale) {
-                int ret;
-                struct v4l2_crop crop = {
-                        .c = {
-                                .left   = 0,
-                                .top    = 0,
-                                .width  = 640,
-                                .height = 480,
-                        },
-                };
-                ret = tw9910_s_crop(sd, &crop);
-                if (ret < 0)
-                        return ret;
-        }
-
         a->c.left       = 0;
         a->c.top        = 0;
-        a->c.width      = priv->scale->width;
-        a->c.height     = priv->scale->height;
+        if (priv->norm & V4L2_STD_NTSC) {
+                a->c.width      = 640;
+                a->c.height     = 480;
+        } else {
+                a->c.width      = 768;
+                a->c.height     = 576;
+        }
         a->type         = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
         return 0;
@@ -720,14 +672,19 @@ static int tw9910_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
 
 static int tw9910_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
 {
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct tw9910_priv *priv = to_tw9910(client);
+
         a->bounds.left                  = 0;
         a->bounds.top                   = 0;
-        a->bounds.width                 = 768;
-        a->bounds.height                = 576;
-        a->defrect.left                 = 0;
-        a->defrect.top                  = 0;
-        a->defrect.width                = 640;
-        a->defrect.height               = 480;
+        if (priv->norm & V4L2_STD_NTSC) {
+                a->bounds.width         = 640;
+                a->bounds.height        = 480;
+        } else {
+                a->bounds.width         = 768;
+                a->bounds.height        = 576;
+        }
+        a->defrect                      = a->bounds;
         a->type                         = V4L2_BUF_TYPE_VIDEO_CAPTURE;
         a->pixelaspect.numerator        = 1;
         a->pixelaspect.denominator      = 1;
@@ -743,15 +700,8 @@ static int tw9910_g_fmt(struct v4l2_subdev *sd,
 
         if (!priv->scale) {
                 int ret;
-                struct v4l2_crop crop = {
-                        .c = {
-                                .left   = 0,
-                                .top    = 0,
-                                .width  = 640,
-                                .height = 480,
-                        },
-                };
-                ret = tw9910_s_crop(sd, &crop);
+                u32 width = 640, height = 480;
+                ret = tw9910_set_frame(sd, &width, &height);
                 if (ret < 0)
                         return ret;
         }
@@ -768,17 +718,7 @@ static int tw9910_g_fmt(struct v4l2_subdev *sd,
 static int tw9910_s_fmt(struct v4l2_subdev *sd,
                         struct v4l2_mbus_framefmt *mf)
 {
-        struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct tw9910_priv *priv = to_tw9910(client);
-        /* See tw9910_s_crop() - no proper cropping support */
-        struct v4l2_crop a = {
-                .c = {
-                        .left   = 0,
-                        .top    = 0,
-                        .width  = mf->width,
-                        .height = mf->height,
-                },
-        };
+        u32 width = mf->width, height = mf->height;
         int ret;
 
         WARN_ON(mf->field != V4L2_FIELD_ANY &&
@@ -792,10 +732,10 @@ static int tw9910_s_fmt(struct v4l2_subdev *sd,
 
         mf->colorspace = V4L2_COLORSPACE_JPEG;
 
-        ret = tw9910_s_crop(sd, &a);
+        ret = tw9910_set_frame(sd, &width, &height);
         if (!ret) {
-                mf->width       = priv->scale->width;
-                mf->height      = priv->scale->height;
+                mf->width       = width;
+                mf->height      = height;
         }
         return ret;
 }
@@ -804,7 +744,7 @@ static int tw9910_try_fmt(struct v4l2_subdev *sd,
                           struct v4l2_mbus_framefmt *mf)
 {
         struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct soc_camera_device *icd = client->dev.platform_data;
+        struct tw9910_priv *priv = to_tw9910(client);
         const struct tw9910_scale_ctrl *scale;
 
         if (V4L2_FIELD_ANY == mf->field) {
@@ -820,7 +760,7 @@ static int tw9910_try_fmt(struct v4l2_subdev *sd,
         /*
          * select suitable norm
          */
-        scale = tw9910_select_norm(icd, mf->width, mf->height);
+        scale = tw9910_select_norm(priv->norm, mf->width, mf->height);
         if (!scale)
                 return -EINVAL;
 
@@ -830,16 +770,11 @@ static int tw9910_try_fmt(struct v4l2_subdev *sd,
         return 0;
 }
 
-static int tw9910_video_probe(struct soc_camera_device *icd,
-                              struct i2c_client *client)
+static int tw9910_video_probe(struct i2c_client *client)
 {
         struct tw9910_priv *priv = to_tw9910(client);
         s32 id;
 
-        /* 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
          */
@@ -868,20 +803,15 @@ static int tw9910_video_probe(struct soc_camera_device *icd,
         dev_info(&client->dev,
                  "tw9910 Product ID %0x:%0x\n", id, priv->revision);
 
-        icd->vdev->tvnorms      = V4L2_STD_NTSC | V4L2_STD_PAL;
-        icd->vdev->current_norm = V4L2_STD_NTSC;
+        priv->norm = V4L2_STD_NTSC;
 
         return 0;
 }
 
-static struct soc_camera_ops tw9910_ops = {
-        .set_bus_param          = tw9910_set_bus_param,
-        .query_bus_param        = tw9910_query_bus_param,
-};
-
 static struct v4l2_subdev_core_ops tw9910_subdev_core_ops = {
         .g_chip_ident   = tw9910_g_chip_ident,
         .s_std          = tw9910_s_std,
+        .g_std          = tw9910_g_std,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .g_register     = tw9910_g_register,
         .s_register     = tw9910_s_register,
@@ -898,6 +828,45 @@ static int tw9910_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
         return 0;
 }
 
+static int tw9910_g_mbus_config(struct v4l2_subdev *sd,
+                                struct v4l2_mbus_config *cfg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+        cfg->flags = V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_MASTER |
+                V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_LOW |
+                V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_LOW |
+                V4L2_MBUS_DATA_ACTIVE_HIGH;
+        cfg->type = V4L2_MBUS_PARALLEL;
+        cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+        return 0;
+}
+
+static int tw9910_s_mbus_config(struct v4l2_subdev *sd,
+                                const struct v4l2_mbus_config *cfg)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+        u8 val = VSSL_VVALID | HSSL_DVALID;
+        unsigned long flags = soc_camera_apply_board_flags(icl, cfg);
+
+        /*
+         * set OUTCTR1
+         *
+         * We use VVALID and DVALID signals to control VSYNC and HSYNC
+         * outputs, in this mode their polarity is inverted.
+         */
+        if (flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+                val |= HSP_HI;
+
+        if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+                val |= VSP_HI;
+
+        return i2c_smbus_write_byte_data(client, OUTCTR1, val);
+}
+
 static struct v4l2_subdev_video_ops tw9910_subdev_video_ops = {
         .s_stream       = tw9910_s_stream,
         .g_mbus_fmt     = tw9910_g_fmt,
@@ -905,8 +874,9 @@ static struct v4l2_subdev_video_ops tw9910_subdev_video_ops = {
         .try_mbus_fmt   = tw9910_try_fmt,
         .cropcap        = tw9910_cropcap,
         .g_crop         = tw9910_g_crop,
-        .s_crop         = tw9910_s_crop,
         .enum_mbus_fmt  = tw9910_enum_fmt,
+        .g_mbus_config  = tw9910_g_mbus_config,
+        .s_mbus_config  = tw9910_s_mbus_config,
 };
 
 static struct v4l2_subdev_ops tw9910_subdev_ops = {
@@ -922,23 +892,18 @@ static int tw9910_probe(struct i2c_client *client,
                         const struct i2c_device_id *did)
 
 {
-        struct tw9910_priv             *priv;
-        struct tw9910_video_info       *info;
-        struct soc_camera_device       *icd = client->dev.platform_data;
-        struct i2c_adapter             *adapter =
+        struct tw9910_priv              *priv;
+        struct tw9910_video_info        *info;
+        struct i2c_adapter              *adapter =
                 to_i2c_adapter(client->dev.parent);
-        struct soc_camera_link         *icl;
-        int                             ret;
+        struct soc_camera_link          *icl = soc_camera_i2c_to_link(client);
+        int                             ret;
 
-        if (!icd) {
-                dev_err(&client->dev, "TW9910: missing soc-camera data!\n");
+        if (!icl || !icl->priv) {
+                dev_err(&client->dev, "TW9910: missing platform data!\n");
                 return -EINVAL;
         }
 
-        icl = to_soc_camera_link(icd);
-        if (!icl || !icl->priv)
-                return -EINVAL;
-
         info = icl->priv;
 
         if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
@@ -956,14 +921,9 @@ static int tw9910_probe(struct i2c_client *client,
 
         v4l2_i2c_subdev_init(&priv->subdev, client, &tw9910_subdev_ops);
 
-        icd->ops     = &tw9910_ops;
-        icd->iface   = icl->bus_id;
-
-        ret = tw9910_video_probe(icd, client);
-        if (ret) {
-                icd->ops = NULL;
+        ret = tw9910_video_probe(client);
+        if (ret)
                 kfree(priv);
-        }
 
         return ret;
 }
@@ -971,9 +931,7 @@ static int tw9910_probe(struct i2c_client *client,
 static int tw9910_remove(struct i2c_client *client)
 {
         struct tw9910_priv *priv = to_tw9910(client);
-        struct soc_camera_device *icd = client->dev.platform_data;
 
-        icd->ops = NULL;
         kfree(priv);
         return 0;
 }
diff --git a/drivers/media/video/v4l2-compat-ioctl32.c b/drivers/media/video/v4l2-compat-ioctl32.c
index 61979b70f388..c68531b88279 100644
--- a/drivers/media/video/v4l2-compat-ioctl32.c
+++ b/drivers/media/video/v4l2-compat-ioctl32.c
@@ -159,11 +159,25 @@ struct v4l2_format32 {
         } fmt;
 };
 
-static int get_v4l2_format32(struct v4l2_format *kp, struct v4l2_format32 __user *up)
+/**
+ * struct v4l2_create_buffers32 - VIDIOC_CREATE_BUFS32 argument
+ * @index:      on return, index of the first created buffer
+ * @count:      entry: number of requested buffers,
+ *              return: number of created buffers
+ * @memory:     buffer memory type
+ * @format:     frame format, for which buffers are requested
+ * @reserved:   future extensions
+ */
+struct v4l2_create_buffers32 {
+        __u32                   index;
+        __u32                   count;
+        enum v4l2_memory        memory;
+        struct v4l2_format32    format;
+        __u32                   reserved[8];
+};
+
+static int __get_v4l2_format32(struct v4l2_format *kp, struct v4l2_format32 __user *up)
 {
-        if (!access_ok(VERIFY_READ, up, sizeof(struct v4l2_format32)) ||
-                        get_user(kp->type, &up->type))
-                        return -EFAULT;
         switch (kp->type) {
         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
@@ -192,11 +206,24 @@ static int get_v4l2_format32(struct v4l2_format *kp, struct v4l2_format32 __user
         }
 }
 
-static int put_v4l2_format32(struct v4l2_format *kp, struct v4l2_format32 __user *up)
+static int get_v4l2_format32(struct v4l2_format *kp, struct v4l2_format32 __user *up)
+{
+        if (!access_ok(VERIFY_READ, up, sizeof(struct v4l2_format32)) ||
+                        get_user(kp->type, &up->type))
+                        return -EFAULT;
+        return __get_v4l2_format32(kp, up);
+}
+
+static int get_v4l2_create32(struct v4l2_create_buffers *kp, struct v4l2_create_buffers32 __user *up)
+{
+        if (!access_ok(VERIFY_READ, up, sizeof(struct v4l2_create_buffers32)) ||
+            copy_from_user(kp, up, offsetof(struct v4l2_create_buffers32, format.fmt)))
+                        return -EFAULT;
+        return __get_v4l2_format32(&kp->format, &up->format);
+}
+
+static int __put_v4l2_format32(struct v4l2_format *kp, struct v4l2_format32 __user *up)
 {
-        if (!access_ok(VERIFY_WRITE, up, sizeof(struct v4l2_format32)) ||
-                put_user(kp->type, &up->type))
-                return -EFAULT;
         switch (kp->type) {
         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
@@ -225,6 +252,22 @@ static int put_v4l2_format32(struct v4l2_format *kp, struct v4l2_format32 __user
         }
 }
 
+static int put_v4l2_format32(struct v4l2_format *kp, struct v4l2_format32 __user *up)
+{
+        if (!access_ok(VERIFY_WRITE, up, sizeof(struct v4l2_format32)) ||
+                put_user(kp->type, &up->type))
+                return -EFAULT;
+        return __put_v4l2_format32(kp, up);
+}
+
+static int put_v4l2_create32(struct v4l2_create_buffers *kp, struct v4l2_create_buffers32 __user *up)
+{
+        if (!access_ok(VERIFY_WRITE, up, sizeof(struct v4l2_create_buffers32)) ||
+            copy_to_user(up, kp, offsetof(struct v4l2_create_buffers32, format.fmt)))
+                        return -EFAULT;
+        return __put_v4l2_format32(&kp->format, &up->format);
+}
+
 struct v4l2_standard32 {
         __u32                index;
         __u32                id[2]; /* __u64 would get the alignment wrong */
@@ -702,6 +745,8 @@ static int put_v4l2_event32(struct v4l2_event *kp, struct v4l2_event32 __user *u
 #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_CREATE_BUFS32    _IOWR('V', 92, struct v4l2_create_buffers32)
+#define VIDIOC_PREPARE_BUF32    _IOWR('V', 93, struct v4l2_buffer32)
 
 #define VIDIOC_OVERLAY32        _IOW ('V', 14, s32)
 #define VIDIOC_STREAMON32       _IOW ('V', 18, s32)
@@ -721,6 +766,7 @@ static long do_video_ioctl(struct file *file, unsigned int cmd, unsigned long ar
                 struct v4l2_standard v2s;
                 struct v4l2_ext_controls v2ecs;
                 struct v4l2_event v2ev;
+                struct v4l2_create_buffers v2crt;
                 unsigned long vx;
                 int vi;
         } karg;
@@ -751,6 +797,8 @@ static long do_video_ioctl(struct file *file, unsigned int cmd, unsigned long ar
         case VIDIOC_S_INPUT32: cmd = VIDIOC_S_INPUT; break;
         case VIDIOC_G_OUTPUT32: cmd = VIDIOC_G_OUTPUT; break;
         case VIDIOC_S_OUTPUT32: cmd = VIDIOC_S_OUTPUT; break;
+        case VIDIOC_CREATE_BUFS32: cmd = VIDIOC_CREATE_BUFS; break;
+        case VIDIOC_PREPARE_BUF32: cmd = VIDIOC_PREPARE_BUF; break;
         }
 
         switch (cmd) {
@@ -775,6 +823,12 @@ static long do_video_ioctl(struct file *file, unsigned int cmd, unsigned long ar
                 compatible_arg = 0;
                 break;
 
+        case VIDIOC_CREATE_BUFS:
+                err = get_v4l2_create32(&karg.v2crt, up);
+                compatible_arg = 0;
+                break;
+
+        case VIDIOC_PREPARE_BUF:
         case VIDIOC_QUERYBUF:
         case VIDIOC_QBUF:
         case VIDIOC_DQBUF:
@@ -860,6 +914,10 @@ static long do_video_ioctl(struct file *file, unsigned int cmd, unsigned long ar
                 err = put_v4l2_format32(&karg.v2f, up);
                 break;
 
+        case VIDIOC_CREATE_BUFS:
+                err = put_v4l2_create32(&karg.v2crt, up);
+                break;
+
         case VIDIOC_QUERYBUF:
         case VIDIOC_QBUF:
         case VIDIOC_DQBUF:
@@ -959,6 +1017,8 @@ long v4l2_compat_ioctl32(struct file *file, unsigned int cmd, unsigned long arg)
         case VIDIOC_DQEVENT32:
         case VIDIOC_SUBSCRIBE_EVENT:
         case VIDIOC_UNSUBSCRIBE_EVENT:
+        case VIDIOC_CREATE_BUFS32:
+        case VIDIOC_PREPARE_BUF32:
                 ret = do_video_ioctl(file, cmd, arg);
                 break;
 
diff --git a/drivers/media/video/v4l2-ctrls.c b/drivers/media/video/v4l2-ctrls.c
index fc8666ae408f..5552f8137571 100644
--- a/drivers/media/video/v4l2-ctrls.c
+++ b/drivers/media/video/v4l2-ctrls.c
@@ -210,6 +210,7 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
                 "Disabled",
                 "50 Hz",
                 "60 Hz",
+                "Auto",
                 NULL
         };
         static const char * const camera_exposure_auto[] = {
diff --git a/drivers/media/video/v4l2-device.c b/drivers/media/video/v4l2-device.c
index e6a2c3b302d4..9fc0ae8a526a 100644
--- a/drivers/media/video/v4l2-device.c
+++ b/drivers/media/video/v4l2-device.c
@@ -21,6 +21,7 @@
 #include <linux/types.h>
 #include <linux/ioctl.h>
 #include <linux/i2c.h>
+#include <linux/slab.h>
 #if defined(CONFIG_SPI)
 #include <linux/spi/spi.h>
 #endif
@@ -193,6 +194,13 @@ int v4l2_device_register_subdev(struct v4l2_device *v4l2_dev,
 }
 EXPORT_SYMBOL_GPL(v4l2_device_register_subdev);
 
+static void v4l2_device_release_subdev_node(struct video_device *vdev)
+{
+        struct v4l2_subdev *sd = video_get_drvdata(vdev);
+        sd->devnode = NULL;
+        kfree(vdev);
+}
+
 int v4l2_device_register_subdev_nodes(struct v4l2_device *v4l2_dev)
 {
         struct video_device *vdev;
@@ -206,22 +214,40 @@ int v4l2_device_register_subdev_nodes(struct v4l2_device *v4l2_dev)
                 if (!(sd->flags & V4L2_SUBDEV_FL_HAS_DEVNODE))
                         continue;
 
-                vdev = &sd->devnode;
+                vdev = kzalloc(sizeof(*vdev), GFP_KERNEL);
+                if (!vdev) {
+                        err = -ENOMEM;
+                        goto clean_up;
+                }
+
+                video_set_drvdata(vdev, sd);
                 strlcpy(vdev->name, sd->name, sizeof(vdev->name));
                 vdev->v4l2_dev = v4l2_dev;
                 vdev->fops = &v4l2_subdev_fops;
-                vdev->release = video_device_release_empty;
+                vdev->release = v4l2_device_release_subdev_node;
                 vdev->ctrl_handler = sd->ctrl_handler;
                 err = __video_register_device(vdev, VFL_TYPE_SUBDEV, -1, 1,
                                               sd->owner);
-                if (err < 0)
-                        return err;
+                if (err < 0) {
+                        kfree(vdev);
+                        goto clean_up;
+                }
 #if defined(CONFIG_MEDIA_CONTROLLER)
                 sd->entity.v4l.major = VIDEO_MAJOR;
                 sd->entity.v4l.minor = vdev->minor;
 #endif
+                sd->devnode = vdev;
         }
         return 0;
+
+clean_up:
+        list_for_each_entry(sd, &v4l2_dev->subdevs, list) {
+                if (!sd->devnode)
+                        break;
+                video_unregister_device(sd->devnode);
+        }
+
+        return err;
 }
 EXPORT_SYMBOL_GPL(v4l2_device_register_subdev_nodes);
 
@@ -247,7 +273,7 @@ void v4l2_device_unregister_subdev(struct v4l2_subdev *sd)
         if (v4l2_dev->mdev)
                 media_device_unregister_entity(&sd->entity);
 #endif
-        video_unregister_device(&sd->devnode);
+        video_unregister_device(sd->devnode);
         module_put(sd->owner);
 }
 EXPORT_SYMBOL_GPL(v4l2_device_unregister_subdev);
diff --git a/drivers/media/video/v4l2-ioctl.c b/drivers/media/video/v4l2-ioctl.c
index 24fd43322150..e1da8fc9dd2f 100644
--- a/drivers/media/video/v4l2-ioctl.c
+++ b/drivers/media/video/v4l2-ioctl.c
@@ -273,6 +273,8 @@ static const char *v4l2_ioctls[] = {
         [_IOC_NR(VIDIOC_DQEVENT)]          = "VIDIOC_DQEVENT",
         [_IOC_NR(VIDIOC_SUBSCRIBE_EVENT)]  = "VIDIOC_SUBSCRIBE_EVENT",
         [_IOC_NR(VIDIOC_UNSUBSCRIBE_EVENT)] = "VIDIOC_UNSUBSCRIBE_EVENT",
+        [_IOC_NR(VIDIOC_CREATE_BUFS)]      = "VIDIOC_CREATE_BUFS",
+        [_IOC_NR(VIDIOC_PREPARE_BUF)]      = "VIDIOC_PREPARE_BUF",
 };
 #define V4L2_IOCTLS ARRAY_SIZE(v4l2_ioctls)
 
@@ -2104,6 +2106,40 @@ static long __video_do_ioctl(struct file *file,
                 dbgarg(cmd, "type=0x%8.8x", sub->type);
                 break;
         }
+        case VIDIOC_CREATE_BUFS:
+        {
+                struct v4l2_create_buffers *create = arg;
+
+                if (!ops->vidioc_create_bufs)
+                        break;
+                if (ret_prio) {
+                        ret = ret_prio;
+                        break;
+                }
+                ret = check_fmt(ops, create->format.type);
+                if (ret)
+                        break;
+
+                ret = ops->vidioc_create_bufs(file, fh, create);
+
+                dbgarg(cmd, "count=%d @ %d\n", create->count, create->index);
+                break;
+        }
+        case VIDIOC_PREPARE_BUF:
+        {
+                struct v4l2_buffer *b = arg;
+
+                if (!ops->vidioc_prepare_buf)
+                        break;
+                ret = check_fmt(ops, b->type);
+                if (ret)
+                        break;
+
+                ret = ops->vidioc_prepare_buf(file, fh, b);
+
+                dbgarg(cmd, "index=%d", b->index);
+                break;
+        }
         default:
                 if (!ops->vidioc_default)
                         break;
diff --git a/drivers/media/video/videobuf2-core.c b/drivers/media/video/videobuf2-core.c
index 3f5c7a38e6e8..979e544388cb 100644
--- a/drivers/media/video/videobuf2-core.c
+++ b/drivers/media/video/videobuf2-core.c
@@ -38,7 +38,8 @@ module_param(debug, int, 0644);
         (((q)->ops->op) ? ((q)->ops->op(args)) : 0)
 
 #define V4L2_BUFFER_STATE_FLAGS (V4L2_BUF_FLAG_MAPPED | V4L2_BUF_FLAG_QUEUED | \
-                                 V4L2_BUF_FLAG_DONE | V4L2_BUF_FLAG_ERROR)
+                                 V4L2_BUF_FLAG_DONE | V4L2_BUF_FLAG_ERROR | \
+                                 V4L2_BUF_FLAG_PREPARED)
 
 /**
  * __vb2_buf_mem_alloc() - allocate video memory for the given buffer
@@ -109,13 +110,22 @@ static void __vb2_buf_userptr_put(struct vb2_buffer *vb)
  * __setup_offsets() - setup unique offsets ("cookies") for every plane in
  * every buffer on the queue
  */
-static void __setup_offsets(struct vb2_queue *q)
+static void __setup_offsets(struct vb2_queue *q, unsigned int n)
 {
         unsigned int buffer, plane;
         struct vb2_buffer *vb;
-        unsigned long off = 0;
+        unsigned long off;
 
-        for (buffer = 0; buffer < q->num_buffers; ++buffer) {
+        if (q->num_buffers) {
+                struct v4l2_plane *p;
+                vb = q->bufs[q->num_buffers - 1];
+                p = &vb->v4l2_planes[vb->num_planes - 1];
+                off = PAGE_ALIGN(p->m.mem_offset + p->length);
+        } else {
+                off = 0;
+        }
+
+        for (buffer = q->num_buffers; buffer < q->num_buffers + n; ++buffer) {
                 vb = q->bufs[buffer];
                 if (!vb)
                         continue;
@@ -161,7 +171,7 @@ static int __vb2_queue_alloc(struct vb2_queue *q, enum v4l2_memory memory,
                 vb->state = VB2_BUF_STATE_DEQUEUED;
                 vb->vb2_queue = q;
                 vb->num_planes = num_planes;
-                vb->v4l2_buf.index = buffer;
+                vb->v4l2_buf.index = q->num_buffers + buffer;
                 vb->v4l2_buf.type = q->type;
                 vb->v4l2_buf.memory = memory;
 
@@ -189,15 +199,13 @@ static int __vb2_queue_alloc(struct vb2_queue *q, enum v4l2_memory memory,
                         }
                 }
 
-                q->bufs[buffer] = vb;
+                q->bufs[q->num_buffers + buffer] = vb;
         }
 
-        q->num_buffers = buffer;
-
-        __setup_offsets(q);
+        __setup_offsets(q, buffer);
 
         dprintk(1, "Allocated %d buffers, %d plane(s) each\n",
-                        q->num_buffers, num_planes);
+                        buffer, num_planes);
 
         return buffer;
 }
@@ -205,12 +213,13 @@ static int __vb2_queue_alloc(struct vb2_queue *q, enum v4l2_memory memory,
 /**
  * __vb2_free_mem() - release all video buffer memory for a given queue
  */
-static void __vb2_free_mem(struct vb2_queue *q)
+static void __vb2_free_mem(struct vb2_queue *q, unsigned int buffers)
 {
         unsigned int buffer;
         struct vb2_buffer *vb;
 
-        for (buffer = 0; buffer < q->num_buffers; ++buffer) {
+        for (buffer = q->num_buffers - buffers; buffer < q->num_buffers;
+             ++buffer) {
                 vb = q->bufs[buffer];
                 if (!vb)
                         continue;
@@ -224,17 +233,18 @@ static void __vb2_free_mem(struct vb2_queue *q)
 }
 
 /**
- * __vb2_queue_free() - free the queue - video memory and related information
- * and return the queue to an uninitialized state. Might be called even if the
- * queue has already been freed.
+ * __vb2_queue_free() - free buffers at the end of the queue - video memory and
+ * related information, if no buffers are left return the queue to an
+ * uninitialized state. Might be called even if the queue has already been freed.
  */
-static void __vb2_queue_free(struct vb2_queue *q)
+static void __vb2_queue_free(struct vb2_queue *q, unsigned int buffers)
 {
         unsigned int buffer;
 
         /* Call driver-provided cleanup function for each buffer, if provided */
         if (q->ops->buf_cleanup) {
-                for (buffer = 0; buffer < q->num_buffers; ++buffer) {
+                for (buffer = q->num_buffers - buffers; buffer < q->num_buffers;
+                     ++buffer) {
                         if (NULL == q->bufs[buffer])
                                 continue;
                         q->ops->buf_cleanup(q->bufs[buffer]);
@@ -242,23 +252,25 @@ static void __vb2_queue_free(struct vb2_queue *q)
         }
 
         /* Release video buffer memory */
-        __vb2_free_mem(q);
+        __vb2_free_mem(q, buffers);
 
         /* Free videobuf buffers */
-        for (buffer = 0; buffer < q->num_buffers; ++buffer) {
+        for (buffer = q->num_buffers - buffers; buffer < q->num_buffers;
+             ++buffer) {
                 kfree(q->bufs[buffer]);
                 q->bufs[buffer] = NULL;
         }
 
-        q->num_buffers = 0;
-        q->memory = 0;
+        q->num_buffers -= buffers;
+        if (!q->num_buffers)
+                q->memory = 0;
 }
 
 /**
  * __verify_planes_array() - verify that the planes array passed in struct
  * v4l2_buffer from userspace can be safely used
  */
-static int __verify_planes_array(struct vb2_buffer *vb, struct v4l2_buffer *b)
+static int __verify_planes_array(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 {
         /* Is memory for copying plane information present? */
         if (NULL == b->m.planes) {
@@ -318,7 +330,7 @@ static bool __buffers_in_use(struct vb2_queue *q)
 static int __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
 {
         struct vb2_queue *q = vb->vb2_queue;
-        int ret = 0;
+        int ret;
 
         /* Copy back data such as timestamp, flags, input, etc. */
         memcpy(b, &vb->v4l2_buf, offsetof(struct v4l2_buffer, m));
@@ -365,6 +377,9 @@ static int __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
         case VB2_BUF_STATE_DONE:
                 b->flags |= V4L2_BUF_FLAG_DONE;
                 break;
+        case VB2_BUF_STATE_PREPARED:
+                b->flags |= V4L2_BUF_FLAG_PREPARED;
+                break;
         case VB2_BUF_STATE_DEQUEUED:
                 /* nothing */
                 break;
@@ -373,7 +388,7 @@ static int __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
         if (__buffer_in_use(q, vb))
                 b->flags |= V4L2_BUF_FLAG_MAPPED;
 
-        return ret;
+        return 0;
 }
 
 /**
@@ -459,7 +474,7 @@ static int __verify_mmap_ops(struct vb2_queue *q)
  */
 int vb2_reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
 {
-        unsigned int num_buffers, num_planes;
+        unsigned int num_buffers, allocated_buffers, num_planes = 0;
         int ret = 0;
 
         if (q->fileio) {
@@ -507,7 +522,7 @@ int vb2_reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
                         return -EBUSY;
                 }
 
-                __vb2_queue_free(q);
+                __vb2_queue_free(q, q->num_buffers);
 
                 /*
                  * In case of REQBUFS(0) return immediately without calling
@@ -529,7 +544,7 @@ int vb2_reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
          * Ask the driver how many buffers and planes per buffer it requires.
          * Driver also sets the size and allocator context for each plane.
          */
-        ret = call_qop(q, queue_setup, q, &num_buffers, &num_planes,
+        ret = call_qop(q, queue_setup, q, NULL, &num_buffers, &num_planes,
                        q->plane_sizes, q->alloc_ctx);
         if (ret)
                 return ret;
@@ -541,44 +556,168 @@ int vb2_reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
                 return -ENOMEM;
         }
 
+        allocated_buffers = ret;
+
         /*
          * Check if driver can handle the allocated number of buffers.
          */
-        if (ret < num_buffers) {
-                unsigned int orig_num_buffers;
+        if (allocated_buffers < num_buffers) {
+                num_buffers = allocated_buffers;
 
-                orig_num_buffers = num_buffers = ret;
-                ret = call_qop(q, queue_setup, q, &num_buffers, &num_planes,
-                               q->plane_sizes, q->alloc_ctx);
-                if (ret)
-                        goto free_mem;
+                ret = call_qop(q, queue_setup, q, NULL, &num_buffers,
+                               &num_planes, q->plane_sizes, q->alloc_ctx);
 
-                if (orig_num_buffers < num_buffers) {
+                if (!ret && allocated_buffers < num_buffers)
                         ret = -ENOMEM;
-                        goto free_mem;
-                }
 
                 /*
-                 * Ok, driver accepted smaller number of buffers.
+                 * Either the driver has accepted a smaller number of buffers,
+                 * or .queue_setup() returned an error
                  */
-                ret = num_buffers;
+        }
+
+        q->num_buffers = allocated_buffers;
+
+        if (ret < 0) {
+                __vb2_queue_free(q, allocated_buffers);
+                return ret;
         }
 
         /*
          * Return the number of successfully allocated buffers
          * to the userspace.
          */
-        req->count = ret;
+        req->count = allocated_buffers;
 
         return 0;
-
-free_mem:
-        __vb2_queue_free(q);
-        return ret;
 }
 EXPORT_SYMBOL_GPL(vb2_reqbufs);
 
 /**
+ * vb2_create_bufs() - Allocate buffers and any required auxiliary structs
+ * @q:          videobuf2 queue
+ * @create:     creation parameters, passed from userspace to vidioc_create_bufs
+ *              handler in driver
+ *
+ * Should be called from vidioc_create_bufs ioctl handler of a driver.
+ * This function:
+ * 1) verifies parameter sanity
+ * 2) calls the .queue_setup() queue operation
+ * 3) performs any necessary memory allocations
+ *
+ * The return values from this function are intended to be directly returned
+ * from vidioc_create_bufs handler in driver.
+ */
+int vb2_create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create)
+{
+        unsigned int num_planes = 0, num_buffers, allocated_buffers;
+        int ret = 0;
+
+        if (q->fileio) {
+                dprintk(1, "%s(): file io in progress\n", __func__);
+                return -EBUSY;
+        }
+
+        if (create->memory != V4L2_MEMORY_MMAP
+                        && create->memory != V4L2_MEMORY_USERPTR) {
+                dprintk(1, "%s(): unsupported memory type\n", __func__);
+                return -EINVAL;
+        }
+
+        if (create->format.type != q->type) {
+                dprintk(1, "%s(): requested type is incorrect\n", __func__);
+                return -EINVAL;
+        }
+
+        /*
+         * Make sure all the required memory ops for given memory type
+         * are available.
+         */
+        if (create->memory == V4L2_MEMORY_MMAP && __verify_mmap_ops(q)) {
+                dprintk(1, "%s(): MMAP for current setup unsupported\n", __func__);
+                return -EINVAL;
+        }
+
+        if (create->memory == V4L2_MEMORY_USERPTR && __verify_userptr_ops(q)) {
+                dprintk(1, "%s(): USERPTR for current setup unsupported\n", __func__);
+                return -EINVAL;
+        }
+
+        if (q->num_buffers == VIDEO_MAX_FRAME) {
+                dprintk(1, "%s(): maximum number of buffers already allocated\n",
+                        __func__);
+                return -ENOBUFS;
+        }
+
+        create->index = q->num_buffers;
+
+        if (!q->num_buffers) {
+                memset(q->plane_sizes, 0, sizeof(q->plane_sizes));
+                memset(q->alloc_ctx, 0, sizeof(q->alloc_ctx));
+                q->memory = create->memory;
+        }
+
+        num_buffers = min(create->count, VIDEO_MAX_FRAME - q->num_buffers);
+
+        /*
+         * Ask the driver, whether the requested number of buffers, planes per
+         * buffer and their sizes are acceptable
+         */
+        ret = call_qop(q, queue_setup, q, &create->format, &num_buffers,
+                       &num_planes, q->plane_sizes, q->alloc_ctx);
+        if (ret)
+                return ret;
+
+        /* Finally, allocate buffers and video memory */
+        ret = __vb2_queue_alloc(q, create->memory, num_buffers,
+                                num_planes);
+        if (ret < 0) {
+                dprintk(1, "Memory allocation failed with error: %d\n", ret);
+                return ret;
+        }
+
+        allocated_buffers = ret;
+
+        /*
+         * Check if driver can handle the so far allocated number of buffers.
+         */
+        if (ret < num_buffers) {
+                num_buffers = ret;
+
+                /*
+                 * q->num_buffers contains the total number of buffers, that the
+                 * queue driver has set up
+                 */
+                ret = call_qop(q, queue_setup, q, &create->format, &num_buffers,
+                               &num_planes, q->plane_sizes, q->alloc_ctx);
+
+                if (!ret && allocated_buffers < num_buffers)
+                        ret = -ENOMEM;
+
+                /*
+                 * Either the driver has accepted a smaller number of buffers,
+                 * or .queue_setup() returned an error
+                 */
+        }
+
+        q->num_buffers += allocated_buffers;
+
+        if (ret < 0) {
+                __vb2_queue_free(q, allocated_buffers);
+                return ret;
+        }
+
+        /*
+         * Return the number of successfully allocated buffers
+         * to the userspace.
+         */
+        create->count = allocated_buffers;
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(vb2_create_bufs);
+
+/**
  * vb2_plane_vaddr() - Return a kernel virtual address of a given plane
  * @vb:         vb2_buffer to which the plane in question belongs to
  * @plane_no:   plane number for which the address is to be returned
@@ -662,7 +801,7 @@ EXPORT_SYMBOL_GPL(vb2_buffer_done);
  * __fill_vb2_buffer() - fill a vb2_buffer with information provided in
  * a v4l2_buffer by the userspace
  */
-static int __fill_vb2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b,
+static int __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b,
                                 struct v4l2_plane *v4l2_planes)
 {
         unsigned int plane;
@@ -726,7 +865,7 @@ static int __fill_vb2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b,
 /**
  * __qbuf_userptr() - handle qbuf of a USERPTR buffer
  */
-static int __qbuf_userptr(struct vb2_buffer *vb, struct v4l2_buffer *b)
+static int __qbuf_userptr(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 {
         struct v4l2_plane planes[VIDEO_MAX_PLANES];
         struct vb2_queue *q = vb->vb2_queue;
@@ -815,7 +954,7 @@ err:
 /**
  * __qbuf_mmap() - handle qbuf of an MMAP buffer
  */
-static int __qbuf_mmap(struct vb2_buffer *vb, struct v4l2_buffer *b)
+static int __qbuf_mmap(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 {
         return __fill_vb2_buffer(vb, b, vb->v4l2_planes);
 }
@@ -832,6 +971,95 @@ static void __enqueue_in_driver(struct vb2_buffer *vb)
         q->ops->buf_queue(vb);
 }
 
+static int __buf_prepare(struct vb2_buffer *vb, const struct v4l2_buffer *b)
+{
+        struct vb2_queue *q = vb->vb2_queue;
+        int ret;
+
+        switch (q->memory) {
+        case V4L2_MEMORY_MMAP:
+                ret = __qbuf_mmap(vb, b);
+                break;
+        case V4L2_MEMORY_USERPTR:
+                ret = __qbuf_userptr(vb, b);
+                break;
+        default:
+                WARN(1, "Invalid queue type\n");
+                ret = -EINVAL;
+        }
+
+        if (!ret)
+                ret = call_qop(q, buf_prepare, vb);
+        if (ret)
+                dprintk(1, "qbuf: buffer preparation failed: %d\n", ret);
+        else
+                vb->state = VB2_BUF_STATE_PREPARED;
+
+        return ret;
+}
+
+/**
+ * vb2_prepare_buf() - Pass ownership of a buffer from userspace to the kernel
+ * @q:          videobuf2 queue
+ * @b:          buffer structure passed from userspace to vidioc_prepare_buf
+ *              handler in driver
+ *
+ * Should be called from vidioc_prepare_buf ioctl handler of a driver.
+ * This function:
+ * 1) verifies the passed buffer,
+ * 2) calls buf_prepare callback in the driver (if provided), in which
+ *    driver-specific buffer initialization can be performed,
+ *
+ * The return values from this function are intended to be directly returned
+ * from vidioc_prepare_buf handler in driver.
+ */
+int vb2_prepare_buf(struct vb2_queue *q, struct v4l2_buffer *b)
+{
+        struct vb2_buffer *vb;
+        int ret;
+
+        if (q->fileio) {
+                dprintk(1, "%s(): file io in progress\n", __func__);
+                return -EBUSY;
+        }
+
+        if (b->type != q->type) {
+                dprintk(1, "%s(): invalid buffer type\n", __func__);
+                return -EINVAL;
+        }
+
+        if (b->index >= q->num_buffers) {
+                dprintk(1, "%s(): buffer index out of range\n", __func__);
+                return -EINVAL;
+        }
+
+        vb = q->bufs[b->index];
+        if (NULL == vb) {
+                /* Should never happen */
+                dprintk(1, "%s(): buffer is NULL\n", __func__);
+                return -EINVAL;
+        }
+
+        if (b->memory != q->memory) {
+                dprintk(1, "%s(): invalid memory type\n", __func__);
+                return -EINVAL;
+        }
+
+        if (vb->state != VB2_BUF_STATE_DEQUEUED) {
+                dprintk(1, "%s(): invalid buffer state %d\n", __func__, vb->state);
+                return -EINVAL;
+        }
+
+        ret = __buf_prepare(vb, b);
+        if (ret < 0)
+                return ret;
+
+        __fill_v4l2_buffer(vb, b);
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(vb2_prepare_buf);
+
 /**
  * vb2_qbuf() - Queue a buffer from userspace
  * @q:          videobuf2 queue
@@ -841,8 +1069,8 @@ static void __enqueue_in_driver(struct vb2_buffer *vb)
  * Should be called from vidioc_qbuf ioctl handler of a driver.
  * This function:
  * 1) verifies the passed buffer,
- * 2) calls buf_prepare callback in the driver (if provided), in which
- *    driver-specific buffer initialization can be performed,
+ * 2) if necessary, calls buf_prepare callback in the driver (if provided), in
+ *    which driver-specific buffer initialization can be performed,
  * 3) if streaming is on, queues the buffer in driver by the means of buf_queue
  *    callback for processing.
  *
@@ -852,7 +1080,7 @@ static void __enqueue_in_driver(struct vb2_buffer *vb)
 int vb2_qbuf(struct vb2_queue *q, struct v4l2_buffer *b)
 {
         struct vb2_buffer *vb;
-        int ret = 0;
+        int ret;
 
         if (q->fileio) {
                 dprintk(1, "qbuf: file io in progress\n");
@@ -881,29 +1109,18 @@ int vb2_qbuf(struct vb2_queue *q, struct v4l2_buffer *b)
                 return -EINVAL;
         }
 
-        if (vb->state != VB2_BUF_STATE_DEQUEUED) {
+        switch (vb->state) {
+        case VB2_BUF_STATE_DEQUEUED:
+                ret = __buf_prepare(vb, b);
+                if (ret)
+                        return ret;
+        case VB2_BUF_STATE_PREPARED:
+                break;
+        default:
                 dprintk(1, "qbuf: buffer already in use\n");
                 return -EINVAL;
         }
 
-        if (q->memory == V4L2_MEMORY_MMAP)
-                ret = __qbuf_mmap(vb, b);
-        else if (q->memory == V4L2_MEMORY_USERPTR)
-                ret = __qbuf_userptr(vb, b);
-        else {
-                WARN(1, "Invalid queue type\n");
-                return -EINVAL;
-        }
-
-        if (ret)
-                return ret;
-
-        ret = call_qop(q, buf_prepare, vb);
-        if (ret) {
-                dprintk(1, "qbuf: buffer preparation failed\n");
-                return ret;
-        }
-
         /*
          * Add to the queued buffers list, a buffer will stay on it until
          * dequeued in dqbuf.
@@ -918,6 +1135,9 @@ int vb2_qbuf(struct vb2_queue *q, struct v4l2_buffer *b)
         if (q->streaming)
                 __enqueue_in_driver(vb);
 
+        /* Fill buffer information for the userspace */
+        __fill_v4l2_buffer(vb, b);
+
         dprintk(1, "qbuf of buffer %d succeeded\n", vb->v4l2_buf.index);
         return 0;
 }
@@ -1347,6 +1567,37 @@ int vb2_mmap(struct vb2_queue *q, struct vm_area_struct *vma)
 }
 EXPORT_SYMBOL_GPL(vb2_mmap);
 
+#ifndef CONFIG_MMU
+unsigned long vb2_get_unmapped_area(struct vb2_queue *q,
+                                    unsigned long addr,
+                                    unsigned long len,
+                                    unsigned long pgoff,
+                                    unsigned long flags)
+{
+        unsigned long off = pgoff << PAGE_SHIFT;
+        struct vb2_buffer *vb;
+        unsigned int buffer, plane;
+        int ret;
+
+        if (q->memory != V4L2_MEMORY_MMAP) {
+                dprintk(1, "Queue is not currently set up for mmap\n");
+                return -EINVAL;
+        }
+
+        /*
+         * Find the plane corresponding to the offset passed by userspace.
+         */
+        ret = __find_plane_by_offset(q, off, &buffer, &plane);
+        if (ret)
+                return ret;
+
+        vb = q->bufs[buffer];
+
+        return (unsigned long)vb2_plane_vaddr(vb, plane);
+}
+EXPORT_SYMBOL_GPL(vb2_get_unmapped_area);
+#endif
+
 static int __vb2_init_fileio(struct vb2_queue *q, int read);
 static int __vb2_cleanup_fileio(struct vb2_queue *q);
 
@@ -1464,7 +1715,7 @@ void vb2_queue_release(struct vb2_queue *q)
 {
         __vb2_cleanup_fileio(q);
         __vb2_queue_cancel(q);
-        __vb2_queue_free(q);
+        __vb2_queue_free(q, q->num_buffers);
 }
 EXPORT_SYMBOL_GPL(vb2_queue_release);
 
diff --git a/drivers/media/video/vivi.c b/drivers/media/video/vivi.c
index 7cf94c09d99a..7d754fbcccbf 100644
--- a/drivers/media/video/vivi.c
+++ b/drivers/media/video/vivi.c
@@ -650,9 +650,9 @@ static void vivi_stop_generating(struct vivi_dev *dev)
 /* ------------------------------------------------------------------
         Videobuf operations
    ------------------------------------------------------------------*/
-static int queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
-                                unsigned int *nplanes, unsigned int sizes[],
-                                void *alloc_ctxs[])
+static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
+                                unsigned int *nbuffers, unsigned int *nplanes,
+                                unsigned int sizes[], void *alloc_ctxs[])
 {
         struct vivi_dev *dev = vb2_get_drv_priv(vq);
         unsigned long size;