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 cf7214edf65..38019bafb86 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 7d0710bb197..26c8b9e5705 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 2ad33ba92ba..2d08c9b5128 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 f027a2c1c3e..c4622618714 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 00000000000..d1f58371c71
--- /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 00000000000..432706ae527
--- /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 546ba5915a5..b5c98da5d9e 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 5a2c7bb386c..364d89f826b 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 89873615e68..13ebeffb705 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 95ddcc4845d..db20904d01f 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 d285c8c9281..b303a3f8a9f 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 11fff97e719..117f9c4b4cb 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 774715d2f84..8c775c59e12 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 9e2f870f425..c6ff32a6137 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 00000000000..5f6b5421371
--- /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 00000000000..aedde18c68f
--- /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 00000000000..09e99de5fd2
--- /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 00000000000..c20d6dece15
--- /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 00000000000..af2ae7c5815
--- /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 00000000000..8eb55b7b88c
--- /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 00000000000..9326a7c729e
--- /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 00000000000..6ace60313b4
--- /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 00000000000..a7fa38f9594
--- /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 00000000000..29e43b03c85
--- /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 00000000000..4d3d0ce4078
--- /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 00000000000..60d197f5755
--- /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 00000000000..1c1c228352d
--- /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 00000000000..fc780d0908d
--- /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 00000000000..6175e096185
--- /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 00000000000..a3fc25a4dc0
--- /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 00000000000..5f05d153bc4
--- /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 00000000000..2a724ddfa53
--- /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 00000000000..d42dab59b66
--- /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 00000000000..c0b80068f46
--- /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 00000000000..268ec8aa6a6
--- /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 00000000000..4d6907cda75
--- /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 00000000000..d0d9538ca5b
--- /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 00000000000..2d2d0093282
--- /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 4240f0b720f..9b747c266af 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 0382ea752e6..8775e262bb6 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 0fb75524484..41108a9a195 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 1141b976dff..80ec64d2d6d 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 9594b52f860..12897e8a331 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 4da9cca939c..63ae5c61c9b 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 07af26e6beb..f023cc092c2 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 30547cc3f89..7ee84cc578b 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 d2e0a50063a..32114a3c0ca 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 51b0fccbfe7..b6a29f7de82 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 087db12a3a6..18e94c7d2be 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 ec2410c0c80..a803d9ea8fd 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 c8e958a07e9..f96f92f00f9 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 30d8896bb71..9c5c19f142d 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 8a947e603ac..e87ae2f634b 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 678e1252047..b818cacf420 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 253fdcce2df..b0b0fa5a357 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 fa1d09b0ad9..904ca8c8b17 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 69161a682b3..0c5f1cb9d99 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 8068cefd8d8..a3c76bf1817 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 ba54d0acdec..58e0bc41489 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 1743856b30d..1163907bcdd 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 aba537af87e..ccb876fe023 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 fa943bd05c7..f54e775c2df 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 69f6af6f6b9..084ea77d65a 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 0bb0f8cd36f..50e593bfcfa 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 73290555226..68d539456c5 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 d86da94fa50..9b7c8654dc8 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 0cb8a9f9d67..d1000723c5a 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 53160aa24e6..08cbfa144e6 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 9ce2fa037b9..b5247cb64fd 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 349a4ad3ccc..bb37ec80f27 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 456d9ad9ae5..d5b057207a7 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 397870f076c..9f6ce3d8a29 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 3681a6ff081..a4f99797eb5 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 f8a51b70792..6b33a972c83 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 edd1ffcca30..d9a9f7174f7 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 360be226718..01ff643e682 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 d07df22a5ec..79fb22c89ae 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 847ccc067e8..6afc6168954 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 00000000000..2446736b787
--- /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 931f469604b..c8d91b0cd9b 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 6c1c9cb5537..19ca6db38b2 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 5f4da80051b..f2481a85e0a 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 bfbe0843205..725634d9736 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 4c90e53bd96..ecef127dbc6 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 4917e2c2b32..e16d3a4bc1d 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 bc8d6bba8ee..9b550687213 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 8615fb81775..f390682629c 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 2893a0134c7..37706eb81f2 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 5bdfe7e16bc..b72580c3895 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 8069cd6bc5e..4402a8a74f7 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 bea7c9cf4f8..cf7f2194ded 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 742482e3001..a514fa61116 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 61979b70f38..c68531b8827 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 fc8666ae408..5552f813757 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 e6a2c3b302d..9fc0ae8a526 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 24fd4332215..e1da8fc9dd2 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 3f5c7a38e6e..979e544388c 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 7cf94c09d99..7d754fbcccb 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;