Conflicts:

	arch/arm/mach-tegra/common.c
	drivers/ata/ahci-tegra.c

19 files changed, 3310 insertions, 170 deletions

diff --git a/drivers/ata/ahci-tegra.c b/drivers/ata/ahci-tegra.c
index df524ff72cc8..c90472827b9b 100644
--- a/drivers/ata/ahci-tegra.c
+++ b/drivers/ata/ahci-tegra.c
@@ -258,10 +258,10 @@ static int tegra_ahci_controller_resume(struct platform_device *pdev);
 static int tegra_ahci_suspend(struct platform_device *pdev, pm_message_t mesg);
 static int tegra_ahci_resume(struct platform_device *pdev);
 static enum port_idle_status tegra_ahci_is_port_idle(struct ata_port *ap);
-//static enum port_idle_status tegra_ahci_is_port_slumber(struct ata_port *ap);
 static bool tegra_ahci_are_all_ports_idle(struct ata_host *host);
-//static bool tegra_ahci_are_all_ports_slumber(struct ata_host *host);
 #ifdef CONFIG_TEGRA_SATA_IDLE_POWERGATE
+static enum port_idle_status tegra_ahci_is_port_slumber(struct ata_port *ap);
+static bool tegra_ahci_are_all_ports_slumber(struct ata_host *host);
 static unsigned int tegra_ahci_qc_issue(struct ata_queued_cmd *qc);
 static int tegra_ahci_hardreset(struct ata_link *link, unsigned int *class,
 				unsigned long deadline);
@@ -926,7 +926,7 @@ static int tegra_ahci_controller_resume(struct platform_device *pdev)
 	}
 
 	spin_lock_irqsave(&host->lock, flags);
-	if (!tegra_hpriv->pg_state == SATA_ON) {
+	if (tegra_hpriv->pg_state == SATA_ON) {
 		dev_dbg(host->dev, "resume: SATA already powered on\n");
 	} else {
 		dev_dbg(host->dev, "resume: powering on SATA...\n");
@@ -1623,7 +1623,20 @@ static enum port_idle_status tegra_ahci_is_port_idle(struct ata_port *ap)
 	return PORT_IS_IDLE;
 }
 
-#if 0
+/* check if all supported ports are idle (no outstanding commands) */
+static bool tegra_ahci_are_all_ports_idle(struct ata_host *host)
+{	int i;
+	struct ata_port *ap;
+
+	for (i = 0; i < host->n_ports; i++) {
+		ap = host->ports[i];
+		if (ap && (tegra_ahci_is_port_idle(ap) == PORT_IS_NOT_IDLE))
+			return false;
+	}
+	return true;
+}
+
+#ifdef CONFIG_TEGRA_SATA_IDLE_POWERGATE
 static enum port_idle_status tegra_ahci_is_port_slumber(struct ata_port *ap)
 {
 	void __iomem *port_mmio = ahci_port_base(ap);
@@ -1638,22 +1651,7 @@ static enum port_idle_status tegra_ahci_is_port_slumber(struct ata_port *ap)
 		return PORT_IS_SLUMBER;
 	return PORT_IS_IDLE_NOT_SLUMBER;
 }
-#endif
 
-/* check if all supported ports are idle (no outstanding commands) */
-static bool tegra_ahci_are_all_ports_idle(struct ata_host *host)
-{	int i;
-	struct ata_port *ap;
-
-	for (i = 0; i < host->n_ports; i++) {
-		ap = host->ports[i];
-		if (ap && (tegra_ahci_is_port_idle(ap) == PORT_IS_NOT_IDLE))
-			return false;
-	}
-	return true;
-}
-
-#if 0
 /* check if all supported ports are in slumber */
 static bool tegra_ahci_are_all_ports_slumber(struct ata_host *host)
 {	int i;
@@ -1666,9 +1664,7 @@ static bool tegra_ahci_are_all_ports_slumber(struct ata_host *host)
 	}
 	return true;
 }
-#endif
 
-#ifdef CONFIG_TEGRA_SATA_IDLE_POWERGATE
 static void tegra_ahci_to_add_idle_timer(struct ata_host *host)
 {
 	struct tegra_ahci_host_priv *tegra_hpriv;
diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
index 38719599a476..651a17e04fd0 100644
--- a/drivers/media/video/Kconfig
+++ b/drivers/media/video/Kconfig
@@ -66,6 +66,13 @@ config VIDEOBUF2_DMA_SG
 	select VIDEOBUF2_CORE
 	select VIDEOBUF2_MEMOPS
 	tristate
+
+config VIDEOBUF2_DMA_NVMAP
+	select VIDEOBUF2_CORE
+	select VIDEOBUF2_MEMOPS
+	select NVMAP_ALLOW_SYSMEM
+	tristate
+
 #
 # Multimedia Video device configuration
 #
@@ -848,6 +855,12 @@ config SOC_CAMERA_OV2640
 	help
 	  This is a ov2640 camera driver
 
+config SOC_CAMERA_OV5640
+	tristate "ov5640 camera support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This is a V4L2 camera driver for the OmniVision OV5640 sensor
+
 config SOC_CAMERA_OV5642
 	tristate "ov5642 camera support"
 	depends on SOC_CAMERA && I2C
@@ -921,6 +934,13 @@ config VIDEO_SH_MOBILE_CEU
 	---help---
 	  This is a v4l2 driver for the SuperH Mobile CEU Interface
 
+config VIDEO_TEGRA
+	tristate "Tegra soc_camera host driver"
+	depends on VIDEO_DEV && ARCH_TEGRA && SOC_CAMERA && HAS_DMA && HAVE_CLK
+	select VIDEOBUF2_DMA_NVMAP
+	---help---
+	  This is a v4l2 driver for the Tegra camera interface
+
 config VIDEO_OMAP1
 	tristate "OMAP1 Camera Interface driver"
 	depends on VIDEO_DEV && ARCH_OMAP1 && SOC_CAMERA
diff --git a/drivers/media/video/Makefile b/drivers/media/video/Makefile
index 225f8823de2b..f63ad7e88fda 100644
--- a/drivers/media/video/Makefile
+++ b/drivers/media/video/Makefile
@@ -79,6 +79,7 @@ obj-$(CONFIG_SOC_CAMERA_MT9T031)	+= mt9t031.o
 obj-$(CONFIG_SOC_CAMERA_MT9T112)	+= mt9t112.o
 obj-$(CONFIG_SOC_CAMERA_MT9V022)	+= mt9v022.o
 obj-$(CONFIG_SOC_CAMERA_OV2640)		+= ov2640.o
+obj-$(CONFIG_SOC_CAMERA_OV5640)		+= ov5640.o
 obj-$(CONFIG_SOC_CAMERA_OV5642)		+= ov5642.o
 obj-$(CONFIG_SOC_CAMERA_OV6650)		+= ov6650.o
 obj-$(CONFIG_SOC_CAMERA_OV772X)		+= ov772x.o
@@ -122,6 +123,7 @@ obj-$(CONFIG_VIDEOBUF2_MEMOPS)		+= videobuf2-memops.o
 obj-$(CONFIG_VIDEOBUF2_VMALLOC)		+= videobuf2-vmalloc.o
 obj-$(CONFIG_VIDEOBUF2_DMA_CONTIG)	+= videobuf2-dma-contig.o
 obj-$(CONFIG_VIDEOBUF2_DMA_SG)		+= videobuf2-dma-sg.o
+obj-$(CONFIG_VIDEOBUF2_DMA_NVMAP)	+= videobuf2-dma-nvmap.o
 
 obj-$(CONFIG_V4L2_MEM2MEM_DEV) += v4l2-mem2mem.o
 
@@ -168,6 +170,7 @@ obj-$(CONFIG_VIDEO_MX3)			+= mx3_camera.o
 obj-$(CONFIG_VIDEO_PXA27x)		+= pxa_camera.o
 obj-$(CONFIG_VIDEO_SH_MOBILE_CSI2)	+= sh_mobile_csi2.o
 obj-$(CONFIG_VIDEO_SH_MOBILE_CEU)	+= sh_mobile_ceu_camera.o
+obj-$(CONFIG_VIDEO_TEGRA)		+= tegra_v4l2_camera.o
 obj-$(CONFIG_VIDEO_OMAP1)		+= omap1_camera.o
 obj-$(CONFIG_VIDEO_ATMEL_ISI)		+= atmel-isi.o
 
@@ -194,3 +197,4 @@ obj-$(CONFIG_ARCH_TEGRA) += tegra/
 EXTRA_CFLAGS += -Idrivers/media/dvb/dvb-core
 EXTRA_CFLAGS += -Idrivers/media/dvb/frontends
 EXTRA_CFLAGS += -Idrivers/media/common/tuners
+EXTRA_CFLAGS += -Idrivers/video/tegra/host
diff --git a/drivers/media/video/ov5640.c b/drivers/media/video/ov5640.c
new file mode 100644
index 000000000000..8e45fa3de5d2
--- /dev/null
+++ b/drivers/media/video/ov5640.c
@@ -0,0 +1,1151 @@
+/*
+ * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/soc_camera.h>
+
+#define to_ov5640(sd)		container_of(sd, struct ov5640_priv, subdev)
+
+#define OV5640_SYSTEM_CTRL		0x3008
+#define OV5640_CHIP_ID_HI		0x300a
+#define OV5640_CHIP_ID_LO		0x300b
+#define OV5640_PAD_OUTPUT_ENABLE00	0x3016
+#define OV5640_PAD_OUTPUT_ENABLE01	0x3017
+#define OV5640_PAD_OUTPUT_ENABLE02	0x3018
+#define OV5640_SC_PLL_CTRL0		0x3034
+#define OV5640_SC_PLL_CTRL1		0x3035
+#define OV5640_SC_PLL_CTRL2		0x3036
+#define OV5640_SC_PLL_CTRL3		0x3037
+
+/* SCCB Control */
+#define OV5640_SCCB_SYSTEM_CTRL1	0x3103
+#define OV5640_SYSTEM_ROOT_DIVIDER	0x3108
+
+/* Timing Control */
+#define OV5640_TIMING_HS_HI		0x3800
+#define OV5640_TIMING_HS_LO		0x3801
+#define OV5640_TIMING_VS_HI		0x3802
+#define OV5640_TIMING_VS_LO		0x3803
+#define OV5640_TIMING_HW_HI		0x3804
+#define OV5640_TIMING_HW_LO		0x3805
+#define OV5640_TIMING_VH_HI		0x3806
+#define OV5640_TIMING_VH_LO		0x3807
+#define OV5640_TIMING_DVPHO_HI		0x3808
+#define OV5640_TIMING_DVPHO_LO		0x3809
+#define OV5640_TIMING_DVPVO_HI		0x380a
+#define OV5640_TIMING_DVPVO_LO		0x380b
+#define OV5640_TIMING_HTS_HI		0x380c
+#define OV5640_TIMING_HTS_LO		0x380d
+#define OV5640_TIMING_VTS_HI		0x380e
+#define OV5640_TIMING_VTS_LO		0x380f
+#define OV5640_TIMING_HOFFSET_HI	0x3810
+#define OV5640_TIMING_HOFFSET_LO	0x3811
+#define OV5640_TIMING_VOFFSET_HI	0x3812
+#define OV5640_TIMING_VOFFSET_LO	0x3813
+#define OV5640_TIMING_X_INC		0x3814
+#define OV5640_TIMING_Y_INC		0x3815
+#define OV5640_TIMING_TC_REG20		0x3820
+#define OV5640_TIMING_TC_REG21		0x3821
+
+/* AEC/AGC Power Down Domain Control */
+#define OV5640_AEC_MAX_EXPO_60HZ_HI	0x3a02
+#define OV5640_AEC_MAX_EXPO_60HZ_LO	0x3a03
+#define OV5640_AEC_B50_STEP_HI		0x3a08
+#define OV5640_AEC_B50_STEP_LO		0x3a09
+#define OV5640_AEC_B60_STEP_HI		0x3a0a
+#define OV5640_AEC_B60_STEP_LO		0x3a0b
+#define OV5640_AEC_CTRL0C		0x3a0c
+#define OV5640_AEC_CTRL0D		0x3a0d
+#define OV5640_AEC_CTRL0E		0x3a0e
+#define OV5640_AEC_CTRL0F		0x3a0f
+#define OV5640_AEC_CTRL10		0x3a10
+#define OV5640_AEC_CTRL11		0x3a11
+#define OV5640_AEC_CTRL12		0x3a12
+#define OV5640_AEC_CTRL13		0x3a13
+#define OV5640_AEC_MAX_EXPO_50HZ_HI	0x3a14
+#define OV5640_AEC_MAX_EXPO_50HZ_LO	0x3a15
+#define OV5640_AEC_GAIN_CEILING_HI	0x3a18
+#define OV5640_AEC_GAIN_CEILING_LO	0x3a19
+#define OV5640_AEC_CTRL1B		0x3a1b
+#define OV5640_AEC_CTRL1E		0x3a1e
+#define OV5640_AEC_CTRL1F		0x3a1f
+
+/* 50/60Hz Detector Control */
+#define OV5640_5060HZ_CTRL00		0x3c00
+#define OV5640_5060HZ_CTRL01		0x3c01
+#define OV5640_5060HZ_CTRL02		0x3c02
+#define OV5640_5060HZ_CTRL03		0x3c03
+#define OV5640_5060HZ_CTRL04		0x3c04
+#define OV5640_5060HZ_CTRL05		0x3c05
+#define OV5640_LIGHT_METER1_THRESHOLD_HI 0x3c06
+#define OV5640_LIGHT_METER1_THRESHOLD_LO 0x3c07
+#define OV5640_LIGHT_METER2_THRESHOLD_HI 0x3c08
+#define OV5640_LIGHT_METER2_THRESHOLD_LO 0x3c09
+#define OV5640_SAMPLE_NUMBER_HI		0x3c0a
+#define OV5640_SAMPLE_NUMBER_LO		0x3c0b
+
+/* ISP General Controls */
+#define OV5640_ISP_CTRL00		0x5000
+#define OV5640_ISP_CTRL01		0x5001
+#define OV5640_ISP_CTRL37		0x5025
+
+/* AWB Control */
+#define OV5640_AWB_CTRL00		0x5180
+#define OV5640_AWB_CTRL01		0x5181
+#define OV5640_AWB_CTRL02		0x5182
+#define OV5640_AWB_CTRL03		0x5183
+#define OV5640_AWB_CTRL04		0x5184
+#define OV5640_AWB_CTRL05		0x5185
+#define OV5640_AWB_CTRL06		0x5186
+#define OV5640_AWB_CTRL07		0x5187
+#define OV5640_AWB_CTRL08		0x5188
+#define OV5640_AWB_CTRL09		0x5189
+#define OV5640_AWB_CTRL10		0x518a
+#define OV5640_AWB_CTRL11		0x518b
+#define OV5640_AWB_CTRL12		0x518c
+#define OV5640_AWB_CTRL13		0x518d
+#define OV5640_AWB_CTRL14		0x518e
+#define OV5640_AWB_CTRL15		0x518f
+#define OV5640_AWB_CTRL16		0x5190
+#define OV5640_AWB_CTRL17		0x5191
+#define OV5640_AWB_CTRL18		0x5192
+#define OV5640_AWB_CTRL19		0x5193
+#define OV5640_AWB_CTRL20		0x5194
+#define OV5640_AWB_CTRL21		0x5195
+#define OV5640_AWB_CTRL22		0x5196
+#define OV5640_AWB_CTRL23		0x5197
+#define OV5640_AWB_CTRL24		0x5198
+#define OV5640_AWB_CTRL25		0x5199
+#define OV5640_AWB_CTRL26		0x519a
+#define OV5640_AWB_CTRL27		0x519b
+#define OV5640_AWB_CTRL28		0x519c
+#define OV5640_AWB_CTRL29		0x519d
+#define OV5640_AWB_CTRL30		0x519e
+
+/* CIP Control */
+#define OV5640_CIP_SHARPENMT_THRESHOLD_1 0x5300
+#define OV5640_CIP_SHARPENMT_THRESHOLD_2 0x5301
+#define OV5640_CIP_SHARPENMT_OFFSET_1	0x5302
+#define OV5640_CIP_SHARPENMT_OFFSET_2	0x5303
+#define OV5640_CIP_DNS_THRESHOLD_1	0x5304
+#define OV5640_CIP_DNS_THRESHOLD_2	0x5305
+#define OV5640_CIP_DNS_OFFSET_1		0x5306
+#define OV5640_CIP_DNS_OFFSET_2		0x5307
+#define OV5640_CIP_CTRL			0x5308
+#define OV5640_CIP_SHARPENTH_THRESHOLD_1 0x5309
+#define OV5640_CIP_SHARPENTH_THRESHOLD_2 0x530a
+#define OV5640_CIP_SHARPENTH_OFFSET_1	0x530b
+#define OV5640_CIP_SHARPENTH_OFFSET_2	0x530c
+#define OV5640_CIP_EDGE_MT_AUTO		0x530d
+#define OV5640_CIP_DNS_THRESHOLD_AUTO	0x530e
+#define OF5640_CIP_SHARPEN_THRESHOLD_AUTO 0x530f
+
+/* CMX Control */
+#define OV5640_CMX_CTRL			0x5380
+#define OV5640_CMX1			0x5381
+#define OV5640_CMX2			0x5382
+#define OV5640_CMX3			0x5383
+#define OV5640_CMX4			0x5384
+#define OV5640_CMX5			0x5385
+#define OV5640_CMX6			0x5386
+#define OV5640_CMX7			0x5387
+#define OV5640_CMX8			0x5388
+#define OV5640_CMX9			0x5389
+#define OV5640_CMXSIGN_HI		0x538a
+#define OV5640_CMXSIGN_LO		0x538b
+
+/* Gamma Control */
+#define OV5640_GAMMA_CTRL00		0x5480
+#define OV5640_YST00			0x5481
+#define OV5640_YST01			0x5482
+#define OV5640_YST02			0x5483
+#define OV5640_YST03			0x5484
+#define OV5640_YST04			0x5485
+#define OV5640_YST05			0x5486
+#define OV5640_YST06			0x5487
+#define OV5640_YST07			0x5488
+#define OV5640_YST08			0x5489
+#define OV5640_YST09			0x548a
+#define OV5640_YST0A			0x548b
+#define OV5640_YST0B			0x548c
+#define OV5640_YST0C			0x548d
+#define OV5640_YST0D			0x548e
+#define OV5640_YST0E			0x548f
+#define OV5640_YST0F			0x5490
+
+/* SDE Control */
+#define OV5640_SDE_CTRL_0		0x5580
+#define OV5640_SDE_CTRL_1		0x5581
+#define OV5640_SDE_CTRL_2		0x5582
+#define OV5640_SDE_CTRL_3		0x5583
+#define OV5640_SDE_CTRL_4		0x5584
+#define OV5640_SDE_CTRL_5		0x5585
+#define OV5640_SDE_CTRL_6		0x5586
+#define OV5640_SDE_CTRL_7		0x5587
+#define OV5640_SDE_CTRL_8		0x5588
+#define OV5640_SDE_CTRL_9		0x5589
+#define OV5640_SDE_CTRL_10		0x558a
+#define OV5640_SDE_CTRL_11		0x558b
+#define OV5640_SDE_CTRL_12		0x558c
+
+/* LENC Control */
+#define OV5640_GMTRX00			0x5800
+#define OV5640_GMTRX01			0x5801
+#define OV5640_GMTRX02			0x5802
+#define OV5640_GMTRX03			0x5803
+#define OV5640_GMTRX04			0x5804
+#define OV5640_GMTRX05			0x5805
+#define OV5640_GMTRX10			0x5806
+#define OV5640_GMTRX11			0x5807
+#define OV5640_GMTRX12			0x5808
+#define OV5640_GMTRX13			0x5809
+#define OV5640_GMTRX14			0x580a
+#define OV5640_GMTRX15			0x580b
+#define OV5640_GMTRX20			0x580c
+#define OV5640_GMTRX21			0x580d
+#define OV5640_GMTRX22			0x580e
+#define OV5640_GMTRX23			0x580f
+#define OV5640_GMTRX24			0x5810
+#define OV5640_GMTRX25			0x5811
+#define OV5640_GMTRX30			0x5812
+#define OV5640_GMTRX31			0x5813
+#define OV5640_GMTRX32			0x5814
+#define OV5640_GMTRX33			0x5815
+#define OV5640_GMTRX34			0x5816
+#define OV5640_GMTRX35			0x5817
+#define OV5640_GMTRX40			0x5818
+#define OV5640_GMTRX41			0x5819
+#define OV5640_GMTRX42			0x581a
+#define OV5640_GMTRX43			0x581b
+#define OV5640_GMTRX44			0x581c
+#define OV5640_GMTRX45			0x581d
+#define OV5640_GMTRX50			0x581e
+#define OV5640_GMTRX51			0x581f
+#define OV5640_GMTRX52			0x5820
+#define OV5640_GMTRX53			0x5821
+#define OV5640_GMTRX54			0x5822
+#define OV5640_GMTRX55			0x5823
+#define OV5640_BRMATRX00		0x5824
+#define OV5640_BRMATRX01		0x5825
+#define OV5640_BRMATRX02		0x5826
+#define OV5640_BRMATRX03		0x5827
+#define OV5640_BRMATRX04		0x5828
+#define OV5640_BRMATRX05		0x5829
+#define OV5640_BRMATRX06		0x582a
+#define OV5640_BRMATRX07		0x582b
+#define OV5640_BRMATRX08		0x582c
+#define OV5640_BRMATRX09		0x582d
+#define OV5640_BRMATRX20		0x582e
+#define OV5640_BRMATRX21		0x582f
+#define OV5640_BRMATRX22		0x5830
+#define OV5640_BRMATRX23		0x5831
+#define OV5640_BRMATRX24		0x5832
+#define OV5640_BRMATRX30		0x5833
+#define OV5640_BRMATRX31		0x5834
+#define OV5640_BRMATRX32		0x5835
+#define OV5640_BRMATRX33		0x5836
+#define OV5640_BRMATRX34		0x5837
+#define OV5640_BRMATRX40		0x5838
+#define OV5640_BRMATRX41		0x5839
+#define OV5640_BRMATRX42		0x583a
+#define OV5640_BRMATRX43		0x583b
+#define OV5640_BRMATRX44		0x583c
+#define OV5640_LENC_BR_OFFSET		0x583d
+
+#define OV5640_MAX_WIDTH		640
+#define OV5640_MAX_HEIGHT		480
+
+/* Misc. structures */
+struct ov5640_reg {
+	u16				reg;
+	u8				val;
+};
+
+struct ov5640_priv {
+	struct v4l2_subdev		subdev;
+
+	int				ident;
+	u16				chip_id;
+	u8				revision;
+	u8				manid;
+	u8				smiaver;
+
+	bool				flag_vflip;
+	bool				flag_hflip;
+
+	/* For suspend/resume. */
+	struct v4l2_mbus_framefmt	current_mf;
+	bool				current_enable;
+};
+
+static const struct ov5640_reg ov5640_defaults[] = {
+	{ OV5640_SCCB_SYSTEM_CTRL1,		0x11},
+	{ OV5640_SYSTEM_CTRL,			0x82},
+	{ OV5640_SYSTEM_CTRL,			0x42},
+	{ OV5640_SCCB_SYSTEM_CTRL1,		0x03},
+	{ OV5640_PAD_OUTPUT_ENABLE01,		0x00},
+	{ OV5640_PAD_OUTPUT_ENABLE02,		0x00},
+	{ OV5640_SC_PLL_CTRL0,			0x18},
+	{ OV5640_SC_PLL_CTRL1,			0x14},
+	{ OV5640_SC_PLL_CTRL2,			0x38},
+	{ OV5640_SC_PLL_CTRL3,			0x13},
+	{ 0x4800,	0x24}, /* noncontinuous clock */
+	{ OV5640_SYSTEM_ROOT_DIVIDER,		0x01},
+	{ 0x3630,	0x36},
+	{ 0x3631,	0x0e},
+	{ 0x3632,	0xe2},
+	{ 0x3633,	0x12},
+	{ 0x3621,	0xe0},
+	{ 0x3704,	0xa0},
+	{ 0x3703,	0x5a},
+	{ 0x3715,	0x78},
+	{ 0x3717,	0x01},
+	{ 0x370b,	0x60},
+	{ 0x3705,	0x1a},
+	{ 0x3905,	0x02},
+	{ 0x3906,	0x10},
+	{ 0x3901,	0x0a},
+	{ 0x3731,	0x12},
+	{ 0x3600,	0x08},
+	{ 0x3601,	0x33},
+	{ 0x302d,	0x60},
+	{ 0x3620,	0x52},
+	{ 0x371b,	0x20},
+	{ 0x471c,	0x50},
+	{ 0x3a13,	0x43},
+	{ 0x3a18,	0x00},
+	{ 0x3a19,	0xf8},
+	{ 0x3635,	0x13},
+	{ 0x3636,	0x03},
+	{ 0x3634,	0x40},
+	{ 0x3622,	0x01},
+	{ 0x3c01,	0x34},
+	{ 0x3c04,	0x28},
+	{ 0x3c05,	0x98},
+	{ 0x3c06,	0x00},
+	{ 0x3c07,	0x08},
+	{ 0x3c08,	0x00},
+	{ 0x3c09,	0x1c},
+	{ 0x3c0a,	0x9c},
+	{ 0x3c0b,	0x40},
+	{ OV5640_TIMING_TC_REG20,	0x41},
+	{ OV5640_TIMING_TC_REG21,	0x01},
+	{ 0x3814,	0x31},
+	{ 0x3815,	0x31},
+	{ 0x3800,	0x00},
+	{ 0x3801,	0x00},
+	{ 0x3802,	0x00},
+	{ 0x3803,	0x04},
+	{ 0x3804,	0x0a},
+	{ 0x3805,	0x3f},
+	{ 0x3806,	0x07},
+	{ 0x3807,	0x9b},
+	{ 0x3808,	0x02},
+	{ 0x3809,	0x80},
+	{ 0x380a,	0x01},
+	{ 0x380b,	0xe0},
+	{ 0x380c,	0x07},
+	{ 0x380d,	0x68},
+	{ 0x380e,	0x03},
+	{ 0x380f,	0xd8},
+	{ 0x3810,	0x00},
+	{ 0x3811,	0x10},
+	{ 0x3812,	0x00},
+	{ 0x3813,	0x06},
+	{ 0x3618,	0x00},
+	{ 0x3612,	0x29},
+	{ 0x3708,	0x64},
+	{ 0x3709,	0x52},
+	{ 0x370c,	0x03},
+
+	/* AEC/AGC Power Down Domain Control */
+	{ OV5640_AEC_MAX_EXPO_60HZ_HI,	0x03},
+	{ OV5640_AEC_MAX_EXPO_60HZ_LO,	0xd8},
+	{ OV5640_AEC_B50_STEP_HI,	0x01},
+	{ OV5640_AEC_B50_STEP_LO,	0x27},
+	{ OV5640_AEC_B60_STEP_HI,	0x00},
+	{ OV5640_AEC_B60_STEP_LO,	0xf6},
+	{ OV5640_AEC_CTRL0E,		0x03},
+	{ OV5640_AEC_CTRL0D,		0x04},
+	{ OV5640_AEC_MAX_EXPO_50HZ_HI,	0x03},
+	{ OV5640_AEC_MAX_EXPO_50HZ_LO,	0xd8},
+
+	{ 0x4001,	0x02},
+	{ 0x4004,	0x02},
+	{ 0x3000,	0x00},
+	{ 0x3002,	0x1c},
+	{ 0x3004,	0xff},
+	{ 0x3006,	0xc3},
+	{ 0x300e,	0x45},
+	{ 0x302e,	0x08},
+	/* org:30 bit[3:0]
+	   0x0:YUYV 0x1:YVYU 0x2:UYVY
+	   0x3:VYUY 0xF:UYVY 0x4~0xE:Not-allowed
+	 */
+	{ 0x4300,	0x32},
+	{ 0x501f,	0x00},
+	{ 0x4713,	0x03},
+	{ 0x4407,	0x04},
+	{ 0x440e,	0x00},
+	{ 0x460b,	0x35},
+	{ 0x460c,	0x22},
+	{ 0x4837,	0x44},
+	{ 0x3824,	0x02},
+	{ 0x5000,	0xa7},
+	{ 0x5001,	0xa3},
+
+	/* AWB Control */
+	{ OV5640_AWB_CTRL00,	0xff},	{ OV5640_AWB_CTRL01,	0xf2},
+	{ OV5640_AWB_CTRL02,	0x00},	{ OV5640_AWB_CTRL03,	0x14},
+	{ OV5640_AWB_CTRL04,	0x25},	{ OV5640_AWB_CTRL05,	0x24},
+	{ OV5640_AWB_CTRL06,	0x09},	{ OV5640_AWB_CTRL07,	0x09},
+	{ OV5640_AWB_CTRL08,	0x09},	{ OV5640_AWB_CTRL09,	0x75},
+	{ OV5640_AWB_CTRL10,	0x54},	{ OV5640_AWB_CTRL11,	0xe0},
+	{ OV5640_AWB_CTRL12,	0xb2},	{ OV5640_AWB_CTRL13,	0x42},
+	{ OV5640_AWB_CTRL14,	0x3d},	{ OV5640_AWB_CTRL15,	0x56},
+	{ OV5640_AWB_CTRL16,	0x46},	{ OV5640_AWB_CTRL17,	0xf8},
+	{ OV5640_AWB_CTRL18,	0x04},	{ OV5640_AWB_CTRL19,	0x70},
+	{ OV5640_AWB_CTRL20,	0xf0},	{ OV5640_AWB_CTRL21,	0xf0},
+	{ OV5640_AWB_CTRL22,	0x03},	{ OV5640_AWB_CTRL23,	0x01},
+	{ OV5640_AWB_CTRL24,	0x04},	{ OV5640_AWB_CTRL25,	0x12},
+	{ OV5640_AWB_CTRL26,	0x04},	{ OV5640_AWB_CTRL27,	0x00},
+	{ OV5640_AWB_CTRL28,	0x06},	{ OV5640_AWB_CTRL29,	0x82},
+	{ OV5640_AWB_CTRL30,	0x38},
+
+	/* CMX Control */
+	{ OV5640_CMX1,		0x1e},
+	{ OV5640_CMX2,		0x5b},
+	{ OV5640_CMX3,		0x08},
+	{ OV5640_CMX4,		0x0a},
+	{ OV5640_CMX5,		0x7e},
+	{ OV5640_CMX6,		0x88},
+	{ OV5640_CMX7,		0x7c},
+	{ OV5640_CMX8,		0x6c},
+	{ OV5640_CMX9,		0x10},
+	{ OV5640_CMXSIGN_HI,	0x01},
+	{ OV5640_CMXSIGN_LO,	0x98},
+
+	/* CIP Control */
+	{ OV5640_CIP_SHARPENMT_THRESHOLD_1,	0x08},
+	{ OV5640_CIP_SHARPENMT_THRESHOLD_2,	0x30},
+	{ OV5640_CIP_SHARPENMT_OFFSET_1,	0x10},
+	{ OV5640_CIP_SHARPENMT_OFFSET_2,	0x00},
+	{ OV5640_CIP_DNS_THRESHOLD_1,		0x08},
+	{ OV5640_CIP_DNS_THRESHOLD_2,		0x30},
+	{ OV5640_CIP_DNS_OFFSET_1,		0x08},
+	{ OV5640_CIP_DNS_OFFSET_2,		0x16},
+	{ OV5640_CIP_SHARPENTH_THRESHOLD_1,	0x08},
+	{ OV5640_CIP_SHARPENTH_THRESHOLD_2,	0x30},
+	{ OV5640_CIP_SHARPENTH_OFFSET_1,	0x04},
+	{ OV5640_CIP_SHARPENTH_OFFSET_2,	0x06},
+
+	/* Gamma Control */
+	{ OV5640_GAMMA_CTRL00,	0x01},
+	{ OV5640_YST00,		0x08},	{ OV5640_YST01,		0x14},
+	{ OV5640_YST02,		0x28},	{ OV5640_YST03,		0x51},
+	{ OV5640_YST04,		0x65},	{ OV5640_YST05,		0x71},
+	{ OV5640_YST06,		0x7d},	{ OV5640_YST07,		0x87},
+	{ OV5640_YST08,		0x91},	{ OV5640_YST09,		0x9a},
+	{ OV5640_YST0A,		0xaa},	{ OV5640_YST0B,		0xb8},
+	{ OV5640_YST0C,		0xcd},	{ OV5640_YST0D,		0xdd},
+	{ OV5640_YST0E,		0xea},	{ OV5640_YST0F,		0x1d},
+
+	/* SDE Control */
+	{ OV5640_SDE_CTRL_0,	0x02},
+	{ OV5640_SDE_CTRL_3,	0x40},
+	{ OV5640_SDE_CTRL_4,	0x10},
+	{ OV5640_SDE_CTRL_9,	0x10},
+	{ OV5640_SDE_CTRL_10,	0x00},
+	{ OV5640_SDE_CTRL_11,	0xf8},
+
+	/* LENC Control */
+	{ OV5640_GMTRX00,	0x23},	{ OV5640_GMTRX01,	0x14},
+	{ OV5640_GMTRX02,	0x0f},	{ OV5640_GMTRX03,	0x0f},
+	{ OV5640_GMTRX04,	0x12},	{ OV5640_GMTRX05,	0x26},
+	{ OV5640_GMTRX10,	0x0c},	{ OV5640_GMTRX11,	0x08},
+	{ OV5640_GMTRX12,	0x05},	{ OV5640_GMTRX13,	0x05},
+	{ OV5640_GMTRX14,	0x08},	{ OV5640_GMTRX15,	0x0d},
+	{ OV5640_GMTRX20,	0x08},	{ OV5640_GMTRX21,	0x03},
+	{ OV5640_GMTRX22,	0x00},	{ OV5640_GMTRX23,	0x00},
+	{ OV5640_GMTRX24,	0x03},	{ OV5640_GMTRX25,	0x09},
+	{ OV5640_GMTRX30,	0x07},	{ OV5640_GMTRX31,	0x03},
+	{ OV5640_GMTRX32,	0x00},	{ OV5640_GMTRX33,	0x01},
+	{ OV5640_GMTRX34,	0x03},	{ OV5640_GMTRX35,	0x08},
+	{ OV5640_GMTRX40,	0x0d},	{ OV5640_GMTRX41,	0x08},
+	{ OV5640_GMTRX42,	0x05},	{ OV5640_GMTRX43,	0x06},
+	{ OV5640_GMTRX44,	0x08},	{ OV5640_GMTRX45,	0x0e},
+	{ OV5640_GMTRX50,	0x29},	{ OV5640_GMTRX51,	0x17},
+	{ OV5640_GMTRX52,	0x11},	{ OV5640_GMTRX53,	0x11},
+	{ OV5640_GMTRX54,	0x15},	{ OV5640_GMTRX55,	0x28},
+	{ OV5640_BRMATRX00,	0x46},	{ OV5640_BRMATRX01,	0x26},
+	{ OV5640_BRMATRX02,	0x08},	{ OV5640_BRMATRX03,	0x26},
+	{ OV5640_BRMATRX04,	0x64},	{ OV5640_BRMATRX05,	0x26},
+	{ OV5640_BRMATRX06,	0x24},	{ OV5640_BRMATRX07,	0x22},
+	{ OV5640_BRMATRX08,	0x24},	{ OV5640_BRMATRX09,	0x24},
+	{ OV5640_BRMATRX20,	0x06},	{ OV5640_BRMATRX21,	0x22},
+	{ OV5640_BRMATRX22,	0x40},	{ OV5640_BRMATRX23,	0x42},
+	{ OV5640_BRMATRX24,	0x24},	{ OV5640_BRMATRX30,	0x26},
+	{ OV5640_BRMATRX31,	0x24},	{ OV5640_BRMATRX32,	0x22},
+	{ OV5640_BRMATRX33,	0x22},	{ OV5640_BRMATRX34,	0x26},
+	{ OV5640_BRMATRX40,	0x44},	{ OV5640_BRMATRX41,	0x24},
+	{ OV5640_BRMATRX42,	0x26},	{ OV5640_BRMATRX43,	0x28},
+	{ OV5640_BRMATRX44,	0x42},	{ OV5640_LENC_BR_OFFSET, 0xce},
+
+	{ OV5640_ISP_CTRL37,	0x00},
+	{ OV5640_AEC_CTRL0F,	0x30},
+	{ OV5640_AEC_CTRL10,	0x28},
+	{ OV5640_AEC_CTRL1B,	0x30},
+	{ OV5640_AEC_CTRL1E,	0x26},
+	{ OV5640_AEC_CTRL11,	0x60},
+	{ OV5640_AEC_CTRL1F,	0x14},
+	{ OV5640_SYSTEM_CTRL,	0x02},
+};
+
+static enum v4l2_mbus_pixelcode ov5640_codes[] = {
+	V4L2_MBUS_FMT_YUYV8_2X8,
+};
+
+static const struct v4l2_queryctrl ov5640_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 ov5640_reg_read(struct i2c_client *client, u16 reg, u8 *val)
+{
+	int ret;
+	unsigned char data[2] = { reg >> 8, reg & 0xff };
+
+	ret = i2c_master_send(client, data, 2);
+	if (ret < 2) {
+		dev_err(&client->dev, "%s: i2c read error, reg: %x\n",
+				__func__, reg);
+		return ret < 0 ? ret : -EIO;
+	}
+
+	ret = i2c_master_recv(client, val, 1);
+	if (ret < 1) {
+		dev_err(&client->dev, "%s: i2c read error, reg: %x\n",
+				__func__, reg);
+		return ret < 0 ? ret : -EIO;
+	}
+
+	return 0;
+}
+
+/* write a register */
+static int ov5640_reg_write(struct i2c_client *client, u16 reg, u8 val)
+{
+	int ret;
+	unsigned char data[3] = { reg >> 8, reg & 0xff, val };
+
+	ret = i2c_master_send(client, data, 3);
+	if (ret < 3) {
+		dev_err(&client->dev, "%s: i2c write error, reg: %x\n",
+				__func__, reg);
+		return ret < 0 ? ret : -EIO;
+	}
+
+	return 0;
+}
+
+
+/* Read a register, alter its bits, write it back */
+static int ov5640_reg_rmw(struct i2c_client *client, u16 reg, u8 set, u8 unset)
+{
+	u8 val;
+	int ret;
+
+	ret = ov5640_reg_read(client, reg, &val);
+	if (ret < 0) {
+		dev_err(&client->dev,
+				"[Read]-Modify-Write of register 0x%04x failed!\n",
+				reg);
+		return ret;
+	}
+
+	val |= set;
+	val &= ~unset;
+
+	ret = ov5640_reg_write(client, reg, val);
+	if (ret < 0) {
+		dev_err(&client->dev,
+				"Read-Modify-[Write] of register 0x%04x failed!\n",
+				reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov5640_reg_write_array(struct i2c_client *client,
+		const struct ov5640_reg *regarray,
+		int regarraylen)
+{
+	int i;
+	int ret;
+
+	for (i = 0; i < regarraylen; i++) {
+		ret = ov5640_reg_write(client,
+				regarray[i].reg, regarray[i].val);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+/* Start/Stop streaming from the device */
+static int ov5640_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5640_priv *priv = to_ov5640(sd);
+	int ret;
+
+	/* Program orientation register. */
+	if (priv->flag_vflip)
+		ret = ov5640_reg_rmw(client, OV5640_TIMING_TC_REG20, 0x2, 0);
+	else
+		ret = ov5640_reg_rmw(client, OV5640_TIMING_TC_REG20, 0, 0x2);
+	if (ret < 0)
+		return ret;
+
+	if (priv->flag_hflip)
+		ret = ov5640_reg_rmw(client, OV5640_TIMING_TC_REG21, 0x2, 0);
+	else
+		ret = ov5640_reg_rmw(client, OV5640_TIMING_TC_REG21, 0, 0x2);
+	if (ret < 0)
+		return ret;
+
+	if (!enable) {
+		/* Software Reset */
+		ret = ov5640_reg_write(client, OV5640_SYSTEM_CTRL, 0x82);
+		if (!ret)
+			/* Setting Streaming to Standby */
+			ret = ov5640_reg_write(client, OV5640_SYSTEM_CTRL,
+					0x42);
+	}
+
+	priv->current_enable = enable;
+
+	return ret;
+}
+
+/* Alter bus settings on camera side */
+static int ov5640_set_bus_param(struct soc_camera_device *icd,
+		unsigned long flags)
+{
+	return 0;
+}
+
+/* Request bus settings on camera side */
+static unsigned long ov5640_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 ov5640_res_roundup(u32 *width, u32 *height)
+{
+	/* Width must be a multiple of 4 pixels. */
+	*width = ALIGN(*width, 4);
+
+	/* Max resolution is 1280x720 (720p). */
+	if (*width > OV5640_MAX_WIDTH)
+		*width = OV5640_MAX_WIDTH;
+
+	if (*height > OV5640_MAX_HEIGHT)
+		*height = OV5640_MAX_HEIGHT;
+}
+
+/* Setup registers according to resolution and color encoding */
+static int ov5640_set_res(struct i2c_client *client, u32 width, u32 height)
+{
+	/* Note, this stuff is bogus.  It's just copied from ov9740.c. */
+#if 0
+	u32 x_start;
+	u32 y_start;
+	u32 x_end;
+	u32 y_end;
+	bool scaling = 0;
+	u32 scale_input_x;
+	u32 scale_input_y;
+	int ret;
+
+	if ((width != OV5640_MAX_WIDTH) || (height != OV5640_MAX_HEIGHT))
+		scaling = 1;
+
+	/*
+	 * Try to use as much of the sensor area as possible when supporting
+	 * smaller resolutions.  Depending on the aspect ratio of the
+	 * chosen resolution, we can either use the full width of the sensor,
+	 * or the full height of the sensor (or both if the aspect ratio is
+	 * the same as 1280x720.
+	 */
+	if ((OV5640_MAX_WIDTH * height) > (OV5640_MAX_HEIGHT * width)) {
+		scale_input_x = (OV5640_MAX_HEIGHT * width) / height;
+		scale_input_y = OV5640_MAX_HEIGHT;
+	} else {
+		scale_input_x = OV5640_MAX_WIDTH;
+		scale_input_y = (OV5640_MAX_WIDTH * height) / width;
+	}
+
+	/* These describe the area of the sensor to use. */
+	x_start = (OV5640_MAX_WIDTH - scale_input_x) / 2;
+	y_start = (OV5640_MAX_HEIGHT - scale_input_y) / 2;
+	x_end = x_start + scale_input_x - 1;
+	y_end = y_start + scale_input_y - 1;
+
+	ret = ov5640_reg_write(client, OV5640_X_ADDR_START_HI, x_start >> 8);
+	if (ret)
+		goto done;
+	ret = ov5640_reg_write(client, OV5640_X_ADDR_START_LO, x_start & 0xff);
+	if (ret)
+		goto done;
+	ret = ov5640_reg_write(client, OV5640_Y_ADDR_START_HI, y_start >> 8);
+	if (ret)
+		goto done;
+	ret = ov5640_reg_write(client, OV5640_Y_ADDR_START_LO, y_start & 0xff);
+	if (ret)
+		goto done;
+
+	ret = ov5640_reg_write(client, OV5640_X_ADDR_END_HI, x_end >> 8);
+	if (ret)
+		goto done;
+	ret = ov5640_reg_write(client, OV5640_X_ADDR_END_LO, x_end & 0xff);
+	if (ret)
+		goto done;
+	ret = ov5640_reg_write(client, OV5640_Y_ADDR_END_HI, y_end >> 8);
+	if (ret)
+		goto done;
+	ret = ov5640_reg_write(client, OV5640_Y_ADDR_END_LO, y_end & 0xff);
+	if (ret)
+		goto done;
+
+	ret = ov5640_reg_write(client, OV5640_X_OUTPUT_SIZE_HI, width >> 8);
+	if (ret)
+		goto done;
+	ret = ov5640_reg_write(client, OV5640_X_OUTPUT_SIZE_LO, width & 0xff);
+	if (ret)
+		goto done;
+	ret = ov5640_reg_write(client, OV5640_Y_OUTPUT_SIZE_HI, height >> 8);
+	if (ret)
+		goto done;
+	ret = ov5640_reg_write(client, OV5640_Y_OUTPUT_SIZE_LO, height & 0xff);
+	if (ret)
+		goto done;
+
+	ret = ov5640_reg_write(client, OV5640_ISP_CTRL1E, scale_input_x >> 8);
+	if (ret)
+		goto done;
+	ret = ov5640_reg_write(client, OV5640_ISP_CTRL1F, scale_input_x & 0xff);
+	if (ret)
+		goto done;
+	ret = ov5640_reg_write(client, OV5640_ISP_CTRL20, scale_input_y >> 8);
+	if (ret)
+		goto done;
+	ret = ov5640_reg_write(client, OV5640_ISP_CTRL21, scale_input_y & 0xff);
+	if (ret)
+		goto done;
+
+	ret = ov5640_reg_write(client, OV5640_VFIFO_READ_START_HI,
+			       (scale_input_x - width) >> 8);
+	if (ret)
+		goto done;
+	ret = ov5640_reg_write(client, OV5640_VFIFO_READ_START_LO,
+			       (scale_input_x - width) & 0xff);
+	if (ret)
+		goto done;
+
+	ret = ov5640_reg_write(client, OV5640_ISP_CTRL00, 0xff);
+	if (ret)
+		goto done;
+	ret = ov5640_reg_write(client, OV5640_ISP_CTRL01, 0xef |
+							  (scaling << 4));
+	if (ret)
+		goto done;
+	ret = ov5640_reg_write(client, OV5640_ISP_CTRL03, 0xff);
+
+done:
+	return ret;
+#endif
+	return 0;
+}
+
+/* set the format we will capture in */
+static int ov5640_s_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5640_priv *priv = to_ov5640(sd);
+	enum v4l2_colorspace cspace;
+	enum v4l2_mbus_pixelcode code = mf->code;
+	int ret;
+
+	ov5640_res_roundup(&mf->width, &mf->height);
+
+	switch (code) {
+	case V4L2_MBUS_FMT_YUYV8_2X8:
+		cspace = V4L2_COLORSPACE_SRGB;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = ov5640_reg_write_array(client, ov5640_defaults,
+				     ARRAY_SIZE(ov5640_defaults));
+	if (ret < 0)
+		return ret;
+
+	ret = ov5640_set_res(client, mf->width, mf->height);
+	if (ret < 0)
+		return ret;
+
+	mf->code	= code;
+	mf->colorspace	= cspace;
+
+	memcpy(&priv->current_mf, mf, sizeof(struct v4l2_mbus_framefmt));
+
+	return ret;
+}
+
+static int ov5640_try_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_mbus_framefmt *mf)
+{
+	ov5640_res_roundup(&mf->width, &mf->height);
+
+	mf->field = V4L2_FIELD_NONE;
+	mf->code = V4L2_MBUS_FMT_YUYV8_2X8;
+	mf->colorspace = V4L2_COLORSPACE_SRGB;
+
+	return 0;
+}
+
+static int ov5640_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			   enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= ARRAY_SIZE(ov5640_codes))
+		return -EINVAL;
+
+	*code = ov5640_codes[index];
+
+	return 0;
+}
+
+static int ov5640_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	a->bounds.left		= 0;
+	a->bounds.top		= 0;
+	a->bounds.width		= OV5640_MAX_WIDTH;
+	a->bounds.height	= OV5640_MAX_HEIGHT;
+	a->defrect		= a->bounds;
+	a->type			= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int ov5640_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	a->c.left		= 0;
+	a->c.top		= 0;
+	a->c.width		= OV5640_MAX_WIDTH;
+	a->c.height		= OV5640_MAX_HEIGHT;
+	a->type			= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+/* Get status of additional camera capabilities */
+static int ov5640_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct ov5640_priv *priv = to_ov5640(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 ov5640_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct ov5640_priv *priv = to_ov5640(sd);
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		priv->flag_vflip = ctrl->value;
+		break;
+	case V4L2_CID_HFLIP:
+		priv->flag_hflip = ctrl->value;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* Get chip identification */
+static int ov5640_g_chip_ident(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_chip_ident *id)
+{
+	struct ov5640_priv *priv = to_ov5640(sd);
+
+	id->ident = priv->ident;
+	id->revision = priv->revision;
+
+	return 0;
+}
+
+static int ov5640_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct ov5640_priv *priv = to_ov5640(sd);
+
+	if (!priv->current_enable)
+		return 0;
+
+	if (on) {
+		ov5640_s_fmt(sd, &priv->current_mf);
+		ov5640_s_stream(sd, priv->current_enable);
+	} else {
+		ov5640_s_stream(sd, 0);
+		priv->current_enable = true;
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov5640_get_register(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u8 val;
+
+	if (reg->reg & ~0xffff)
+		return -EINVAL;
+
+	reg->size = 2;
+
+	ret = ov5640_reg_read(client, reg->reg, &val);
+	if (ret)
+		return ret;
+
+	reg->val = (__u64)val;
+
+	return ret;
+}
+
+static int ov5640_set_register(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg & ~0xffff || reg->val & ~0xff)
+		return -EINVAL;
+
+	return ov5640_reg_write(client, reg->reg, reg->val);
+}
+#endif
+
+static int ov5640_video_probe(struct soc_camera_device *icd,
+			      struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov5640_priv *priv = to_ov5640(sd);
+	u8 chip_id_hi, chip_id_lo;
+	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
+	 */
+	ret = ov5640_reg_read(client, OV5640_CHIP_ID_HI, &chip_id_hi);
+	if (ret < 0)
+		goto err;
+
+	ret = ov5640_reg_read(client, OV5640_CHIP_ID_LO, &chip_id_lo);
+	if (ret < 0)
+		goto err;
+
+	priv->chip_id = (chip_id_hi << 8) | chip_id_lo;
+
+	if (priv->chip_id != 0x5640) {
+		ret = -ENODEV;
+		goto err;
+	}
+
+	priv->ident = V4L2_IDENT_OV5640;
+
+	dev_info(&client->dev, "Chip ID 0x%04x\n", priv->chip_id);
+
+err:
+	return ret;
+}
+
+static struct soc_camera_ops ov5640_ops = {
+	.set_bus_param		= ov5640_set_bus_param,
+	.query_bus_param	= ov5640_query_bus_param,
+	.controls		= ov5640_controls,
+	.num_controls		= ARRAY_SIZE(ov5640_controls),
+};
+
+static struct v4l2_subdev_video_ops ov5640_video_ops = {
+	.s_stream		= ov5640_s_stream,
+	.s_mbus_fmt		= ov5640_s_fmt,
+	.try_mbus_fmt		= ov5640_try_fmt,
+	.enum_mbus_fmt		= ov5640_enum_fmt,
+	.cropcap		= ov5640_cropcap,
+	.g_crop			= ov5640_g_crop,
+};
+
+static struct v4l2_subdev_core_ops ov5640_core_ops = {
+	.g_ctrl			= ov5640_g_ctrl,
+	.s_ctrl			= ov5640_s_ctrl,
+	.g_chip_ident		= ov5640_g_chip_ident,
+	.s_power		= ov5640_s_power,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register		= ov5640_get_register,
+	.s_register		= ov5640_set_register,
+#endif
+};
+
+static struct v4l2_subdev_ops ov5640_subdev_ops = {
+	.core			= &ov5640_core_ops,
+	.video			= &ov5640_video_ops,
+};
+
+/*
+ * i2c_driver function
+ */
+static int ov5640_probe(struct i2c_client *client,
+			const struct i2c_device_id *did)
+{
+	struct ov5640_priv *priv;
+	struct soc_camera_device *icd	= client->dev.platform_data;
+	struct soc_camera_link *icl;
+	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;
+	}
+
+	priv = kzalloc(sizeof(struct ov5640_priv), GFP_KERNEL);
+	if (!priv) {
+		dev_err(&client->dev, "Failed to allocate private data!\n");
+		return -ENOMEM;
+	}
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &ov5640_subdev_ops);
+
+	icd->ops = &ov5640_ops;
+
+	ret = ov5640_video_probe(icd, client);
+	if (ret < 0) {
+		icd->ops = NULL;
+		kfree(priv);
+	}
+
+	return ret;
+}
+
+static int ov5640_remove(struct i2c_client *client)
+{
+	struct ov5640_priv *priv = i2c_get_clientdata(client);
+
+	kfree(priv);
+
+	return 0;
+}
+
+static const struct i2c_device_id ov5640_id[] = {
+	{ "ov5640", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ov5640_id);
+
+static struct i2c_driver ov5640_i2c_driver = {
+	.driver = {
+		.name = "ov5640",
+	},
+	.probe    = ov5640_probe,
+	.remove   = ov5640_remove,
+	.id_table = ov5640_id,
+};
+
+static int __init ov5640_module_init(void)
+{
+	return i2c_add_driver(&ov5640_i2c_driver);
+}
+
+static void __exit ov5640_module_exit(void)
+{
+	i2c_del_driver(&ov5640_i2c_driver);
+}
+
+module_init(ov5640_module_init);
+module_exit(ov5640_module_exit);
+
+MODULE_DESCRIPTION("SoC Camera driver for OmniVision OV5640");
+MODULE_AUTHOR("Andrew Chew <achew@nvidia.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/video/tegra/nvavp/nvavp_dev.c b/drivers/media/video/tegra/nvavp/nvavp_dev.c
index 4501abb9a735..d5edcbe685bf 100644
--- a/drivers/media/video/tegra/nvavp/nvavp_dev.c
+++ b/drivers/media/video/tegra/nvavp/nvavp_dev.c
@@ -1355,7 +1355,6 @@ static int tegra_nvavp_open(struct inode *inode, struct file *filp, int channel_
 	if (!ret)
 		nvavp->refcount++;
 
-	clientctx->nvmap = nvavp->nvmap;
 	clientctx->nvavp = nvavp;
 
 	filp->private_data = clientctx;
diff --git a/drivers/media/video/tegra_v4l2_camera.c b/drivers/media/video/tegra_v4l2_camera.c
new file mode 100644
index 000000000000..644d0be53803
--- /dev/null
+++ b/drivers/media/video/tegra_v4l2_camera.c
@@ -0,0 +1,1765 @@
+/*
+ * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/pm_runtime.h>
+#include <linux/nvhost.h>
+
+#include <mach/iomap.h>
+
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
+#include <media/videobuf2-dma-nvmap.h>
+#include <media/tegra_v4l2_camera.h>
+
+#include "dev.h"
+#include "bus_client.h"
+#include "host1x/host1x_syncpt.h"
+
+#define TEGRA_CAM_DRV_NAME "vi"
+#define TEGRA_CAM_VERSION_CODE KERNEL_VERSION(0, 0, 5)
+
+#define TEGRA_SYNCPT_VI_WAIT_TIMEOUT                    200
+#define TEGRA_SYNCPT_CSI_WAIT_TIMEOUT                   200
+
+#define TEGRA_SYNCPT_RETRY_COUNT			10
+
+#define TEGRA_VIP_H_ACTIVE_START			0x98
+#define TEGRA_VIP_V_ACTIVE_START			0x10
+
+/* SYNCPTs 12-17 are reserved for VI. */
+#define TEGRA_VI_SYNCPT_VI                              NVSYNCPT_VI_ISP_2
+#define TEGRA_VI_SYNCPT_CSI                             NVSYNCPT_VI_ISP_3
+
+/* Tegra CSI-MIPI registers. */
+#define TEGRA_VI_OUT_1_INCR_SYNCPT			0x0000
+#define TEGRA_VI_OUT_1_INCR_SYNCPT_CNTRL		0x0004
+#define TEGRA_VI_OUT_1_INCR_SYNCPT_ERROR		0x0008
+#define TEGRA_VI_OUT_2_INCR_SYNCPT			0x0020
+#define TEGRA_VI_OUT_2_INCR_SYNCPT_CNTRL		0x0024
+#define TEGRA_VI_OUT_2_INCR_SYNCPT_ERROR		0x0028
+#define TEGRA_VI_MISC_INCR_SYNCPT			0x0040
+#define TEGRA_VI_MISC_INCR_SYNCPT_CNTRL			0x0044
+#define TEGRA_VI_MISC_INCR_SYNCPT_ERROR			0x0048
+#define TEGRA_VI_CONT_SYNCPT_OUT_1			0x0060
+#define TEGRA_VI_CONT_SYNCPT_OUT_2			0x0064
+#define TEGRA_VI_CONT_SYNCPT_VIP_VSYNC			0x0068
+#define TEGRA_VI_CONT_SYNCPT_VI2EPP			0x006c
+#define TEGRA_VI_CONT_SYNCPT_CSI_PPA_FRAME_START	0x0070
+#define TEGRA_VI_CONT_SYNCPT_CSI_PPA_FRAME_END		0x0074
+#define TEGRA_VI_CONT_SYNCPT_CSI_PPB_FRAME_START	0x0078
+#define TEGRA_VI_CONT_SYNCPT_CSI_PPB_FRAME_END		0x007c
+#define TEGRA_VI_CTXSW					0x0080
+#define TEGRA_VI_INTSTATUS				0x0084
+#define TEGRA_VI_VI_INPUT_CONTROL			0x0088
+#define TEGRA_VI_VI_CORE_CONTROL			0x008c
+#define TEGRA_VI_VI_FIRST_OUTPUT_CONTROL		0x0090
+#define TEGRA_VI_VI_SECOND_OUTPUT_CONTROL		0x0094
+#define TEGRA_VI_HOST_INPUT_FRAME_SIZE			0x0098
+#define TEGRA_VI_HOST_H_ACTIVE				0x009c
+#define TEGRA_VI_HOST_V_ACTIVE				0x00a0
+#define TEGRA_VI_VIP_H_ACTIVE				0x00a4
+#define TEGRA_VI_VIP_V_ACTIVE				0x00a8
+#define TEGRA_VI_VI_PEER_CONTROL			0x00ac
+#define TEGRA_VI_VI_DMA_SELECT				0x00b0
+#define TEGRA_VI_HOST_DMA_WRITE_BUFFER			0x00b4
+#define TEGRA_VI_HOST_DMA_BASE_ADDRESS			0x00b8
+#define TEGRA_VI_HOST_DMA_WRITE_BUFFER_STATUS		0x00bc
+#define TEGRA_VI_HOST_DMA_WRITE_PEND_BUFCOUNT		0x00c0
+#define TEGRA_VI_VB0_START_ADDRESS_FIRST		0x00c4
+#define TEGRA_VI_VB0_BASE_ADDRESS_FIRST			0x00c8
+#define TEGRA_VI_VB0_START_ADDRESS_U			0x00cc
+#define TEGRA_VI_VB0_BASE_ADDRESS_U			0x00d0
+#define TEGRA_VI_VB0_START_ADDRESS_V			0x00d4
+#define TEGRA_VI_VB0_BASE_ADDRESS_V			0x00d8
+#define TEGRA_VI_VB_SCRATCH_ADDRESS_UV			0x00dc
+#define TEGRA_VI_FIRST_OUTPUT_FRAME_SIZE		0x00e0
+#define TEGRA_VI_VB0_COUNT_FIRST			0x00e4
+#define TEGRA_VI_VB0_SIZE_FIRST				0x00e8
+#define TEGRA_VI_VB0_BUFFER_STRIDE_FIRST		0x00ec
+#define TEGRA_VI_VB0_START_ADDRESS_SECOND		0x00f0
+#define TEGRA_VI_VB0_BASE_ADDRESS_SECOND		0x00f4
+#define TEGRA_VI_SECOND_OUTPUT_FRAME_SIZE		0x00f8
+#define TEGRA_VI_VB0_COUNT_SECOND			0x00fc
+#define TEGRA_VI_VB0_SIZE_SECOND			0x0100
+#define TEGRA_VI_VB0_BUFFER_STRIDE_SECOND		0x0104
+#define TEGRA_VI_H_LPF_CONTROL				0x0108
+#define TEGRA_VI_H_DOWNSCALE_CONTROL			0x010c
+#define TEGRA_VI_V_DOWNSCALE_CONTROL			0x0110
+#define TEGRA_VI_CSC_Y					0x0114
+#define TEGRA_VI_CSC_UV_R				0x0118
+#define TEGRA_VI_CSC_UV_G				0x011c
+#define TEGRA_VI_CSC_UV_B				0x0120
+#define TEGRA_VI_CSC_ALPHA				0x0124
+#define TEGRA_VI_HOST_VSYNC				0x0128
+#define TEGRA_VI_COMMAND				0x012c
+#define TEGRA_VI_HOST_FIFO_STATUS			0x0130
+#define TEGRA_VI_INTERRUPT_MASK				0x0134
+#define TEGRA_VI_INTERRUPT_TYPE_SELECT			0x0138
+#define TEGRA_VI_INTERRUPT_POLARITY_SELECT		0x013c
+#define TEGRA_VI_INTERRUPT_STATUS			0x0140
+#define TEGRA_VI_VIP_INPUT_STATUS			0x0144
+#define TEGRA_VI_VIDEO_BUFFER_STATUS			0x0148
+#define TEGRA_VI_SYNC_OUTPUT				0x014c
+#define TEGRA_VI_VVS_OUTPUT_DELAY			0x0150
+#define TEGRA_VI_PWM_CONTROL				0x0154
+#define TEGRA_VI_PWM_SELECT_PULSE_A			0x0158
+#define TEGRA_VI_PWM_SELECT_PULSE_B			0x015c
+#define TEGRA_VI_PWM_SELECT_PULSE_C			0x0160
+#define TEGRA_VI_PWM_SELECT_PULSE_D			0x0164
+#define TEGRA_VI_VI_DATA_INPUT_CONTROL			0x0168
+#define TEGRA_VI_PIN_INPUT_ENABLE			0x016c
+#define TEGRA_VI_PIN_OUTPUT_ENABLE			0x0170
+#define TEGRA_VI_PIN_INVERSION				0x0174
+#define TEGRA_VI_PIN_INPUT_DATA				0x0178
+#define TEGRA_VI_PIN_OUTPUT_DATA			0x017c
+#define TEGRA_VI_PIN_OUTPUT_SELECT			0x0180
+#define TEGRA_VI_RAISE_VIP_BUFFER_FIRST_OUTPUT		0x0184
+#define TEGRA_VI_RAISE_VIP_FRAME_FIRST_OUTPUT		0x0188
+#define TEGRA_VI_RAISE_VIP_BUFFER_SECOND_OUTPUT		0x018c
+#define TEGRA_VI_RAISE_VIP_FRAME_SECOND_OUTPUT		0x0190
+#define TEGRA_VI_RAISE_HOST_FIRST_OUTPUT		0x0194
+#define TEGRA_VI_RAISE_HOST_SECOND_OUTPUT		0x0198
+#define TEGRA_VI_RAISE_EPP				0x019c
+#define TEGRA_VI_CAMERA_CONTROL				0x01a0
+#define TEGRA_VI_VI_ENABLE				0x01a4
+#define TEGRA_VI_VI_ENABLE_2				0x01a8
+#define TEGRA_VI_VI_RAISE				0x01ac
+#define TEGRA_VI_Y_FIFO_WRITE				0x01b0
+#define TEGRA_VI_U_FIFO_WRITE				0x01b4
+#define TEGRA_VI_V_FIFO_WRITE				0x01b8
+#define TEGRA_VI_VI_MCCIF_FIFOCTRL			0x01bc
+#define TEGRA_VI_TIMEOUT_WCOAL_VI			0x01c0
+#define TEGRA_VI_MCCIF_VIRUV_HP				0x01c4
+#define TEGRA_VI_MCCIF_VIWSB_HP				0x01c8
+#define TEGRA_VI_MCCIF_VIWU_HP				0x01cc
+#define TEGRA_VI_MCCIF_VIWV_HP				0x01d0
+#define TEGRA_VI_MCCIF_VIWY_HP				0x01d4
+#define TEGRA_VI_CSI_PPA_RAISE_FRAME_START		0x01d8
+#define TEGRA_VI_CSI_PPA_RAISE_FRAME_END		0x01dc
+#define TEGRA_VI_CSI_PPB_RAISE_FRAME_START		0x01e0
+#define TEGRA_VI_CSI_PBB_RAISE_FRAME_END		0x01e4
+#define TEGRA_VI_CSI_PPA_H_ACTIVE			0x01e8
+#define TEGRA_VI_CSI_PPA_V_ACTIVE			0x01ec
+#define TEGRA_VI_CSI_PPB_H_ACTIVE			0x01f0
+#define TEGRA_VI_CSI_PPB_V_ACTIVE			0x01f4
+#define TEGRA_VI_ISP_H_ACTIVE				0x01f8
+#define TEGRA_VI_ISP_V_ACTIVE				0x01fc
+#define TEGRA_VI_STREAM_1_RESOURCE_DEFINE		0x0200
+#define TEGRA_VI_STREAM_2_RESOURCE_DEFINE		0x0204
+#define TEGRA_VI_RAISE_STREAM_1_DONE			0x0208
+#define TEGRA_VI_RAISE_STREAM_2_DONE			0x020c
+#define TEGRA_VI_TS_MODE				0x0210
+#define TEGRA_VI_TS_CONTROL				0x0214
+#define TEGRA_VI_TS_PACKET_COUNT			0x0218
+#define TEGRA_VI_TS_ERROR_COUNT				0x021c
+#define TEGRA_VI_TS_CPU_FLOW_CTL			0x0220
+#define TEGRA_VI_VB0_CHROMA_BUFFER_STRIDE_FIRST		0x0224
+#define TEGRA_VI_VB0_CHROMA_LINE_STRIDE_FIRST		0x0228
+#define TEGRA_VI_EPP_LINES_PER_BUFFER			0x022c
+#define TEGRA_VI_BUFFER_RELEASE_OUTPUT1			0x0230
+#define TEGRA_VI_BUFFER_RELEASE_OUTPUT2			0x0234
+#define TEGRA_VI_DEBUG_FLOW_CONTROL_COUNTER_OUTPUT1	0x0238
+#define TEGRA_VI_DEBUG_FLOW_CONTROL_COUNTER_OUTPUT2	0x023c
+#define TEGRA_VI_TERMINATE_BW_FIRST			0x0240
+#define TEGRA_VI_TERMINATE_BW_SECOND			0x0244
+#define TEGRA_VI_VB0_FIRST_BUFFER_ADDR_MODE		0x0248
+#define TEGRA_VI_VB0_SECOND_BUFFER_ADDR_MODE		0x024c
+#define TEGRA_VI_RESERVE_0				0x0250
+#define TEGRA_VI_RESERVE_1				0x0254
+#define TEGRA_VI_RESERVE_2				0x0258
+#define TEGRA_VI_RESERVE_3				0x025c
+#define TEGRA_VI_RESERVE_4				0x0260
+#define TEGRA_VI_MCCIF_VIRUV_HYST			0x0264
+#define TEGRA_VI_MCCIF_VIWSB_HYST			0x0268
+#define TEGRA_VI_MCCIF_VIWU_HYST			0x026c
+#define TEGRA_VI_MCCIF_VIWV_HYST			0x0270
+#define TEGRA_VI_MCCIF_VIWY_HYST			0x0274
+
+#define TEGRA_CSI_VI_INPUT_STREAM_CONTROL		0x0800
+#define TEGRA_CSI_HOST_INPUT_STREAM_CONTROL		0x0808
+#define TEGRA_CSI_INPUT_STREAM_A_CONTROL		0x0810
+#define TEGRA_CSI_PIXEL_STREAM_A_CONTROL0		0x0818
+#define TEGRA_CSI_PIXEL_STREAM_A_CONTROL1		0x081c
+#define TEGRA_CSI_PIXEL_STREAM_A_WORD_COUNT		0x0820
+#define TEGRA_CSI_PIXEL_STREAM_A_GAP			0x0824
+#define TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND		0x0828
+#define TEGRA_CSI_INPUT_STREAM_B_CONTROL		0x083c
+#define TEGRA_CSI_PIXEL_STREAM_B_CONTROL0		0x0844
+#define TEGRA_CSI_PIXEL_STREAM_B_CONTROL1		0x0848
+#define TEGRA_CSI_PIXEL_STREAM_B_WORD_COUNT		0x084c
+#define TEGRA_CSI_PIXEL_STREAM_B_GAP			0x0850
+#define TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND		0x0854
+#define TEGRA_CSI_PHY_CIL_COMMAND			0x0868
+#define TEGRA_CSI_PHY_CILA_CONTROL0			0x086c
+#define TEGRA_CSI_PHY_CILB_CONTROL0			0x0870
+#define TEGRA_CSI_CSI_PIXEL_PARSER_STATUS		0x0878
+#define TEGRA_CSI_CSI_CIL_STATUS			0x087c
+#define TEGRA_CSI_CSI_PIXEL_PARSER_INTERRUPT_MASK	0x0880
+#define TEGRA_CSI_CSI_CIL_INTERRUPT_MASK		0x0884
+#define TEGRA_CSI_CSI_READONLY_STATUS			0x0888
+#define TEGRA_CSI_ESCAPE_MODE_COMMAND			0x088c
+#define TEGRA_CSI_ESCAPE_MODE_DATA			0x0890
+#define TEGRA_CSI_CILA_PAD_CONFIG0			0x0894
+#define TEGRA_CSI_CILA_PAD_CONFIG1			0x0898
+#define TEGRA_CSI_CILB_PAD_CONFIG0			0x089c
+#define TEGRA_CSI_CILB_PAD_CONFIG1			0x08a0
+#define TEGRA_CSI_CIL_PAD_CONFIG			0x08a4
+#define TEGRA_CSI_CILA_MIPI_CAL_CONFIG			0x08a8
+#define TEGRA_CSI_CILB_MIPI_CAL_CONFIG			0x08ac
+#define TEGRA_CSI_CIL_MIPI_CAL_STATUS			0x08b0
+#define TEGRA_CSI_CLKEN_OVERRIDE			0x08b4
+#define TEGRA_CSI_DEBUG_CONTROL				0x08b8
+#define TEGRA_CSI_DEBUG_COUNTER_0			0x08bc
+#define TEGRA_CSI_DEBUG_COUNTER_1			0x08c0
+#define TEGRA_CSI_DEBUG_COUNTER_2			0x08c4
+#define TEGRA_CSI_PIXEL_STREAM_A_EXPECTED_FRAME		0x08c8
+#define TEGRA_CSI_PIXEL_STREAM_B_EXPECTED_FRAME		0x08cc
+#define TEGRA_CSI_DSI_MIPI_CAL_CONFIG			0x08d0
+
+#define TC_VI_REG_RD(DEV, REG) readl(DEV->vi_base + REG)
+#define TC_VI_REG_WT(DEV, REG, VAL) writel(VAL, DEV->vi_base + REG)
+
+#define tegra_camera_port_is_valid(port) \
+	(((port) >= TEGRA_CAMERA_PORT_CSI_A) && \
+	 ((port) <= TEGRA_CAMERA_PORT_VIP))
+
+#define tegra_camera_port_is_csi(port) \
+	(((port) == TEGRA_CAMERA_PORT_CSI_A) || \
+	 ((port) == TEGRA_CAMERA_PORT_CSI_B))
+
+/*
+ * Structures
+ */
+
+/* buffer for one video frame */
+struct tegra_buffer {
+	struct vb2_buffer		vb; /* v4l buffer must be first */
+	struct list_head		queue;
+
+	/*
+	 * Various buffer addresses shadowed so we don't have to recalculate
+	 * per frame.  These are calculated during videobuf_prepare.
+	 */
+	dma_addr_t			buffer_addr;
+	dma_addr_t			buffer_addr_u;
+	dma_addr_t			buffer_addr_v;
+	dma_addr_t			start_addr;
+	dma_addr_t			start_addr_u;
+	dma_addr_t			start_addr_v;
+};
+
+struct tegra_camera_dev {
+	struct soc_camera_host		ici;
+	struct soc_camera_device	*icd;
+	struct nvhost_device		*ndev;
+	struct tegra_camera_platform_data *pdata;
+
+	struct clk			*clk_vi;
+	struct clk			*clk_vi_sensor;
+	struct clk			*clk_csi;
+	struct clk			*clk_isp;
+	struct clk			*clk_csus;
+
+	void __iomem			*vi_base;
+	spinlock_t			videobuf_queue_lock;
+	struct list_head		capture;
+	struct vb2_buffer		*active;
+	struct vb2_alloc_ctx		*alloc_ctx;
+	enum v4l2_field			field;
+	int				sequence;
+
+	struct work_struct		work;
+	struct mutex			work_mutex;
+
+	u32				syncpt_vi;
+	u32				syncpt_csi;
+
+	/* Debug */
+	int num_frames;
+};
+
+static const struct soc_mbus_pixelfmt tegra_camera_formats[] = {
+	{
+		.fourcc			= V4L2_PIX_FMT_UYVY,
+		.name			= "YUV422 (UYVY) packed",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_VYUY,
+		.name			= "YUV422 (VYUY) packed",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_YUYV,
+		.name			= "YUV422 (YUYV) packed",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_YVYU,
+		.name			= "YUV422 (YVYU) packed",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_YUV420,
+		.name			= "YUV420 (YU12) planar",
+		.bits_per_sample	= 12,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_YVU420,
+		.name			= "YVU420 (YV12) planar",
+		.bits_per_sample	= 12,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+};
+
+static struct tegra_buffer *to_tegra_vb(struct vb2_buffer *vb)
+{
+	return container_of(vb, struct tegra_buffer, vb);
+}
+
+static void tegra_camera_save_syncpts(struct tegra_camera_dev *pcdev)
+{
+	pcdev->syncpt_csi =
+		nvhost_syncpt_read_ext(pcdev->ndev,
+				       TEGRA_VI_SYNCPT_CSI);
+
+	pcdev->syncpt_vi =
+		nvhost_syncpt_read_ext(pcdev->ndev,
+				       TEGRA_VI_SYNCPT_VI);
+}
+
+static void tegra_camera_incr_syncpts(struct tegra_camera_dev *pcdev)
+{
+	nvhost_syncpt_cpu_incr_ext(pcdev->ndev,
+				   TEGRA_VI_SYNCPT_CSI);
+
+	nvhost_syncpt_cpu_incr_ext(pcdev->ndev,
+				   TEGRA_VI_SYNCPT_VI);
+}
+
+static void tegra_camera_capture_setup_csi_a(struct tegra_camera_dev *pcdev,
+					     int input_format,
+					     int yuv_input_format)
+{
+	struct soc_camera_device *icd = pcdev->icd;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+						icd->current_fmt->host_fmt);
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_CORE_CONTROL, 0x02000000);
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_INPUT_CONTROL,
+		(yuv_input_format << 8) |
+		(input_format << 2));
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_H_DOWNSCALE_CONTROL, 0x00000004);
+	TC_VI_REG_WT(pcdev, TEGRA_VI_V_DOWNSCALE_CONTROL, 0x00000004);
+
+	/* CSI-A H_ACTIVE and V_ACTIVE */
+	TC_VI_REG_WT(pcdev, TEGRA_VI_CSI_PPA_H_ACTIVE,
+		     (icd->user_width << 16));
+	TC_VI_REG_WT(pcdev, TEGRA_VI_CSI_PPA_V_ACTIVE,
+		     (icd->user_height << 16));
+
+	/* CSI A */
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_VI_INPUT_STREAM_CONTROL, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_HOST_INPUT_STREAM_CONTROL, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_INPUT_STREAM_A_CONTROL, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_CONTROL0, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_CONTROL1, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_WORD_COUNT, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_GAP, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND, 0x00000000);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILA_PAD_CONFIG0, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILA_PAD_CONFIG1, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CIL_PAD_CONFIG, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILA_MIPI_CAL_CONFIG, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CIL_MIPI_CAL_STATUS, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CLKEN_OVERRIDE, 0x00000000);
+
+	/* pad1s enabled, virtual channel ID 00 */
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_CONTROL0,
+		(0x1 << 16) | /* Output 1 pixel per clock */
+		(0x1e << 8) | /* If hdr shows wrong fmt, use YUV422 */
+		(0x1 << 7) | /* Check header CRC */
+		(0x1 << 6) | /* Use word count field in the header */
+		(0x1 << 5) | /* Look at data identifier byte in hdr */
+		(0x1 << 4)); /* Expect packet header */
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_CONTROL1,
+		0x1); /* Frame # for top field detect for interlaced */
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_WORD_COUNT,
+		bytes_per_line);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_GAP, 0x00140000);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_EXPECTED_FRAME,
+		(icd->user_height << 16) |
+		(0x100 << 4) | /* Wait 0x100 vi clks for timeout */
+		0x1); /* Enable line timeout */
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_INPUT_STREAM_A_CONTROL,
+		     (0x3f << 16) | /* Skip packet threshold */
+		     (pcdev->pdata->lanes - 1));
+
+	/* Use 0x00000022 for continuous clock mode. */
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CILA_CONTROL0,
+		(pcdev->pdata->continuous_clk << 5) |
+		0x5); /* Clock settle time */
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_CSI_PPA_FRAME_END,
+		(0x1 << 8) | /* Enable continuous syncpt */
+		TEGRA_VI_SYNCPT_CSI);
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_OUT_1,
+		(0x1 << 8) | /* Enable continuous syncpt */
+		TEGRA_VI_SYNCPT_VI);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CIL_COMMAND, 0x00020001);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND, 0x0000f002);
+}
+
+static void tegra_camera_capture_setup_csi_b(struct tegra_camera_dev *pcdev,
+					     int input_format,
+					     int yuv_input_format)
+{
+	struct soc_camera_device *icd = pcdev->icd;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+						icd->current_fmt->host_fmt);
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_CORE_CONTROL, 0x04000000);
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_INPUT_CONTROL,
+		(yuv_input_format << 8) |
+		(input_format << 2));
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_H_DOWNSCALE_CONTROL, 0x00000008);
+	TC_VI_REG_WT(pcdev, TEGRA_VI_V_DOWNSCALE_CONTROL, 0x00000008);
+
+	/* CSI-B H_ACTIVE and V_ACTIVE */
+	TC_VI_REG_WT(pcdev, TEGRA_VI_CSI_PPB_H_ACTIVE,
+		(icd->user_width << 16));
+	TC_VI_REG_WT(pcdev, TEGRA_VI_CSI_PPB_V_ACTIVE,
+		(icd->user_height << 16));
+
+	/* CSI B */
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_VI_INPUT_STREAM_CONTROL, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_HOST_INPUT_STREAM_CONTROL, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_INPUT_STREAM_B_CONTROL, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_CONTROL0, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_CONTROL1, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_WORD_COUNT, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_GAP, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND, 0x00000000);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILB_PAD_CONFIG0, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILB_PAD_CONFIG1, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CIL_PAD_CONFIG, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILB_MIPI_CAL_CONFIG, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CIL_MIPI_CAL_STATUS, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CLKEN_OVERRIDE, 0x00000000);
+
+	/* pad1s enabled, virtual channel ID 00 */
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_CONTROL0,
+		(0x1 << 16) | /* Output 1 pixel per clock */
+		(0x1e << 8) | /* If hdr shows wrong fmt, use YUV422 */
+		(0x1 << 7) | /* Check header CRC */
+		(0x1 << 6) | /* Use word count field in the header */
+		(0x1 << 5) | /* Look at data identifier byte in hdr */
+		(0x1 << 4) | /* Expect packet header */
+		0x1); /* Set PPB stream source to CSI B */
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_CONTROL1,
+		0x1); /* Frame # for top field detect for interlaced */
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_WORD_COUNT,
+		bytes_per_line);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_GAP, 0x00140000);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_EXPECTED_FRAME,
+		(icd->user_height << 16) |
+		(0x100 << 4) | /* Wait 0x100 vi clks for timeout */
+		0x1); /* Enable line timeout */
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_INPUT_STREAM_B_CONTROL,
+		     (0x3f << 16) | /* Skip packet threshold */
+		     (pcdev->pdata->lanes - 1));
+
+	/* Use 0x00000022 for continuous clock mode. */
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CILB_CONTROL0,
+		(pcdev->pdata->continuous_clk << 5) |
+		0x5); /* Clock settle time */
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_CSI_PPB_FRAME_END,
+		(0x1 << 8) | /* Enable continuous syncpt */
+		TEGRA_VI_SYNCPT_CSI);
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_OUT_1,
+		(0x1 << 8) | /* Enable continuous syncpt */
+		TEGRA_VI_SYNCPT_VI);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CIL_COMMAND, 0x00010002);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND, 0x0000f002);
+}
+
+static void tegra_camera_capture_setup_vip(struct tegra_camera_dev *pcdev,
+					     int input_format,
+					     int yuv_input_format)
+{
+	struct soc_camera_device *icd = pcdev->icd;
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_CORE_CONTROL, 0x00000000);
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_INPUT_CONTROL,
+		(1 << 27) | /* field detect */
+		(1 << 25) | /* hsync/vsync decoded from data (BT.656) */
+		(yuv_input_format << 8) |
+		(1 << 1) | /* VIP_INPUT_ENABLE */
+		(input_format << 2));
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_H_DOWNSCALE_CONTROL, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_VI_V_DOWNSCALE_CONTROL, 0x00000000);
+
+	/* VIP H_ACTIVE and V_ACTIVE */
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VIP_H_ACTIVE,
+		(icd->user_width << 16) |
+		TEGRA_VIP_H_ACTIVE_START);
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VIP_V_ACTIVE,
+		(icd->user_height << 16) |
+		TEGRA_VIP_V_ACTIVE_START);
+
+	/*
+	 * For VIP, D9..D2 is mapped to the video decoder's P7..P0.
+	 * Disable/mask out the other Dn wires.
+	 */
+	TC_VI_REG_WT(pcdev, TEGRA_VI_PIN_INPUT_ENABLE, 0x000003fc);
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_DATA_INPUT_CONTROL, 0x000003fc);
+	TC_VI_REG_WT(pcdev, TEGRA_VI_PIN_INVERSION, 0x00000000);
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_VIP_VSYNC,
+		(0x1 << 8) | /* Enable continuous syncpt */
+		TEGRA_VI_SYNCPT_VI);
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_CAMERA_CONTROL, 0x00000004);
+}
+
+static void tegra_camera_capture_setup(struct tegra_camera_dev *pcdev)
+{
+	struct soc_camera_device *icd = pcdev->icd;
+	const struct soc_camera_format_xlate *current_fmt = icd->current_fmt;
+	enum v4l2_mbus_pixelcode input_code = current_fmt->code;
+	u32 output_fourcc = current_fmt->host_fmt->fourcc;
+	int yuv_input_format = 0x0;
+	int input_format = 0x0; /* Default to YUV422 */
+	int yuv_output_format = 0x0;
+	int output_format = 0x3; /* Default to YUV422 */
+	int port = pcdev->pdata->port;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+						icd->current_fmt->host_fmt);
+
+	switch (input_code) {
+	case V4L2_MBUS_FMT_UYVY8_2X8:
+		yuv_input_format = 0x2;
+		break;
+	case V4L2_MBUS_FMT_VYUY8_2X8:
+		yuv_input_format = 0x3;
+		break;
+	case V4L2_MBUS_FMT_YUYV8_2X8:
+		yuv_input_format = 0x0;
+		break;
+	case V4L2_MBUS_FMT_YVYU8_2X8:
+		yuv_input_format = 0x1;
+		break;
+	default:
+		BUG_ON(1);
+	}
+
+	switch (output_fourcc) {
+	case V4L2_PIX_FMT_UYVY:
+		yuv_output_format = 0x0;
+		break;
+	case V4L2_PIX_FMT_VYUY:
+		yuv_output_format = 0x1;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+		yuv_output_format = 0x2;
+		break;
+	case V4L2_PIX_FMT_YVYU:
+		yuv_output_format = 0x3;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		output_format = 0x6; /* YUV420 planar */
+		break;
+	default:
+		BUG_ON(1);
+	}
+
+	BUG_ON(!tegra_camera_port_is_valid(port));
+
+	/*
+	 * Set up low pass filter.  Use 0x240 for chromaticity and 0x240
+	 * for luminance, which is the default and means not to touch
+	 * anything.
+	 */
+	TC_VI_REG_WT(pcdev, TEGRA_VI_H_LPF_CONTROL, 0x02400240);
+
+	/* Set up raise-on-edge, so we get an interrupt on end of frame. */
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_RAISE, 0x00000001);
+
+	if (port == TEGRA_CAMERA_PORT_CSI_A)
+		tegra_camera_capture_setup_csi_a(pcdev, input_format,
+						 yuv_input_format);
+	else if (port == TEGRA_CAMERA_PORT_CSI_B)
+		tegra_camera_capture_setup_csi_b(pcdev, input_format,
+						 yuv_input_format);
+	else
+		tegra_camera_capture_setup_vip(pcdev, input_format,
+					       yuv_input_format);
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_FIRST_OUTPUT_CONTROL,
+		(pcdev->pdata->flip_v ? (0x1 << 20) : 0) |
+		(pcdev->pdata->flip_h ? (0x1 << 19) : 0) |
+		(yuv_output_format << 17) |
+		output_format);
+
+	/*
+	 * Set up frame size.  Bits 31:16 are the number of lines, and
+	 * bits 15:0 are the number of pixels per line.
+	 */
+	TC_VI_REG_WT(pcdev, TEGRA_VI_FIRST_OUTPUT_FRAME_SIZE,
+		(icd->user_height << 16) | icd->user_width);
+
+	/* First output memory enabled */
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_ENABLE, 0x00000000);
+
+	/* Set the number of frames in the buffer. */
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_COUNT_FIRST, 0x00000001);
+
+	/* Set up buffer frame size. */
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_SIZE_FIRST,
+		(icd->user_height << 16) | icd->user_width);
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BUFFER_STRIDE_FIRST,
+		(icd->user_height * bytes_per_line));
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_ENABLE, 0x00000000);
+}
+
+static int tegra_camera_capture_start(struct tegra_camera_dev *pcdev,
+				      struct tegra_buffer *buf)
+{
+	struct soc_camera_device *icd = pcdev->icd;
+	int port = pcdev->pdata->port;
+	int err;
+
+	pcdev->syncpt_csi++;
+	pcdev->syncpt_vi++;
+
+	switch (icd->current_fmt->host_fmt->fourcc) {
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BASE_ADDRESS_U,
+			     buf->buffer_addr_u);
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_START_ADDRESS_U,
+			     buf->start_addr_u);
+
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BASE_ADDRESS_V,
+			     buf->buffer_addr_v);
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_START_ADDRESS_V,
+			     buf->start_addr_v);
+
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_VYUY:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_YVYU:
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BASE_ADDRESS_FIRST,
+			     buf->buffer_addr);
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_START_ADDRESS_FIRST,
+			     buf->start_addr);
+
+		break;
+
+	default:
+		BUG_ON(1);
+	}
+
+	BUG_ON(!tegra_camera_port_is_valid(port));
+
+	if (port == TEGRA_CAMERA_PORT_CSI_A)
+		TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND,
+			     0x0000f005);
+	else if (port == TEGRA_CAMERA_PORT_CSI_B)
+		TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND,
+			     0x0000f005);
+	else
+		TC_VI_REG_WT(pcdev, TEGRA_VI_CAMERA_CONTROL,
+			     0x00000001);
+
+	/*
+	 * Only wait on CSI frame end syncpt if we're using CSI.  Otherwise,
+	 * wait on VIP VSYNC syncpt.
+	 */
+	if (tegra_camera_port_is_csi(port))
+		err = nvhost_syncpt_wait_timeout_ext(pcdev->ndev,
+			TEGRA_VI_SYNCPT_CSI,
+			pcdev->syncpt_csi,
+			TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
+			NULL);
+	else
+		err = nvhost_syncpt_wait_timeout_ext(pcdev->ndev,
+			TEGRA_VI_SYNCPT_VI,
+			pcdev->syncpt_csi,
+			TEGRA_SYNCPT_VI_WAIT_TIMEOUT,
+			NULL);
+
+	if (!err)
+		return 0;
+
+	if (tegra_camera_port_is_csi(port)) {
+		u32 ppstatus;
+		u32 cilstatus;
+		u32 rostatus;
+
+		dev_err(&pcdev->ndev->dev, "Timeout on CSI syncpt\n");
+		dev_err(&pcdev->ndev->dev, "buffer_addr = 0x%08x\n",
+			buf->buffer_addr);
+
+		ppstatus = TC_VI_REG_RD(pcdev,
+			TEGRA_CSI_CSI_PIXEL_PARSER_STATUS);
+		cilstatus = TC_VI_REG_RD(pcdev,
+			 TEGRA_CSI_CSI_CIL_STATUS);
+		rostatus = TC_VI_REG_RD(pcdev,
+			TEGRA_CSI_CSI_READONLY_STATUS);
+
+		dev_err(&pcdev->ndev->dev,
+			"PPSTATUS = 0x%08x, "
+			"CILSTATUS = 0x%08x, "
+			"ROSTATUS = 0x%08x\n",
+			ppstatus, cilstatus, rostatus);
+	} else {
+		u32 vip_input_status;
+
+		dev_err(&pcdev->ndev->dev, "Timeout on VI syncpt\n");
+		dev_err(&pcdev->ndev->dev, "buffer_addr = 0x%08x\n",
+			buf->buffer_addr);
+
+		vip_input_status = TC_VI_REG_RD(pcdev,
+			TEGRA_VI_VIP_INPUT_STATUS);
+
+		dev_err(&pcdev->ndev->dev,
+			"VIP_INPUT_STATUS = 0x%08x\n",
+			vip_input_status);
+	}
+
+	return err;
+}
+
+static int tegra_camera_capture_stop(struct tegra_camera_dev *pcdev)
+{
+	int port = pcdev->pdata->port;
+	int err;
+
+	BUG_ON(!tegra_camera_port_is_valid(port));
+
+	if (port == TEGRA_CAMERA_PORT_CSI_A)
+		TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND,
+			     0x0000f002);
+	else if (port == TEGRA_CAMERA_PORT_CSI_B)
+		TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND,
+			     0x0000f002);
+	else
+		TC_VI_REG_WT(pcdev, TEGRA_VI_CAMERA_CONTROL,
+			     0x00000005);
+
+	if (tegra_camera_port_is_csi(port))
+		err = nvhost_syncpt_wait_timeout_ext(pcdev->ndev,
+			TEGRA_VI_SYNCPT_VI,
+			pcdev->syncpt_vi,
+			TEGRA_SYNCPT_VI_WAIT_TIMEOUT,
+			NULL);
+	else
+		err = 0;
+
+	if (err) {
+		u32 buffer_addr;
+		u32 ppstatus;
+		u32 cilstatus;
+
+		dev_err(&pcdev->ndev->dev, "Timeout on VI syncpt\n");
+		buffer_addr = TC_VI_REG_RD(pcdev,
+					   TEGRA_VI_VB0_BASE_ADDRESS_FIRST);
+		dev_err(&pcdev->ndev->dev, "buffer_addr = 0x%08x\n",
+			buffer_addr);
+
+		ppstatus = TC_VI_REG_RD(pcdev,
+					TEGRA_CSI_CSI_PIXEL_PARSER_STATUS);
+		cilstatus = TC_VI_REG_RD(pcdev,
+					 TEGRA_CSI_CSI_CIL_STATUS);
+		dev_err(&pcdev->ndev->dev,
+			"PPSTATUS = 0x%08x, CILSTATUS = 0x%08x\n",
+			ppstatus, cilstatus);
+	}
+
+	return err;
+}
+
+static int tegra_camera_capture_frame(struct tegra_camera_dev *pcdev)
+{
+	struct vb2_buffer *vb;
+	struct tegra_buffer *buf;
+	int retry = TEGRA_SYNCPT_RETRY_COUNT;
+	int port = pcdev->pdata->port;
+	int err;
+
+	if (!pcdev->active)
+		return 0;
+
+	vb = pcdev->active;
+	buf = to_tegra_vb(vb);
+
+	while (retry) {
+		err = tegra_camera_capture_start(pcdev, buf);
+		if (!err)
+			err = tegra_camera_capture_stop(pcdev);
+
+		if (err != 0) {
+			retry--;
+
+			/* Stop streaming. */
+			if (port == TEGRA_CAMERA_PORT_CSI_A) {
+				TC_VI_REG_WT(pcdev,
+					     TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND,
+					     0x0000f002);
+				/* Clear status registers. */
+				TC_VI_REG_WT(pcdev,
+					     TEGRA_CSI_CSI_PIXEL_PARSER_STATUS,
+					     0xffffffff);
+				TC_VI_REG_WT(pcdev,
+					     TEGRA_CSI_CSI_CIL_STATUS,
+					     0xffffffff);
+			} else if (port == TEGRA_CAMERA_PORT_CSI_B) {
+				TC_VI_REG_WT(pcdev,
+					     TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND,
+					     0x0000f002);
+				/* Clear status registers. */
+				TC_VI_REG_WT(pcdev,
+					     TEGRA_CSI_CSI_PIXEL_PARSER_STATUS,
+					     0xffffffff);
+				TC_VI_REG_WT(pcdev,
+					     TEGRA_CSI_CSI_CIL_STATUS,
+					     0xffffffff);
+			} else {
+				TC_VI_REG_WT(pcdev,
+					     TEGRA_VI_CAMERA_CONTROL,
+					     0x00000005);
+			}
+
+
+			tegra_camera_incr_syncpts(pcdev);
+			tegra_camera_save_syncpts(pcdev);
+
+			continue;
+		}
+
+		break;
+	}
+
+	spin_lock_irq(&pcdev->videobuf_queue_lock);
+
+	/*
+	 * If vb->state is VB2_BUF_STATE_ERROR, then the vb has already been
+	 * removed, so we shouldn't remove it again.
+	 */
+	if (vb->state != VB2_BUF_STATE_ERROR)
+		list_del_init(&buf->queue);
+
+	if (!list_empty(&pcdev->capture))
+		pcdev->active = &list_entry(pcdev->capture.next,
+					    struct tegra_buffer, queue)->vb;
+	else
+		pcdev->active = NULL;
+
+	do_gettimeofday(&vb->v4l2_buf.timestamp);
+	vb->v4l2_buf.field = pcdev->field;
+	vb->v4l2_buf.sequence = pcdev->sequence++;
+
+	vb2_buffer_done(vb, err < 0 ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+
+	pcdev->num_frames++;
+
+	spin_unlock_irq(&pcdev->videobuf_queue_lock);
+
+	return err;
+}
+
+static void tegra_camera_work(struct work_struct *work)
+{
+	struct tegra_camera_dev *pcdev =
+		container_of(work, struct tegra_camera_dev, work);
+
+	mutex_lock(&pcdev->work_mutex);
+
+	while (pcdev->active)
+		tegra_camera_capture_frame(pcdev);
+
+	mutex_unlock(&pcdev->work_mutex);
+}
+
+static void tegra_camera_activate(struct tegra_camera_dev *pcdev)
+{
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	u32 val;
+	void __iomem *apb_misc;
+#endif
+
+	nvhost_module_busy_ext(pcdev->ndev);
+
+	/* Turn on relevant clocks. */
+	clk_enable(pcdev->clk_vi);
+	clk_enable(pcdev->clk_vi_sensor);
+	clk_enable(pcdev->clk_csi);
+	clk_enable(pcdev->clk_isp);
+	clk_enable(pcdev->clk_csus);
+
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	apb_misc = IO_ADDRESS(TEGRA_APB_MISC_BASE);
+	val = readl(apb_misc + 0x42c);
+	writel(val | 0x1, apb_misc + 0x42c);
+#endif
+
+	/* Save current syncpt values. */
+	tegra_camera_save_syncpts(pcdev);
+}
+
+static void tegra_camera_deactivate(struct tegra_camera_dev *pcdev)
+{
+	mutex_lock(&pcdev->work_mutex);
+
+	/* Cancel active buffer. */
+	if (pcdev->active) {
+		list_del_init(&to_tegra_vb(pcdev->active)->queue);
+		vb2_buffer_done(pcdev->active, VB2_BUF_STATE_ERROR);
+		pcdev->active = NULL;
+	}
+
+	mutex_unlock(&pcdev->work_mutex);
+
+	/* Turn off relevant clocks. */
+	clk_disable(pcdev->clk_vi);
+	clk_disable(pcdev->clk_vi_sensor);
+	clk_disable(pcdev->clk_csi);
+	clk_disable(pcdev->clk_isp);
+	clk_disable(pcdev->clk_csus);
+
+	nvhost_module_idle_ext(pcdev->ndev);
+}
+
+static void tegra_camera_init_buffer(struct tegra_camera_dev *pcdev,
+				     struct tegra_buffer *buf)
+{
+	struct soc_camera_device *icd = pcdev->icd;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+						icd->current_fmt->host_fmt);
+
+	switch (icd->current_fmt->host_fmt->fourcc) {
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_VYUY:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_YVYU:
+		buf->buffer_addr = vb2_dma_nvmap_plane_paddr(&buf->vb, 0);
+		buf->start_addr = buf->buffer_addr;
+
+		if (pcdev->pdata->flip_v)
+			buf->start_addr += bytes_per_line *
+					   (icd->user_height-1);
+
+		if (pcdev->pdata->flip_h)
+			buf->start_addr += bytes_per_line - 1;
+
+		break;
+
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		buf->buffer_addr = vb2_dma_nvmap_plane_paddr(&buf->vb, 0);
+		buf->buffer_addr_u = buf->buffer_addr +
+				     icd->user_width * icd->user_height;
+		buf->buffer_addr_v = buf->buffer_addr_u +
+				     (icd->user_width * icd->user_height) / 4;
+
+		/* For YVU420, we swap the locations of the U and V planes. */
+		if (icd->current_fmt->host_fmt->fourcc == V4L2_PIX_FMT_YVU420) {
+			dma_addr_t temp = buf->buffer_addr_u;
+			buf->buffer_addr_u = buf->buffer_addr_v;
+			buf->buffer_addr_v = temp;
+		}
+
+		buf->start_addr = buf->buffer_addr;
+		buf->start_addr_u = buf->buffer_addr_u;
+		buf->start_addr_v = buf->buffer_addr_v;
+
+		if (pcdev->pdata->flip_v) {
+			buf->start_addr += icd->user_width *
+					   (icd->user_height - 1);
+
+			buf->start_addr_u += ((icd->user_width/2) *
+					      ((icd->user_height/2) - 1));
+
+			buf->start_addr_v += ((icd->user_width/2) *
+					      ((icd->user_height/2) - 1));
+		}
+
+		if (pcdev->pdata->flip_h) {
+			buf->start_addr += icd->user_width - 1;
+
+			buf->start_addr_u += (icd->user_width/2) - 1;
+
+			buf->start_addr_v += (icd->user_width/2) - 1;
+		}
+
+		break;
+
+	default:
+		BUG_ON(1);
+	}
+}
+
+/*
+ *  Videobuf operations
+ */
+static int tegra_camera_videobuf_setup(struct vb2_queue *vq,
+				       unsigned int *num_buffers,
+				       unsigned int *num_planes,
+				       unsigned long sizes[],
+				       void *alloc_ctxs[])
+{
+	struct soc_camera_device *icd = container_of(vq,
+						     struct soc_camera_device,
+						     vb2_vidq);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct tegra_camera_dev *pcdev = ici->priv;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+						icd->current_fmt->host_fmt);
+
+	dev_dbg(icd->parent, "In tegra_camera_videobuf_setup()\n");
+
+	if (bytes_per_line < 0)
+		return bytes_per_line;
+
+	*num_planes = 1;
+
+	pcdev->sequence = 0;
+	sizes[0] = bytes_per_line * icd->user_height;
+	alloc_ctxs[0] = pcdev->alloc_ctx;
+
+	if (!*num_buffers)
+		*num_buffers = 2;
+
+	dev_dbg(icd->parent, "num_buffers=%u, size=%u\n",
+		*num_buffers, sizes[0]);
+
+	tegra_camera_capture_setup(pcdev);
+
+	dev_dbg(icd->parent, "Finished tegra_camera_videobuf_setup()\n");
+
+	return 0;
+}
+
+static int tegra_camera_videobuf_prepare(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 tegra_camera_dev *pcdev = ici->priv;
+	struct tegra_buffer *buf;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+						icd->current_fmt->host_fmt);
+	unsigned long size;
+
+	dev_dbg(icd->parent, "In tegra_camera_videobuf_prepare()\n");
+
+	if (bytes_per_line < 0)
+		return bytes_per_line;
+
+	buf = to_tegra_vb(vb);
+
+	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
+		vb, vb2_plane_vaddr(vb, 0), vb2_plane_size(vb, 0));
+
+#ifdef PREFILL_BUFFER
+	/*
+	 * This can be useful if you want to see if we actually fill
+	 * the buffer with something
+	 */
+	if (vb2_plane_vaddr(vb, 0))
+		memset(vb2_plane_vaddr(vb, 0), 0xbd, vb2_plane_size(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);
+
+	tegra_camera_init_buffer(pcdev, buf);
+
+	dev_dbg(icd->parent, "Finished tegra_camera_videobuf_prepare()\n");
+
+	return 0;
+}
+
+static void tegra_camera_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 tegra_camera_dev *pcdev = ici->priv;
+	struct tegra_buffer *buf = to_tegra_vb(vb);
+
+	dev_dbg(icd->parent, "In tegra_camera_videobuf_queue()\n");
+
+	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->videobuf_queue_lock);
+	list_add_tail(&buf->queue, &pcdev->capture);
+
+	if (!pcdev->active) {
+		pcdev->active = vb;
+		schedule_work(&pcdev->work);
+	}
+	spin_unlock_irq(&pcdev->videobuf_queue_lock);
+
+	dev_dbg(icd->parent, "Finished tegra_camera_videobuf_queue()\n");
+}
+
+static void tegra_camera_videobuf_release(struct vb2_buffer *vb)
+{
+	struct soc_camera_device *icd = container_of(vb->vb2_queue,
+						     struct soc_camera_device,
+						     vb2_vidq);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct tegra_buffer *buf = to_tegra_vb(vb);
+	struct tegra_camera_dev *pcdev = ici->priv;
+
+	dev_dbg(icd->parent, "In tegra_camera_videobuf_release()\n");
+
+	mutex_lock(&pcdev->work_mutex);
+
+	spin_lock_irq(&pcdev->videobuf_queue_lock);
+
+	if (pcdev->active == vb)
+		pcdev->active = NULL;
+
+	/*
+	 * Doesn't hurt also if the list is empty, but it hurts, if queuing the
+	 * buffer failed, and .buf_init() hasn't been called
+	 */
+	if (buf->queue.next)
+		list_del_init(&buf->queue);
+
+	spin_unlock_irq(&pcdev->videobuf_queue_lock);
+
+	mutex_unlock(&pcdev->work_mutex);
+
+	dev_dbg(icd->parent, "Finished tegra_camera_videobuf_release()\n");
+}
+
+static int tegra_camera_videobuf_init(struct vb2_buffer *vb)
+{
+	/* This is for locking debugging only */
+	INIT_LIST_HEAD(&to_tegra_vb(vb)->queue);
+
+	return 0;
+}
+
+static int tegra_camera_stop_streaming(struct vb2_queue *q)
+{
+	struct soc_camera_device *icd = container_of(q,
+						     struct soc_camera_device,
+						     vb2_vidq);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct tegra_camera_dev *pcdev = ici->priv;
+	struct list_head *buf_head, *tmp;
+
+	mutex_lock(&pcdev->work_mutex);
+
+	spin_lock_irq(&pcdev->videobuf_queue_lock);
+
+	pcdev->active = NULL;
+
+	list_for_each_safe(buf_head, tmp, &pcdev->capture)
+		list_del_init(buf_head);
+
+	spin_unlock_irq(&pcdev->videobuf_queue_lock);
+
+	mutex_unlock(&pcdev->work_mutex);
+
+	return 0;
+}
+
+static struct vb2_ops tegra_camera_videobuf_ops = {
+	.queue_setup	= tegra_camera_videobuf_setup,
+	.buf_prepare	= tegra_camera_videobuf_prepare,
+	.buf_queue	= tegra_camera_videobuf_queue,
+	.buf_cleanup	= tegra_camera_videobuf_release,
+	.buf_init	= tegra_camera_videobuf_init,
+	.wait_prepare	= soc_camera_unlock,
+	.wait_finish	= soc_camera_lock,
+	.stop_streaming	= tegra_camera_stop_streaming,
+};
+
+/*
+ *  SOC camera host operations
+ */
+static int tegra_camera_init_videobuf(struct vb2_queue *q,
+				      struct soc_camera_device *icd)
+{
+	dev_dbg(icd->parent, "In tegra_camera_init_videobuf()\n");
+
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR;
+	q->drv_priv = icd;
+	q->ops = &tegra_camera_videobuf_ops;
+	q->mem_ops = &vb2_dma_nvmap_memops;
+	q->buf_struct_size = sizeof(struct tegra_buffer);
+
+	dev_dbg(icd->parent, "Finished tegra_camera_init_videobuf()\n");
+
+	return vb2_queue_init(q);
+}
+
+/*
+ * Called with .video_lock held
+ */
+static int tegra_camera_add_device(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct tegra_camera_dev *pcdev = ici->priv;
+	int err;
+
+	if (pcdev->icd)
+		return -EBUSY;
+
+	pm_runtime_get_sync(ici->v4l2_dev.dev);
+
+	if (pcdev->pdata->enable_camera) {
+		err = pcdev->pdata->enable_camera(pcdev->ndev);
+		if (IS_ERR_VALUE(err))
+			return err;
+	}
+
+	tegra_camera_activate(pcdev);
+
+	pcdev->icd = icd;
+
+	pcdev->num_frames = 0;
+
+	dev_dbg(icd->parent, "TEGRA Camera host attached to camera %d\n",
+		icd->devnum);
+
+	return 0;
+}
+
+/* Called with .video_lock held */
+static void tegra_camera_remove_device(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct tegra_camera_dev *pcdev = ici->priv;
+
+	tegra_camera_deactivate(pcdev);
+
+	pcdev->icd = NULL;
+
+	if (pcdev->pdata->disable_camera)
+		pcdev->pdata->disable_camera(pcdev->ndev);
+
+	pm_runtime_put_sync(ici->v4l2_dev.dev);
+
+	dev_dbg(icd->parent, "Frames captured: %d\n", pcdev->num_frames);
+
+	dev_dbg(icd->parent, "TEGRA camera host detached from camera %d\n",
+		icd->devnum);
+}
+
+static int tegra_camera_set_bus_param(struct soc_camera_device *icd,
+					__u32 pixfmt)
+{
+	return 0;
+}
+
+static int tegra_camera_get_formats(struct soc_camera_device *icd,
+				    unsigned int idx,
+				    struct soc_camera_format_xlate *xlate)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct device *dev = icd->parent;
+	int formats = 0;
+	int ret;
+	enum v4l2_mbus_pixelcode code;
+	const struct soc_mbus_pixelfmt *fmt;
+	int k;
+
+	ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
+	if (ret != 0)
+		/* No more formats */
+		return 0;
+
+	fmt = soc_mbus_get_fmtdesc(code);
+	if (!fmt) {
+		dev_err(dev, "Invalid format code #%u: %d\n", idx, code);
+		return 0;
+	}
+
+	switch (code) {
+	case V4L2_MBUS_FMT_UYVY8_2X8:
+	case V4L2_MBUS_FMT_VYUY8_2X8:
+	case V4L2_MBUS_FMT_YUYV8_2X8:
+	case V4L2_MBUS_FMT_YVYU8_2X8:
+		formats += ARRAY_SIZE(tegra_camera_formats);
+		for (k = 0;
+		     xlate && (k < ARRAY_SIZE(tegra_camera_formats));
+		     k++) {
+			xlate->host_fmt	= &tegra_camera_formats[k];
+			xlate->code	= code;
+			xlate++;
+
+			dev_info(dev, "Providing format %s using code %d\n",
+				 tegra_camera_formats[k].name, code);
+		}
+		break;
+	default:
+		dev_info(dev, "Not supporting %s\n", fmt->name);
+		return 0;
+	}
+
+	return formats;
+}
+
+static void tegra_camera_put_formats(struct soc_camera_device *icd)
+{
+	kfree(icd->host_priv);
+	icd->host_priv = NULL;
+}
+
+static int tegra_camera_set_fmt(struct soc_camera_device *icd,
+			      struct v4l2_format *f)
+{
+	struct device *dev = icd->parent;
+	struct soc_camera_host *ici = to_soc_camera_host(dev);
+	struct tegra_camera_dev *pcdev = ici->priv;
+
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	const struct soc_camera_format_xlate *xlate = NULL;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_mbus_framefmt mf;
+	int ret;
+
+	dev_dbg(dev, "In tegra_camera_set_fmt()\n");
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
+	if (!xlate) {
+		dev_warn(dev, "Format %x not found\n", pix->pixelformat);
+		return -EINVAL;
+	}
+
+	mf.width	= pix->width;
+	mf.height	= pix->height;
+	mf.field	= pix->field;
+	mf.colorspace	= pix->colorspace;
+	mf.code		= xlate->code;
+
+	ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
+	if (IS_ERR_VALUE(ret)) {
+		dev_warn(dev, "Failed to configure for format %x\n",
+			 pix->pixelformat);
+		return ret;
+	}
+
+	if (mf.code != xlate->code) {
+		dev_warn(dev, "mf.code = %d, xlate->code = %d, mismatch\n",
+			mf.code, xlate->code);
+		return -EINVAL;
+	}
+
+	icd->user_width		= mf.width;
+	icd->user_height	= mf.height;
+	icd->current_fmt	= xlate;
+
+	pcdev->field = pix->field;
+
+	dev_dbg(dev, "Finished tegra_camera_set_fmt(), returning %d\n", ret);
+
+	return ret;
+}
+
+static int tegra_camera_try_fmt(struct soc_camera_device *icd,
+				struct v4l2_format *f)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	const struct soc_camera_format_xlate *xlate;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_mbus_framefmt mf;
+	__u32 pixfmt = pix->pixelformat;
+	int ret;
+
+	dev_dbg(icd->parent, "In tegra_camera_try_fmt()\n");
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	if (!xlate) {
+		dev_warn(icd->parent, "Format %x not found\n", pixfmt);
+		return -EINVAL;
+	}
+
+	pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
+						    xlate->host_fmt);
+	if (pix->bytesperline < 0)
+		return pix->bytesperline;
+	pix->sizeimage = pix->height * pix->bytesperline;
+
+	/* limit to sensor capabilities */
+	mf.width	= pix->width;
+	mf.height	= pix->height;
+	mf.field	= pix->field;
+	mf.colorspace	= pix->colorspace;
+	mf.code		= xlate->code;
+
+	ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
+	if (IS_ERR_VALUE(ret))
+		return ret;
+
+	pix->width	= mf.width;
+	pix->height	= mf.height;
+	pix->colorspace	= mf.colorspace;
+	/*
+	 * width and height could have been changed, therefore update the
+	 * bytesperline and sizeimage here.
+	 */
+	pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
+						    xlate->host_fmt);
+	pix->sizeimage = pix->height * pix->bytesperline;
+
+	switch (mf.field) {
+	case V4L2_FIELD_ANY:
+	case V4L2_FIELD_NONE:
+		pix->field	= V4L2_FIELD_NONE;
+		break;
+	default:
+		/* TODO: support interlaced at least in pass-through mode */
+		dev_err(icd->parent, "Field type %d unsupported.\n",
+			mf.field);
+		return -EINVAL;
+	}
+
+	dev_dbg(icd->parent,
+		"Finished tegra_camera_try_fmt(), returning %d\n", ret);
+
+	return ret;
+}
+
+static int tegra_camera_reqbufs(struct soc_camera_device *icd,
+				struct v4l2_requestbuffers *p)
+{
+	return 0;
+}
+
+static unsigned int tegra_camera_poll(struct file *file, poll_table *pt)
+{
+	struct soc_camera_device *icd = file->private_data;
+
+	return vb2_poll(&icd->vb2_vidq, file, pt);
+}
+
+static int tegra_camera_querycap(struct soc_camera_host *ici,
+				 struct v4l2_capability *cap)
+{
+	strlcpy(cap->card, TEGRA_CAM_DRV_NAME, sizeof(cap->card));
+	cap->version = TEGRA_CAM_VERSION_CODE;
+	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+
+	return 0;
+}
+
+static struct soc_camera_host_ops tegra_soc_camera_host_ops = {
+	.owner		= THIS_MODULE,
+	.init_videobuf2	= tegra_camera_init_videobuf,
+	.add		= tegra_camera_add_device,
+	.remove		= tegra_camera_remove_device,
+	.set_bus_param	= tegra_camera_set_bus_param,
+	.get_formats	= tegra_camera_get_formats,
+	.put_formats	= tegra_camera_put_formats,
+	.set_fmt	= tegra_camera_set_fmt,
+	.try_fmt	= tegra_camera_try_fmt,
+	.reqbufs	= tegra_camera_reqbufs,
+	.poll		= tegra_camera_poll,
+	.querycap	= tegra_camera_querycap,
+};
+
+static int __devinit tegra_camera_probe(struct nvhost_device *ndev,
+					struct nvhost_device_id *id_table)
+{
+	struct tegra_camera_dev *pcdev;
+	int err = 0;
+
+	pcdev = kzalloc(sizeof(struct tegra_camera_dev), GFP_KERNEL);
+	if (!pcdev) {
+		dev_err(&ndev->dev, "Could not allocate pcdev\n");
+		err = -ENOMEM;
+		goto exit;
+	}
+
+	pcdev->pdata			= ndev->dev.platform_data;
+	pcdev->ndev			= ndev;
+
+	pcdev->ici.priv		= pcdev;
+	pcdev->ici.v4l2_dev.dev	= &ndev->dev;
+	pcdev->ici.nr		= ndev->id;
+	pcdev->ici.drv_name	= dev_name(&ndev->dev);
+	pcdev->ici.ops		= &tegra_soc_camera_host_ops;
+
+	INIT_LIST_HEAD(&pcdev->capture);
+	INIT_WORK(&pcdev->work, tegra_camera_work);
+	spin_lock_init(&pcdev->videobuf_queue_lock);
+	mutex_init(&pcdev->work_mutex);
+
+	nvhost_set_drvdata(ndev, pcdev);
+
+	if (!tegra_camera_port_is_valid(pcdev->pdata->port)) {
+		dev_err(&ndev->dev, "Invalid camera port %d in platform data\n",
+			pcdev->pdata->port);
+		goto exit_free_pcdev;
+	}
+
+	pcdev->clk_vi = clk_get_sys("tegra_camera", "vi");
+	if (IS_ERR_OR_NULL(pcdev->clk_vi)) {
+		dev_err(&ndev->dev, "Failed to get vi clock.\n");
+		goto exit_free_pcdev;
+	}
+
+	pcdev->clk_vi_sensor = clk_get_sys("tegra_camera", "vi_sensor");
+	if (IS_ERR_OR_NULL(pcdev->clk_vi_sensor)) {
+		dev_err(&ndev->dev, "Failed to get vi_sensor clock.\n");
+		goto exit_put_clk_vi;
+	}
+
+	pcdev->clk_csi = clk_get_sys("tegra_camera", "csi");
+	if (IS_ERR_OR_NULL(pcdev->clk_csi)) {
+		dev_err(&ndev->dev, "Failed to get csi clock.\n");
+		goto exit_put_clk_vi_sensor;
+	}
+
+	pcdev->clk_isp = clk_get_sys("tegra_camera", "isp");
+	if (IS_ERR_OR_NULL(pcdev->clk_isp)) {
+		dev_err(&ndev->dev, "Failed to get isp clock.\n");
+		goto exit_put_clk_csi;
+	}
+
+	pcdev->clk_csus = clk_get_sys("tegra_camera", "csus");
+	if (IS_ERR_OR_NULL(pcdev->clk_csus)) {
+		dev_err(&ndev->dev, "Failed to get csus clock.\n");
+		goto exit_put_clk_isp;
+	}
+
+	clk_set_rate(pcdev->clk_vi, 150000000);
+	clk_set_rate(pcdev->clk_vi_sensor, 24000000);
+
+	err = nvhost_client_device_get_resources(ndev);
+	if (err)
+		goto exit_put_clk_csus;
+
+	nvhost_client_device_init(ndev);
+
+	pcdev->vi_base = ndev->aperture;
+
+	pm_suspend_ignore_children(&ndev->dev, true);
+	pm_runtime_enable(&ndev->dev);
+	pm_runtime_resume(&ndev->dev);
+
+	pcdev->alloc_ctx = vb2_dma_nvmap_init_ctx(NULL);
+	if (IS_ERR(pcdev->alloc_ctx)) {
+		err = PTR_ERR(pcdev->alloc_ctx);
+		goto exit_put_resources;
+	}
+
+	err = soc_camera_host_register(&pcdev->ici);
+	if (IS_ERR_VALUE(err))
+		goto exit_cleanup_alloc_ctx;
+
+	dev_notice(&ndev->dev, "Tegra camera driver loaded.\n");
+
+	return err;
+
+exit_cleanup_alloc_ctx:
+	vb2_dma_nvmap_cleanup_ctx(&ndev->dev);
+exit_put_resources:
+	pm_runtime_disable(&ndev->dev);
+	nvhost_client_device_put_resources(ndev);
+exit_put_clk_csus:
+	clk_put(pcdev->clk_csus);
+exit_put_clk_isp:
+	clk_put(pcdev->clk_isp);
+exit_put_clk_csi:
+	clk_put(pcdev->clk_csi);
+exit_put_clk_vi_sensor:
+	clk_put(pcdev->clk_vi_sensor);
+exit_put_clk_vi:
+	clk_put(pcdev->clk_vi);
+exit_free_pcdev:
+	kfree(pcdev);
+exit:
+	return err;
+}
+
+static int __devexit tegra_camera_remove(struct nvhost_device *ndev)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(&ndev->dev);
+	struct tegra_camera_dev *pcdev = container_of(ici,
+					struct tegra_camera_dev, ici);
+	struct resource *res;
+
+	res = nvhost_get_resource_byname(ndev, IORESOURCE_MEM, "regs");
+	if (!res)
+		return -EBUSY;
+
+	soc_camera_host_unregister(ici);
+
+	vb2_dma_nvmap_cleanup_ctx(&ndev->dev);
+
+	pm_runtime_disable(&ndev->dev);
+
+	nvhost_client_device_put_resources(ndev);
+
+	clk_put(pcdev->clk_csus);
+	clk_put(pcdev->clk_isp);
+	clk_put(pcdev->clk_csi);
+	clk_put(pcdev->clk_vi_sensor);
+	clk_put(pcdev->clk_vi);
+
+	kfree(pcdev);
+
+	dev_notice(&ndev->dev, "Tegra camera host driver unloaded\n");
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_FISH
+static int tegra_camera_suspend(struct nvhost_device *ndev, pm_message_t state)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(&ndev->dev);
+	struct tegra_camera_dev *pcdev = container_of(ici,
+					struct tegra_camera_dev, ici);
+
+	mutex_lock(&pcdev->work_mutex);
+
+	/* We only need to do something if a camera sensor is attached. */
+	if (pcdev->icd) {
+		/* Suspend the camera sensor. */
+		WARN_ON(!pcdev->icd->ops->suspend);
+		pcdev->icd->ops->suspend(pcdev->icd, state);
+
+		/* Power off the camera subsystem. */
+		pcdev->pdata->disable_camera(pcdev->ndev);
+
+		nvhost_module_idle_ext(nvhost_get_parent(ndev));
+	}
+
+	return 0;
+}
+
+static int tegra_camera_resume(struct nvhost_device *ndev)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(&ndev->dev);
+	struct tegra_camera_dev *pcdev = container_of(ici,
+					struct tegra_camera_dev, ici);
+
+	/* We only need to do something if a camera sensor is attached. */
+	if (pcdev->icd) {
+		nvhost_module_busy_ext(nvhost_get_parent(ndev));
+
+		/* Power on the camera subsystem. */
+		pcdev->pdata->enable_camera(pcdev->ndev);
+
+		/* Resume the camera host. */
+		tegra_camera_save_syncpts(pcdev);
+		tegra_camera_capture_setup(pcdev);
+
+		/* Resume the camera sensor. */
+		WARN_ON(!pcdev->icd->ops->resume);
+		pcdev->icd->ops->resume(pcdev->icd);
+	}
+
+	mutex_unlock(&pcdev->work_mutex);
+
+	return 0;
+}
+#endif
+
+static struct nvhost_driver tegra_camera_driver = {
+	.driver	= {
+		.name	= TEGRA_CAM_DRV_NAME,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= tegra_camera_probe,
+	.remove		= __devexit_p(tegra_camera_remove),
+#ifdef CONFIG_PM_FISH
+	.suspend	= tegra_camera_suspend,
+	.resume		= tegra_camera_resume,
+#endif
+};
+
+
+static int __init tegra_camera_init(void)
+{
+	return nvhost_driver_register(&tegra_camera_driver);
+}
+
+static void __exit tegra_camera_exit(void)
+{
+	nvhost_driver_unregister(&tegra_camera_driver);
+}
+
+module_init(tegra_camera_init);
+module_exit(tegra_camera_exit);
+
+MODULE_DESCRIPTION("TEGRA SoC Camera Host driver");
+MODULE_AUTHOR("Andrew Chew <achew@nvidia.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("nvhost:" TEGRA_CAM_DRV_NAME);
diff --git a/drivers/media/video/videobuf2-dma-nvmap.c b/drivers/media/video/videobuf2-dma-nvmap.c
new file mode 100644
index 000000000000..27f43e5a3a57
--- /dev/null
+++ b/drivers/media/video/videobuf2-dma-nvmap.c
@@ -0,0 +1,238 @@
+/*
+ * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/dma-mapping.h>
+#include <linux/nvmap.h>
+
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-memops.h>
+
+struct vb2_dc_conf {
+	struct device		*dev;
+	struct nvmap_client	*nvmap_client;
+};
+
+struct vb2_dc_buf {
+	struct vb2_dc_conf		*conf;
+	void				*vaddr;
+	dma_addr_t			paddr;
+	unsigned long			size;
+	struct vm_area_struct		*vma;
+	atomic_t			refcount;
+	struct vb2_vmarea_handler	handler;
+
+	struct nvmap_handle_ref		*nvmap_ref;
+};
+
+static void vb2_dma_nvmap_put(void *buf_priv);
+
+static void *vb2_dma_nvmap_alloc(void *alloc_ctx, unsigned long size)
+{
+	struct vb2_dc_conf *conf = alloc_ctx;
+	struct vb2_dc_buf *buf;
+	int ret;
+
+	buf = kzalloc(sizeof *buf, GFP_KERNEL);
+	if (!buf) {
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+	buf->nvmap_ref = nvmap_alloc(conf->nvmap_client, size, 32,
+				     NVMAP_HANDLE_CACHEABLE, NVMAP_HEAP_SYSMEM);
+	if (IS_ERR(buf->nvmap_ref)) {
+		dev_err(conf->dev, "nvmap_alloc failed\n");
+		ret = -ENOMEM;
+		goto exit_free;
+	}
+
+	buf->paddr = nvmap_pin(conf->nvmap_client, buf->nvmap_ref);
+	if (IS_ERR_VALUE(buf->paddr)) {
+		dev_err(conf->dev, "nvmap_pin failed\n");
+		ret = -ENOMEM;
+		goto exit_dealloc;
+	}
+
+	buf->vaddr = nvmap_mmap(buf->nvmap_ref);
+	if (!buf->vaddr) {
+		dev_err(conf->dev, "nvmap_mmap failed\n");
+		ret = -ENOMEM;
+		goto exit_unpin;
+	}
+
+	buf->conf = conf;
+	buf->size = size;
+
+	buf->handler.refcount = &buf->refcount;
+	buf->handler.put = vb2_dma_nvmap_put;
+	buf->handler.arg = buf;
+
+	atomic_inc(&buf->refcount);
+
+	return buf;
+
+exit_unpin:
+	nvmap_unpin(conf->nvmap_client, buf->nvmap_ref);
+exit_dealloc:
+	nvmap_free(conf->nvmap_client, buf->nvmap_ref);
+exit_free:
+	kfree(buf);
+exit:
+	return ERR_PTR(ret);
+}
+
+static void vb2_dma_nvmap_put(void *buf_priv)
+{
+	struct vb2_dc_buf *buf = buf_priv;
+
+	if (atomic_dec_and_test(&buf->refcount)) {
+		nvmap_munmap(buf->nvmap_ref, buf->vaddr);
+		nvmap_unpin(buf->conf->nvmap_client, buf->nvmap_ref);
+		nvmap_free(buf->conf->nvmap_client, buf->nvmap_ref);
+		kfree(buf);
+	}
+}
+
+static void *vb2_dma_nvmap_cookie(void *buf_priv)
+{
+	struct vb2_dc_buf *buf = buf_priv;
+
+	return &buf->paddr;
+}
+
+static void *vb2_dma_nvmap_vaddr(void *buf_priv)
+{
+	struct vb2_dc_buf *buf = buf_priv;
+	if (!buf)
+		return 0;
+
+	return buf->vaddr;
+}
+
+static unsigned int vb2_dma_nvmap_num_users(void *buf_priv)
+{
+	struct vb2_dc_buf *buf = buf_priv;
+
+	return atomic_read(&buf->refcount);
+}
+
+static int vb2_dma_nvmap_mmap(void *buf_priv, struct vm_area_struct *vma)
+{
+	struct vb2_dc_buf *buf = buf_priv;
+
+	if (!buf) {
+		printk(KERN_ERR "No buffer to map\n");
+		return -EINVAL;
+	}
+
+	return vb2_mmap_pfn_range(vma, buf->paddr, buf->size,
+				  &vb2_common_vm_ops, &buf->handler);
+}
+
+static void *vb2_dma_nvmap_get_userptr(void *alloc_ctx, unsigned long vaddr,
+					unsigned long size, int write)
+{
+	struct vb2_dc_buf *buf;
+	struct vm_area_struct *vma;
+	dma_addr_t paddr = 0;
+	int ret;
+
+	buf = kzalloc(sizeof *buf, GFP_KERNEL);
+	if (!buf)
+		return ERR_PTR(-ENOMEM);
+
+	ret = vb2_get_contig_userptr(vaddr, size, &vma, &paddr);
+	if (ret) {
+		printk(KERN_ERR "Failed acquiring VMA for vaddr 0x%08lx\n",
+				vaddr);
+		kfree(buf);
+		return ERR_PTR(ret);
+	}
+
+	buf->size = size;
+	buf->paddr = paddr;
+	buf->vma = vma;
+
+	return buf;
+}
+
+static void vb2_dma_nvmap_put_userptr(void *mem_priv)
+{
+	struct vb2_dc_buf *buf = mem_priv;
+
+	if (!buf)
+		return;
+
+	vb2_put_vma(buf->vma);
+	kfree(buf);
+}
+
+const struct vb2_mem_ops vb2_dma_nvmap_memops = {
+	.alloc		= vb2_dma_nvmap_alloc,
+	.put		= vb2_dma_nvmap_put,
+	.cookie		= vb2_dma_nvmap_cookie,
+	.vaddr		= vb2_dma_nvmap_vaddr,
+	.mmap		= vb2_dma_nvmap_mmap,
+	.get_userptr	= vb2_dma_nvmap_get_userptr,
+	.put_userptr	= vb2_dma_nvmap_put_userptr,
+	.num_users	= vb2_dma_nvmap_num_users,
+};
+EXPORT_SYMBOL_GPL(vb2_dma_nvmap_memops);
+
+void *vb2_dma_nvmap_init_ctx(struct device *dev)
+{
+	struct vb2_dc_conf *conf;
+	int ret;
+
+	conf = kzalloc(sizeof *conf, GFP_KERNEL);
+	if (!conf) {
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+	conf->dev = dev;
+
+	conf->nvmap_client = nvmap_create_client(nvmap_dev,
+						 "videobuf2-dma-nvmap");
+	if (!conf->nvmap_client) {
+		ret = -ENOMEM;
+		goto exit_free;
+	}
+
+	return conf;
+
+exit_free:
+	kfree(conf);
+exit:
+	return ERR_PTR(ret);
+}
+EXPORT_SYMBOL_GPL(vb2_dma_nvmap_init_ctx);
+
+void vb2_dma_nvmap_cleanup_ctx(void *alloc_ctx)
+{
+	struct vb2_dc_conf *conf = alloc_ctx;
+
+	nvmap_client_put(conf->nvmap_client);
+
+	kfree(alloc_ctx);
+}
+EXPORT_SYMBOL_GPL(vb2_dma_nvmap_cleanup_ctx);
+
+MODULE_DESCRIPTION("DMA-nvmap memory handling routines for videobuf2");
+MODULE_AUTHOR("Andrew Chew <achew@nvidia.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/wireless/bcm4329/wl_iw.c b/drivers/net/wireless/bcm4329/wl_iw.c
index e71ab64c2caa..8cb0f3af84ee 100644
--- a/drivers/net/wireless/bcm4329/wl_iw.c
+++ b/drivers/net/wireless/bcm4329/wl_iw.c
@@ -4339,6 +4339,7 @@ wl_iw_get_essid(
 	if (!extra)
 		return -EINVAL;
 
+	memset(&ssid, 0, sizeof(ssid));
 	if ((error = dev_wlc_ioctl(dev, WLC_GET_SSID, &ssid, sizeof(ssid)))) {
 		WL_ERROR(("Error getting the SSID\n"));
 		return error;
diff --git a/drivers/net/wireless/bcmdhd/dhd_linux.c b/drivers/net/wireless/bcmdhd/dhd_linux.c
index 44879e4a1170..a6b6dc8be91f 100644
--- a/drivers/net/wireless/bcmdhd/dhd_linux.c
+++ b/drivers/net/wireless/bcmdhd/dhd_linux.c
@@ -3676,6 +3676,7 @@ void dhd_detach(dhd_pub_t *dhdp)
 #endif /* defined(CONFIG_HAS_EARLYSUSPEND) */
 
 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	dhd->pub.hang_was_sent = 1;
 	cancel_work_sync(&dhd->work_hang);
 #endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))  */
 
diff --git a/drivers/staging/iio/light/isl29028.c b/drivers/staging/iio/light/isl29028.c
index 0b1d2f2f73aa..5874c32958d9 100644
--- a/drivers/staging/iio/light/isl29028.c
+++ b/drivers/staging/iio/light/isl29028.c
@@ -1193,9 +1193,9 @@ static int __devinit isl29028_probe(struct i2c_client *client,
 				"error %d\n", chip->irq, err);
 			goto exit_free;
 		}
+		chip->is_int_enable = true;
 	}
 
-	chip->is_int_enable = true;
 	chip->indio_dev = iio_allocate_device(0);
 	if (!chip->indio_dev) {
 		dev_err(&client->dev, "iio allocation fails\n");
diff --git a/drivers/usb/gadget/tegra_udc.c b/drivers/usb/gadget/tegra_udc.c
index 3f4f123da013..074a9b1ce6d9 100644
--- a/drivers/usb/gadget/tegra_udc.c
+++ b/drivers/usb/gadget/tegra_udc.c
@@ -38,6 +38,7 @@
 #include <linux/err.h>
 #include <linux/io.h>
 #include <linux/pm_qos_params.h>
+#include <linux/platform_data/tegra_usb.h>
 
 #include <asm/byteorder.h>
 #include <asm/io.h>
@@ -649,7 +650,7 @@ static int tegra_ep_disable(struct usb_ep *_ep)
 	ep_num = ep_index(ep);
 
 	/* Touch the registers if cable is connected and phy is on */
-	if (vbus_enabled(udc)) {
+	if (udc->vbus_active) {
 		epctrl = udc_readl(udc, EP_CONTROL_REG_OFFSET + (ep_num * 4));
 		if (ep_is_in(ep))
 			epctrl &= ~EPCTRL_TX_ENABLE;
@@ -1009,7 +1010,7 @@ static int tegra_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
 	ep_num = ep_index(ep);
 
 	/* Touch the registers if cable is connected and phy is on */
-	if (vbus_enabled(udc)) {
+	if (udc->vbus_active) {
 		epctrl = udc_readl(udc, EP_CONTROL_REG_OFFSET + (ep_num * 4));
 		if (ep_is_in(ep))
 			epctrl &= ~EPCTRL_TX_ENABLE;
@@ -1060,7 +1061,7 @@ static int tegra_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
 	/* Enable EP */
 out:
 	/* Touch the registers if cable is connected and phy is on */
-	if (vbus_enabled(udc)) {
+	if (udc->vbus_active) {
 		epctrl = udc_readl(udc, EP_CONTROL_REG_OFFSET + (ep_num * 4));
 		if (ep_is_in(ep))
 			epctrl |= EPCTRL_TX_ENABLE;
@@ -1182,7 +1183,7 @@ static void tegra_ep_fifo_flush(struct usb_ep *_ep)
 		bits = 1 << ep_num;
 
 	/* Touch the registers if cable is connected and phy is on */
-	if (!vbus_enabled(udc))
+	if (!udc->vbus_active)
 		return;
 
 	timeout = jiffies + UDC_FLUSH_TIMEOUT_MS;
@@ -2377,14 +2378,13 @@ static int tegra_udc_start(struct usb_gadget_driver *driver,
 		goto out;
 	}
 
-
 	/* Enable DR IRQ reg and Set usbcmd reg  Run bit */
 	if (vbus_enabled(udc)) {
 		dr_controller_run(udc);
 		udc->usb_state = USB_STATE_ATTACHED;
 		udc->ep0_state = WAIT_FOR_SETUP;
 		udc->ep0_dir = 0;
-		udc->vbus_active = vbus_enabled(udc);
+		udc->vbus_active = 1;
 	}
 
 	printk(KERN_INFO "%s: bind to driver %s\n",
@@ -2580,6 +2580,7 @@ static int __init tegra_udc_probe(struct platform_device *pdev)
 {
 	struct tegra_udc *udc;
 	struct resource *res;
+	struct tegra_usb_platform_data *pdata = dev_get_platdata(&pdev->dev);
 	int err = -ENODEV;
 	DBG("%s(%d) BEGIN\n", __func__, __LINE__);
 
@@ -2630,12 +2631,14 @@ static int __init tegra_udc_probe(struct platform_device *pdev)
 		goto err_iounmap;
 	}
 
-	err = enable_irq_wake(udc->irq);
-	if (err < 0) {
-		dev_warn(&pdev->dev,
-			"Couldn't enable USB udc mode wakeup, irq=%d, error=%d\n",
-			udc->irq, err);
-		err = 0;
+	if (pdata->u_data.dev.remote_wakeup_supported) {
+		err = enable_irq_wake(udc->irq);
+		if (err < 0) {
+			dev_warn(&pdev->dev,
+				"Couldn't enable USB udc mode wakeup, irq=%d,"
+				" error=%d\n", udc->irq, err);
+			err = 0;
+		}
 	}
 
 	udc->phy = tegra_usb_phy_open(pdev);
diff --git a/drivers/usb/host/ehci-tegra.c b/drivers/usb/host/ehci-tegra.c
index 13d96d3b1320..095447d9e3a2 100644
--- a/drivers/usb/host/ehci-tegra.c
+++ b/drivers/usb/host/ehci-tegra.c
@@ -522,6 +522,7 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 	struct resource *res;
 	struct usb_hcd *hcd;
 	struct tegra_ehci_hcd *tegra;
+	struct tegra_usb_platform_data *pdata = dev_get_platdata(&pdev->dev);
 	int err = 0;
 	int irq;
 
@@ -592,13 +593,15 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 		goto fail_phy;
 	}
 
-	err = enable_irq_wake(tegra->irq);
-	if (err < 0) {
-		dev_warn(&pdev->dev,
-				"Couldn't enable USB host mode wakeup, irq=%d, "
-				"error=%d\n", irq, err);
-		err = 0;
-		tegra->irq = 0;
+	if (pdata->u_data.host.remote_wakeup_supported) {
+		err = enable_irq_wake(tegra->irq);
+		if (err < 0) {
+			dev_warn(&pdev->dev,
+					"Couldn't enable USB host mode wakeup,"
+					" irq=%d error=%d\n", irq, err);
+			err = 0;
+			tegra->irq = 0;
+		}
 	}
 
 	tegra->ehci = hcd_to_ehci(hcd);
@@ -646,6 +649,7 @@ static int tegra_ehci_suspend(struct platform_device *pdev, pm_message_t state)
 static int tegra_ehci_remove(struct platform_device *pdev)
 {
 	struct tegra_ehci_hcd *tegra = platform_get_drvdata(pdev);
+	struct tegra_usb_platform_data *pdata = dev_get_platdata(&pdev->dev);
 	struct usb_hcd *hcd = ehci_to_hcd(tegra->ehci);
 
 	if (tegra == NULL || hcd == NULL)
@@ -658,7 +662,7 @@ static int tegra_ehci_remove(struct platform_device *pdev)
 	}
 #endif
 
-	if (tegra->irq)
+	if (tegra->irq && pdata->u_data.host.remote_wakeup_supported)
 		disable_irq_wake(tegra->irq);
 
 	/* Make sure phy is powered ON to access USB register */
diff --git a/drivers/usb/otg/tegra-otg.c b/drivers/usb/otg/tegra-otg.c
index 22e303ede24e..12c359bcbe46 100644
--- a/drivers/usb/otg/tegra-otg.c
+++ b/drivers/usb/otg/tegra-otg.c
@@ -173,8 +173,6 @@ static void tegra_stop_host(struct tegra_otg_data *tegra)
 
 	if (pdev) {
 		/* unregister host from otg */
-		kfree(pdev->dev.platform_data);
-		pdev->dev.platform_data = NULL;
 		platform_device_unregister(pdev);
 		tegra->pdev = NULL;
 	}
@@ -472,12 +470,14 @@ static int tegra_otg_probe(struct platform_device *pdev)
 		goto err_irq;
 	}
 
-	err = enable_irq_wake(tegra->irq);
-	if (err < 0) {
-		dev_warn(&pdev->dev,
-			"Couldn't enable USB otg mode wakeup, irq=%d, error=%d\n",
-			tegra->irq, err);
-		err = 0;
+	if (pdata->ehci_pdata->u_data.host.remote_wakeup_supported) {
+		err = enable_irq_wake(tegra->irq);
+		if (err < 0) {
+			dev_warn(&pdev->dev,
+				"Couldn't enable USB otg mode wakeup,"
+				" irq=%d, error=%d\n", tegra->irq, err);
+			err = 0;
+		}
 	}
 
 	INIT_WORK(&tegra->work, irq_work);
diff --git a/drivers/video/tegra/dc/dc.c b/drivers/video/tegra/dc/dc.c
index ca2103f73c8a..d4c3f8bc9a49 100644
--- a/drivers/video/tegra/dc/dc.c
+++ b/drivers/video/tegra/dc/dc.c
@@ -58,21 +58,6 @@
 #define DC_COM_PIN_OUTPUT_POLARITY1_INIT_VAL	0x01000000
 #define DC_COM_PIN_OUTPUT_POLARITY3_INIT_VAL	0x0
 
-static struct fb_videomode tegra_dc_hdmi_fallback_mode = {
-	.refresh = 60,
-	.xres = 640,
-	.yres = 480,
-	.pixclock = KHZ2PICOS(25200),
-	.hsync_len = 96,	/* h_sync_width */
-	.vsync_len = 2,		/* v_sync_width */
-	.left_margin = 48,	/* h_back_porch */
-	.upper_margin = 33,	/* v_back_porch */
-	.right_margin = 16,	/* h_front_porch */
-	.lower_margin = 10,	/* v_front_porch */
-	.vmode = 0,
-	.sync = 0,
-};
-
 static struct tegra_dc_mode override_disp_mode[3];
 
 static void _tegra_dc_controller_disable(struct tegra_dc *dc);
@@ -1462,34 +1447,6 @@ static bool _tegra_dc_controller_reset_enable(struct tegra_dc *dc)
 }
 #endif
 
-static int _tegra_dc_set_default_videomode(struct tegra_dc *dc)
-{
-	if (dc->mode.pclk == 0) {
-		switch (dc->out->type) {
-		case TEGRA_DC_OUT_HDMI:
-		/* DC enable called but no videomode is loaded.
-		     Check if HDMI is connected, then set fallback mdoe */
-		if (tegra_dc_hpd(dc)) {
-			return tegra_dc_set_fb_mode(dc,
-					&tegra_dc_hdmi_fallback_mode, 0);
-		} else
-			return false;
-
-		break;
-
-		/* Do nothing for other outputs for now */
-		case TEGRA_DC_OUT_RGB:
-
-		case TEGRA_DC_OUT_DSI:
-
-		default:
-			return false;
-		}
-	}
-
-	return false;
-}
-
 static bool _tegra_dc_enable(struct tegra_dc *dc)
 {
 	if (dc->mode.pclk == 0)
@@ -1881,13 +1838,6 @@ static int tegra_dc_probe(struct nvhost_device *ndev,
 		dc->ext = NULL;
 	}
 
-	mutex_lock(&dc->lock);
-	if (dc->pdata->flags & TEGRA_DC_FLAG_ENABLED) {
-		_tegra_dc_set_default_videomode(dc);
-		dc->enabled = _tegra_dc_enable(dc);
-	}
-	mutex_unlock(&dc->lock);
-
 	/* interrupt handler must be registered before tegra_fb_register() */
 	if (request_irq(irq, tegra_dc_irq, 0,
 			dev_name(&ndev->dev), dc)) {
@@ -1895,6 +1845,12 @@ static int tegra_dc_probe(struct nvhost_device *ndev,
 		ret = -EBUSY;
 		goto err_put_emc_clk;
 	}
+	disable_dc_irq(irq);
+
+	mutex_lock(&dc->lock);
+	if (dc->pdata->flags & TEGRA_DC_FLAG_ENABLED)
+		dc->enabled = _tegra_dc_enable(dc);
+	mutex_unlock(&dc->lock);
 
 	tegra_dc_create_debugfs(dc);
 
@@ -1919,6 +1875,7 @@ static int tegra_dc_probe(struct nvhost_device *ndev,
 		}
 
 		dc->fb = tegra_fb_register(ndev, dc, dc->pdata->fb, fb_mem);
+
 		if (IS_ERR_OR_NULL(dc->fb))
 			dc->fb = NULL;
 	}
@@ -2033,10 +1990,8 @@ static int tegra_dc_resume(struct nvhost_device *ndev)
 	mutex_lock(&dc->lock);
 	dc->suspended = false;
 
-	if (dc->enabled) {
-		_tegra_dc_set_default_videomode(dc);
+	if (dc->enabled)
 		_tegra_dc_enable(dc);
-	}
 
 	if (dc->out && dc->out->hotplug_init)
 		dc->out->hotplug_init();
diff --git a/drivers/video/tegra/dc/dc_config.c b/drivers/video/tegra/dc/dc_config.c
index f238faddab12..90f505bba28a 100644
--- a/drivers/video/tegra/dc/dc_config.c
+++ b/drivers/video/tegra/dc/dc_config.c
@@ -23,7 +23,7 @@
 static struct tegra_dc_feature_entry t20_feature_entries_a[] = {
 	{ 0, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_A,} },
 	{ 0, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
-	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 16, 4095, 16,} },
+	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
 	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
 	{ 0, TEGRA_DC_FEATURE_FILTER_TYPE, {0, 0,} },
 	{ 0, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1,} },
@@ -32,7 +32,7 @@ static struct tegra_dc_feature_entry t20_feature_entries_a[] = {
 	{ 1, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_B,} },
 	{ 1, TEGRA_DC_FEATURE_PREFERRED_FORMATS, {TEGRA_WIN_PREF_FMT_WIN_B,} },
 	{ 1, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
-	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 16, 4095, 16,} },
+	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
 	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
 	{ 1, TEGRA_DC_FEATURE_FILTER_TYPE, {1, 1,} },
 	{ 1, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1,} },
@@ -40,7 +40,7 @@ static struct tegra_dc_feature_entry t20_feature_entries_a[] = {
 
 	{ 2, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_C,} },
 	{ 2, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
-	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 16, 4095, 16,} },
+	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
 	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
 	{ 2, TEGRA_DC_FEATURE_FILTER_TYPE, {0, 1,} },
 	{ 2, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1,} },
@@ -50,7 +50,7 @@ static struct tegra_dc_feature_entry t20_feature_entries_a[] = {
 static struct tegra_dc_feature_entry t20_feature_entries_b[] = {
 	{ 0, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_A,} },
 	{ 0, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
-	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 16, 4095, 16,} },
+	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
 	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
 	{ 0, TEGRA_DC_FEATURE_FILTER_TYPE, {0, 0,} },
 	{ 0, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1,} },
@@ -59,7 +59,7 @@ static struct tegra_dc_feature_entry t20_feature_entries_b[] = {
 	{ 1, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_B,} },
 	{ 1, TEGRA_DC_FEATURE_PREFERRED_FORMATS, {TEGRA_WIN_PREF_FMT_WIN_B,} },
 	{ 1, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
-	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 16, 4095, 16,} },
+	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
 	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
 	{ 1, TEGRA_DC_FEATURE_FILTER_TYPE, {1, 1,} },
 	{ 1, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1,} },
@@ -67,7 +67,7 @@ static struct tegra_dc_feature_entry t20_feature_entries_b[] = {
 
 	{ 2, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_C,} },
 	{ 2, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
-	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 16, 4095, 16,} },
+	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
 	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
 	{ 2, TEGRA_DC_FEATURE_FILTER_TYPE, {0, 1,} },
 	{ 2, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1,} },
@@ -85,7 +85,7 @@ struct tegra_dc_feature t20_feature_table_b = {
 static struct tegra_dc_feature_entry t30_feature_entries_a[] = {
 	{ 0, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_A,} },
 	{ 0, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
-	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 16, 4095, 16,} },
+	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
 	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
 	{ 0, TEGRA_DC_FEATURE_FILTER_TYPE, {0, 0,} },
 	{ 0, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1} },
@@ -94,7 +94,7 @@ static struct tegra_dc_feature_entry t30_feature_entries_a[] = {
 	{ 1, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_B,} },
 	{ 1, TEGRA_DC_FEATURE_PREFERRED_FORMATS, {TEGRA_WIN_PREF_FMT_WIN_B,} },
 	{ 1, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
-	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 16, 4095, 16,} },
+	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
 	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
 	{ 1, TEGRA_DC_FEATURE_FILTER_TYPE, {1, 1,} },
 	{ 1, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1} },
@@ -102,7 +102,7 @@ static struct tegra_dc_feature_entry t30_feature_entries_a[] = {
 
 	{ 2, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_C,} },
 	{ 2, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
-	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 16, 4095, 16,} },
+	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
 	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
 	{ 2, TEGRA_DC_FEATURE_FILTER_TYPE, {0, 1,} },
 	{ 2, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1} },
@@ -112,7 +112,7 @@ static struct tegra_dc_feature_entry t30_feature_entries_a[] = {
 static struct tegra_dc_feature_entry t30_feature_entries_b[] = {
 	{ 0, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_A,} },
 	{ 0, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
-	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 16, 4095, 16,} },
+	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
 	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
 	{ 0, TEGRA_DC_FEATURE_FILTER_TYPE, {0, 0,} },
 	{ 0, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1,} },
@@ -121,7 +121,7 @@ static struct tegra_dc_feature_entry t30_feature_entries_b[] = {
 	{ 1, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_B,} },
 	{ 1, TEGRA_DC_FEATURE_PREFERRED_FORMATS, {TEGRA_WIN_PREF_FMT_WIN_B,} },
 	{ 1, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
-	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 16, 4095, 16,} },
+	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
 	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
 	{ 1, TEGRA_DC_FEATURE_FILTER_TYPE, {1, 1,} },
 	{ 1, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1,} },
@@ -129,7 +129,7 @@ static struct tegra_dc_feature_entry t30_feature_entries_b[] = {
 
 	{ 2, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_C,} },
 	{ 2, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
-	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 16, 4095, 16,} },
+	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
 	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
 	{ 2, TEGRA_DC_FEATURE_FILTER_TYPE, {0, 1,} },
 	{ 2, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1,} },
diff --git a/drivers/video/tegra/fb.c b/drivers/video/tegra/fb.c
index 59c162bb3b4c..f69048f62cc2 100644
--- a/drivers/video/tegra/fb.c
+++ b/drivers/video/tegra/fb.c
@@ -126,12 +126,9 @@ static int tegra_fb_set_par(struct fb_info *info)
 			return -EINVAL;
 		}
 		/* if line_length unset, then pad the stride */
-		if (!info->fix.line_length) {
-			info->fix.line_length = var->xres * var->bits_per_pixel
-				/ 8;
-			info->fix.line_length = round_up(info->fix.line_length,
-						TEGRA_LINEAR_PITCH_ALIGNMENT);
-		}
+		info->fix.line_length = var->xres * var->bits_per_pixel / 8;
+		info->fix.line_length = round_up(info->fix.line_length,
+					TEGRA_LINEAR_PITCH_ALIGNMENT);
 		tegra_fb->win->stride = info->fix.line_length;
 		tegra_fb->win->stride_uv = 0;
 		tegra_fb->win->phys_addr_u = 0;
@@ -496,33 +493,15 @@ const struct fb_videomode *tegra_fb_find_best_mode(
 	return best;
 }
 
-static int tegra_fb_activate_mode(struct tegra_fb_info *fb_info,
-				struct fb_var_screeninfo *var)
-{
-	int err;
-	struct fb_info *info = fb_info->info;
-
-	var->activate |= FB_ACTIVATE_FORCE;
-	console_lock();
-	info->flags |= FBINFO_MISC_USEREVENT;
-	err = fb_set_var(info, var);
-	info->flags &= ~FBINFO_MISC_USEREVENT;
-	console_unlock();
-	if (err)
-		return err;
-	return 0;
-}
-
 void tegra_fb_update_monspecs(struct tegra_fb_info *fb_info,
 			      struct fb_monspecs *specs,
 			      bool (*mode_filter)(const struct tegra_dc *dc,
 						  struct fb_videomode *mode))
 {
 	int i;
-	int ret = 0;
+	bool first = false;
 	struct fb_event event;
 	struct fb_info *info = fb_info->info;
-	const struct fb_videomode *best_mode = NULL;
 	struct fb_var_screeninfo var = {0,};
 
 	mutex_lock(&fb_info->info->lock);
@@ -561,6 +540,13 @@ void tegra_fb_update_monspecs(struct tegra_fb_info *fb_info,
 				fb_var_to_videomode(&m, &var);
 				fb_add_videomode(&m,
 						 &fb_info->info->modelist);
+				/* EDID stds recommend first detailed mode
+				to be applied as default,but if first mode
+				doesn't pass mode filter, we have to select
+				and apply other mode. So flag on if first
+				mode passes mode filter */
+				if (!i)
+					first = true;
 			}
 		} else {
 			fb_add_videomode(&specs->modedb[i],
@@ -568,36 +554,31 @@ void tegra_fb_update_monspecs(struct tegra_fb_info *fb_info,
 		}
 	}
 
-	/* Get the best mode from modedb and apply on fb */
-	var.xres = 0;
-	var.yres = 0;
-	best_mode = tegra_fb_find_best_mode(&var, &info->modelist);
-
-	/* Update framebuffer with best mode */
-	fb_videomode_to_var(&var, best_mode);
-
-	/* TODO: Get proper way of getting rid of a 0 bpp */
-	if (!var.bits_per_pixel)
-		var.bits_per_pixel = 32;
-
-	memcpy(&info->var, &var, sizeof(struct fb_var_screeninfo));
-
-	ret = tegra_fb_activate_mode(fb_info, &var);
-	if (ret)
-		return;
+	/* We can't apply first detailed mode, so get the best mode
+	based on resolution and apply on fb */
+	if (!first) {
+		var.xres = 0;
+		var.yres = 0;
+		info->mode = (struct fb_videomode *)
+			tegra_fb_find_best_mode(&var, &info->modelist);
+	}
 
+	/* Prepare fb info with new mode details */
+	fb_videomode_to_var(&info->var, info->mode);
 	event.info = fb_info->info;
 
 #ifdef CONFIG_FRAMEBUFFER_CONSOLE
-/* Lock the console before sending the noti. Fbconsole
-  * on HDMI might be using console
-  */
+	/* Send a noti to change fb_display[].mode for all vc's */
+	console_lock();
+	fb_notifier_call_chain(FB_EVENT_MODE_CHANGE_ALL, &event);
+	console_unlock();
+
+	/* Notify framebuffer console about mode change */
 	console_lock();
-#endif
 	fb_notifier_call_chain(FB_EVENT_NEW_MODELIST, &event);
-#ifdef CONFIG_FRAMEBUFFER_CONSOLE
-/* Unlock the console */
 	console_unlock();
+#else
+	fb_notifier_call_chain(FB_EVENT_NEW_MODELIST, &event);
 #endif
 
 	mutex_unlock(&fb_info->info->lock);
@@ -649,12 +630,8 @@ struct tegra_fb_info *tegra_fb_register(struct nvhost_device *ndev,
 		tegra_fb->valid = true;
 	}
 
-	info->fix.line_length = fb_data->xres * fb_data->bits_per_pixel / 8;
-
-	stride = tegra_dc_get_stride(dc, 0);
-	if (!stride) /* default to pad the stride */
-		stride = round_up(info->fix.line_length,
-			TEGRA_LINEAR_PITCH_ALIGNMENT);
+	stride = fb_data->xres * fb_data->bits_per_pixel / 8;
+	stride = round_up(stride, TEGRA_LINEAR_PITCH_ALIGNMENT);
 
 	info->fbops = &tegra_fb_ops;
 	info->pseudo_palette = pseudo_palette;
diff --git a/drivers/video/tegra/host/bus_client.c b/drivers/video/tegra/host/bus_client.c
index aaa038221971..4614689ec37c 100644
--- a/drivers/video/tegra/host/bus_client.c
+++ b/drivers/video/tegra/host/bus_client.c
@@ -658,3 +658,15 @@ fail:
 
 	return -ENXIO;
 }
+
+void nvhost_client_device_put_resources(struct nvhost_device *dev)
+{
+	struct resource *r;
+
+	r = nvhost_get_resource(dev, IORESOURCE_MEM, 0);
+	BUG_ON(!r);
+
+	iounmap(dev->aperture);
+
+	release_mem_region(r->start, resource_size(r));
+}
diff --git a/drivers/video/tegra/host/bus_client.h b/drivers/video/tegra/host/bus_client.h
index e95ea0bc3401..8c7bdc9faefe 100644
--- a/drivers/video/tegra/host/bus_client.h
+++ b/drivers/video/tegra/host/bus_client.h
@@ -37,5 +37,6 @@ int nvhost_client_device_init(struct nvhost_device *dev);
 int nvhost_client_device_suspend(struct nvhost_device *dev);
 
 int nvhost_client_device_get_resources(struct nvhost_device *dev);
+void nvhost_client_device_put_resources(struct nvhost_device *dev);
 
 #endif
diff --git a/drivers/video/tegra/nvmap/nvmap.c b/drivers/video/tegra/nvmap/nvmap.c
index a0dcf26337f8..19b821d41284 100644
--- a/drivers/video/tegra/nvmap/nvmap.c
+++ b/drivers/video/tegra/nvmap/nvmap.c
@@ -605,3 +605,16 @@ void nvmap_free(struct nvmap_client *client, struct nvmap_handle_ref *r)
 
 	nvmap_free_handle_id(client, nvmap_ref_to_id(r));
 }
+
+int nvmap_mark_global(struct nvmap_client *client, struct nvmap_handle_ref *r)
+{
+	struct nvmap_handle *h;
+
+	h = nvmap_get_handle_id(client, (unsigned long)r->handle);
+	if (!h)
+		return -EINVAL;
+	r->handle->global = true;
+	nvmap_handle_put(h);
+
+	return 0;
+}