Merge branch 'buckets/media' into after-buckets

20 files changed, 4684 insertions, 337 deletions

diff --git a/drivers/media/video/tegra/Kconfig b/drivers/media/video/tegra/Kconfig
index ae77e8994dc8..afcbec6b25ab 100644
--- a/drivers/media/video/tegra/Kconfig
+++ b/drivers/media/video/tegra/Kconfig
@@ -1,4 +1,5 @@
 source "drivers/media/video/tegra/avp/Kconfig"
+source "drivers/media/video/tegra/mediaserver/Kconfig"
 
 config TEGRA_CAMERA
         bool "Enable support for tegra camera/isp hardware"
@@ -8,3 +9,44 @@ config TEGRA_CAMERA
           Enables support for the Tegra camera interface
 
           If unsure, say Y
+
+config VIDEO_OV5650
+        tristate "OV5650 camera sensor support"
+        depends on I2C && ARCH_TEGRA
+        ---help---
+          This is a driver for the Omnivision OV5650 5MP camera sensor
+          for use with the tegra isp.
+
+config VIDEO_OV2710
+        tristate "OV2710 camera sensor support"
+        depends on I2C && ARCH_TEGRA
+        ---help---
+          This is a driver for the Omnivision OV2710 camera sensor
+          for use with the tegra isp.
+
+config VIDEO_SOC380
+        tristate "SOC380 camera sensor support"
+        depends on I2C && ARCH_TEGRA
+        ---help---
+          This is a driver for the Semco soc380 camera sensor
+          for use with the tegra isp.
+
+config TORCH_SSL3250A
+        tristate "SSL3250A flash/torch support"
+        depends on I2C && ARCH_TEGRA
+        ---help---
+          This is a driver for the SSL3250A flash/torch camera device
+
+config VIDEO_SH532U
+        tristate "SH532U focuser support"
+        depends on I2C && ARCH_TEGRA
+        ---help---
+          This is a driver for the SEMCO SH532U focuser
+          for use with the tegra isp.
+
+config VIDEO_AD5820
+        tristate "AD5820 focuser support"
+        depends on I2C && ARCH_TEGRA
+        ---help---
+          This is a driver for the AD5820 focuser
+          for use with the tegra isp.
diff --git a/drivers/media/video/tegra/Makefile b/drivers/media/video/tegra/Makefile
index 68b5c42b0e7a..516fd7e0445a 100644
--- a/drivers/media/video/tegra/Makefile
+++ b/drivers/media/video/tegra/Makefile
@@ -1,2 +1,10 @@
 obj-y				+= avp/
+obj-$(CONFIG_TEGRA_MEDIASERVER)		+= mediaserver/
 obj-$(CONFIG_TEGRA_CAMERA)		+= tegra_camera.o
+obj-$(CONFIG_VIDEO_OV5650)		+= ov5650.o
+obj-$(CONFIG_VIDEO_OV2710)		+= ov2710.o
+obj-$(CONFIG_VIDEO_SOC380)		+= soc380.o
+obj-$(CONFIG_TORCH_SSL3250A)		+= ssl3250a.o
+obj-$(CONFIG_VIDEO_SH532U)		+= sh532u.o
+obj-$(CONFIG_VIDEO_AD5820)		+= ad5820.o
+
diff --git a/drivers/media/video/tegra/ad5820.c b/drivers/media/video/tegra/ad5820.c
new file mode 100644
index 000000000000..365c0869dc86
--- /dev/null
+++ b/drivers/media/video/tegra/ad5820.c
@@ -0,0 +1,215 @@
+/*
+ * AD5820 focuser driver.
+ *
+ * Copyright (C) 2010-2011 NVIDIA Corporation.
+ *
+ * Contributors:
+ *      Sachin Nikam <snikam@nvidia.com>
+ *
+ * Based on ov5650.c.
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ */
+
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <media/ad5820.h>
+
+#define POS_LOW (144)
+#define POS_HIGH (520)
+#define SETTLETIME_MS 100
+#define FOCAL_LENGTH (4.507f)
+#define FNUMBER (2.8f)
+#define FPOS_COUNT 1024
+
+#define AD5820_MAX_RETRIES (3)
+
+struct ad5820_info {
+	struct i2c_client *i2c_client;
+	struct regulator *regulator;
+	struct ad5820_config config;
+};
+
+static int ad5820_write(struct i2c_client *client, u16 value)
+{
+	int count;
+	struct i2c_msg msg[1];
+	unsigned char data[2];
+	int retry = 0;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data[1] = (u8) ((value >> 4) & 0x3F);
+	data[0] = (u8) ((value & 0xF) << 4);
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = ARRAY_SIZE(data);
+	msg[0].buf = data;
+
+	do {
+		count = i2c_transfer(client->adapter, msg, ARRAY_SIZE(msg));
+		if (count == ARRAY_SIZE(msg))
+			return 0;
+		retry++;
+		pr_err("ad5820: i2c transfer failed, retrying %x\n",
+		       value);
+		msleep(3);
+	} while (retry <= AD5820_MAX_RETRIES);
+	return -EIO;
+}
+
+static int ad5820_set_position(struct ad5820_info *info, u32 position)
+{
+	if (position < info->config.pos_low ||
+	    position > info->config.pos_high)
+		return -EINVAL;
+
+	return ad5820_write(info->i2c_client, position);
+}
+
+static long ad5820_ioctl(struct file *file,
+			unsigned int cmd, unsigned long arg)
+{
+	struct ad5820_info *info = file->private_data;
+
+	switch (cmd) {
+	case AD5820_IOCTL_GET_CONFIG:
+	{
+		if (copy_to_user((void __user *) arg,
+				 &info->config,
+				 sizeof(info->config))) {
+			pr_err("%s: 0x%x\n", __func__, __LINE__);
+			return -EFAULT;
+		}
+
+		break;
+	}
+	case AD5820_IOCTL_SET_POSITION:
+		return ad5820_set_position(info, (u32) arg);
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+struct ad5820_info *info;
+
+static int ad5820_open(struct inode *inode, struct file *file)
+{
+	file->private_data = info;
+	if (info->regulator)
+		regulator_enable(info->regulator);
+	return 0;
+}
+
+int ad5820_release(struct inode *inode, struct file *file)
+{
+	if (info->regulator)
+		regulator_disable(info->regulator);
+	file->private_data = NULL;
+	return 0;
+}
+
+
+static const struct file_operations ad5820_fileops = {
+	.owner = THIS_MODULE,
+	.open = ad5820_open,
+	.unlocked_ioctl = ad5820_ioctl,
+	.release = ad5820_release,
+};
+
+static struct miscdevice ad5820_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "ad5820",
+	.fops = &ad5820_fileops,
+};
+
+static int ad5820_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int err;
+
+	pr_info("ad5820: probing sensor.\n");
+
+	info = kzalloc(sizeof(struct ad5820_info), GFP_KERNEL);
+	if (!info) {
+		pr_err("ad5820: Unable to allocate memory!\n");
+		return -ENOMEM;
+	}
+
+	err = misc_register(&ad5820_device);
+	if (err) {
+		pr_err("ad5820: Unable to register misc device!\n");
+		kfree(info);
+		return err;
+	}
+
+	info->regulator = regulator_get(&client->dev, "vdd_vcore_af");
+	if (IS_ERR_OR_NULL(info->regulator)) {
+		dev_err(&client->dev, "unable to get regulator %s\n",
+			dev_name(&client->dev));
+		info->regulator = NULL;
+	} else {
+		regulator_enable(info->regulator);
+	}
+
+	info->i2c_client = client;
+	info->config.settle_time = SETTLETIME_MS;
+	info->config.focal_length = FOCAL_LENGTH;
+	info->config.fnumber = FNUMBER;
+	info->config.pos_low = POS_LOW;
+	info->config.pos_high = POS_HIGH;
+	i2c_set_clientdata(client, info);
+	return 0;
+}
+
+static int ad5820_remove(struct i2c_client *client)
+{
+	struct ad5820_info *info;
+	info = i2c_get_clientdata(client);
+	misc_deregister(&ad5820_device);
+	kfree(info);
+	return 0;
+}
+
+static const struct i2c_device_id ad5820_id[] = {
+	{ "ad5820", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, ad5820_id);
+
+static struct i2c_driver ad5820_i2c_driver = {
+	.driver = {
+		.name = "ad5820",
+		.owner = THIS_MODULE,
+	},
+	.probe = ad5820_probe,
+	.remove = ad5820_remove,
+	.id_table = ad5820_id,
+};
+
+static int __init ad5820_init(void)
+{
+	pr_info("ad5820 sensor driver loading\n");
+	return i2c_add_driver(&ad5820_i2c_driver);
+}
+
+static void __exit ad5820_exit(void)
+{
+	i2c_del_driver(&ad5820_i2c_driver);
+}
+
+module_init(ad5820_init);
+module_exit(ad5820_exit);
+
diff --git a/drivers/media/video/tegra/avp/avp.c b/drivers/media/video/tegra/avp/avp.c
index ced838ac6e2b..dd6b60aba4af 100644
--- a/drivers/media/video/tegra/avp/avp.c
+++ b/drivers/media/video/tegra/avp/avp.c
@@ -2,6 +2,8 @@
  * Copyright (C) 2010 Google, Inc.
  * Author: Dima Zavin <dima@android.com>
  *
+ * Copyright (C) 2010-2011 NVIDIA Corporation
+ *
  * This software is licensed under the terms of the GNU General Public
  * License version 2, as published by the Free Software Foundation, and
  * may be copied, distributed, and modified under those terms.
@@ -48,6 +50,7 @@
 #include "avp_msg.h"
 #include "trpc.h"
 #include "avp.h"
+#include "nvavp.h"
 
 enum {
 	AVP_DBG_TRACE_XPC	= 1U << 0,
@@ -59,7 +62,15 @@ enum {
 	AVP_DBG_TRACE_LIB	= 1U << 6,
 };
 
-static u32 avp_debug_mask = 0;
+static u32 avp_debug_mask =
+	AVP_DBG_TRACE_XPC	|
+	/* AVP_DBG_TRACE_XPC_IRQ	| */
+	/* AVP_DBG_TRACE_XPC_MSG	| */
+	/* AVP_DBG_TRACE_TRPC_MSG	| */
+	AVP_DBG_TRACE_XPC_CONN	|
+	AVP_DBG_TRACE_TRPC_CONN	|
+	AVP_DBG_TRACE_LIB;
+
 module_param_named(debug_mask, avp_debug_mask, uint, S_IWUSR | S_IRUGO);
 
 #define DBG(flag, args...) \
@@ -67,9 +78,7 @@ module_param_named(debug_mask, avp_debug_mask, uint, S_IWUSR | S_IRUGO);
 
 #define TEGRA_AVP_NAME			"tegra-avp"
 
-#define TEGRA_AVP_KERNEL_FW		"nvrm_avp.bin"
-
-#define TEGRA_AVP_RESET_VECTOR_ADDR	\
+#define TEGRA_AVP_RESET_VECTOR_ADDR \
 		(IO_ADDRESS(TEGRA_EXCEPTION_VECTORS_BASE) + 0x200)
 
 #define TEGRA_AVP_RESUME_ADDR		IO_ADDRESS(TEGRA_IRAM_BASE)
@@ -82,94 +91,95 @@ module_param_named(debug_mask, avp_debug_mask, uint, S_IWUSR | S_IRUGO);
 #define MBOX_TO_AVP			IO_ADDRESS(TEGRA_RES_SEMA_BASE + 0x20)
 
 /* Layout of the mailbox registers:
- * bit 31	- pending message interrupt enable (mailbox full, i.e. valid=1)
- * bit 30	- message cleared interrupt enable (mailbox empty, i.e. valid=0)
- * bit 29	- message valid. peer clears this bit after reading msg
- * bits 27:0	- message data
+ * bit 31    - pending message interrupt enable (mailbox full, i.e. valid=1)
+ * bit 30    - message cleared interrupt enable (mailbox empty, i.e. valid=0)
+ * bit 29    - message valid. peer clears this bit after reading msg
+ * bits 27:0 - message data
  */
 #define MBOX_MSG_PENDING_INT_EN		(1 << 31)
 #define MBOX_MSG_READ_INT_EN		(1 << 30)
 #define MBOX_MSG_VALID			(1 << 29)
 
-#define AVP_MSG_MAX_CMD_LEN		16
+#define AVP_MSG_MAX_CMD_LEN	16
 #define AVP_MSG_AREA_SIZE	(AVP_MSG_MAX_CMD_LEN + TEGRA_RPC_MAX_MSG_LEN)
 
-struct avp_info {
-	struct clk			*cop_clk;
+struct tegra_avp_info {
+	struct clk		*cop_clk;
 
-	int				mbox_from_avp_pend_irq;
+	int			mbox_from_avp_pend_irq;
 
-	dma_addr_t			msg_area_addr;
-	u32				msg;
-	void				*msg_to_avp;
-	void				*msg_from_avp;
-	struct mutex			to_avp_lock;
-	struct mutex			from_avp_lock;
+	dma_addr_t		msg_area_addr;
+	u32			msg;
+	void			*msg_to_avp;
+	void			*msg_from_avp;
+	struct mutex		to_avp_lock;
+	struct mutex		from_avp_lock;
 
-	struct work_struct		recv_work;
-	struct workqueue_struct		*recv_wq;
+	struct work_struct	recv_work;
+	struct workqueue_struct	*recv_wq;
 
-	struct trpc_node		*rpc_node;
-	struct miscdevice		misc_dev;
-	bool				opened;
-	struct mutex			open_lock;
+	struct trpc_node	*rpc_node;
+	struct miscdevice	misc_dev;
+	int			refcount;
+	struct mutex		open_lock;
 
-	spinlock_t			state_lock;
-	bool				initialized;
-	bool				shutdown;
-	bool				suspending;
-	bool				defer_remote;
+	spinlock_t		state_lock;
+	bool			initialized;
+	bool			shutdown;
+	bool			suspending;
+	bool			defer_remote;
 
-	struct mutex			libs_lock;
-	struct list_head		libs;
-	struct nvmap_client		*nvmap_libs;
+	struct mutex		libs_lock;
+	struct list_head	libs;
+	struct nvmap_client	*nvmap_libs;
 
 	/* client for driver allocations, persistent */
-	struct nvmap_client		*nvmap_drv;
-	struct nvmap_handle_ref		*kernel_handle;
-	void				*kernel_data;
-	unsigned long			kernel_phys;
+	struct nvmap_client	*nvmap_drv;
+	struct nvmap_handle_ref	*kernel_handle;
+	void			*kernel_data;
+	phys_addr_t		kernel_phys;
 
-	struct nvmap_handle_ref		*iram_backup_handle;
-	void				*iram_backup_data;
-	unsigned long			iram_backup_phys;
-	unsigned long			resume_addr;
+	struct nvmap_handle_ref *iram_backup_handle;
+	void			*iram_backup_data;
+	phys_addr_t		iram_backup_phys;
+	unsigned long		resume_addr;
+	unsigned long		reset_addr;
 
-	struct trpc_endpoint		*avp_ep;
-	struct rb_root			endpoints;
+	struct trpc_endpoint	*avp_ep;
+	struct rb_root		endpoints;
 
-	struct avp_svc_info		*avp_svc;
+	struct avp_svc_info	*avp_svc;
 };
 
 struct remote_info {
-	u32				loc_id;
-	u32				rem_id;
-	struct kref			ref;
+	u32			loc_id;
+	u32			rem_id;
+	struct kref		ref;
 
-	struct trpc_endpoint		*trpc_ep;
-	struct rb_node			rb_node;
+	struct trpc_endpoint	*trpc_ep;
+	struct rb_node		rb_node;
 };
 
 struct lib_item {
-	struct list_head		list;
-	u32				handle;
-	char				name[TEGRA_AVP_LIB_MAX_NAME];
+	struct list_head	list;
+	u32			handle;
+	char			name[TEGRA_AVP_LIB_MAX_NAME];
 };
 
-static struct avp_info *tegra_avp;
+static struct tegra_avp_info *tegra_avp;
 
 static int avp_trpc_send(struct trpc_endpoint *ep, void *buf, size_t len);
 static void avp_trpc_close(struct trpc_endpoint *ep);
 static void avp_trpc_show(struct seq_file *s, struct trpc_endpoint *ep);
-static void libs_cleanup(struct avp_info *avp);
+static void libs_cleanup(struct tegra_avp_info *avp);
 
 static struct trpc_ep_ops remote_ep_ops = {
-	.send	= avp_trpc_send,
-	.close	= avp_trpc_close,
-	.show	= avp_trpc_show,
+	.send   = avp_trpc_send,
+	.close  = avp_trpc_close,
+	.show   = avp_trpc_show,
 };
 
-static struct remote_info *rinfo_alloc(struct avp_info *avp)
+static struct remote_info *rinfo_alloc(struct tegra_avp_info *avp)
 {
 	struct remote_info *rinfo;
 
@@ -196,7 +206,7 @@ static inline void rinfo_put(struct remote_info *rinfo)
 	kref_put(&rinfo->ref, _rinfo_release);
 }
 
-static int remote_insert(struct avp_info *avp, struct remote_info *rinfo)
+static int remote_insert(struct tegra_avp_info *avp, struct remote_info *rinfo)
 {
 	struct rb_node **p;
 	struct rb_node *parent;
@@ -225,7 +235,7 @@ static int remote_insert(struct avp_info *avp, struct remote_info *rinfo)
 	return 0;
 }
 
-static struct remote_info *remote_find(struct avp_info *avp, u32 local_id)
+static struct remote_info *remote_find(struct tegra_avp_info *avp, u32 local_id)
 {
 	struct rb_node *n = avp->endpoints.rb_node;
 	struct remote_info *rinfo;
@@ -243,7 +253,7 @@ static struct remote_info *remote_find(struct avp_info *avp, u32 local_id)
 	return NULL;
 }
 
-static void remote_remove(struct avp_info *avp, struct remote_info *rinfo)
+static void remote_remove(struct tegra_avp_info *avp, struct remote_info *rinfo)
 {
 	rb_erase(&rinfo->rb_node, &avp->endpoints);
 	rinfo_put(rinfo);
@@ -251,8 +261,8 @@ static void remote_remove(struct avp_info *avp, struct remote_info *rinfo)
 
 /* test whether or not the trpc endpoint provided is a valid AVP node
  * endpoint */
-static struct remote_info *validate_trpc_ep(struct avp_info *avp,
-					    struct trpc_endpoint *ep)
+static struct remote_info *validate_trpc_ep(struct tegra_avp_info *avp,
+						struct trpc_endpoint *ep)
 {
 	struct remote_info *tmp = trpc_priv(ep);
 	struct remote_info *rinfo;
@@ -267,7 +277,7 @@ static struct remote_info *validate_trpc_ep(struct avp_info *avp,
 
 static void avp_trpc_show(struct seq_file *s, struct trpc_endpoint *ep)
 {
-	struct avp_info *avp = tegra_avp;
+	struct tegra_avp_info *avp = tegra_avp;
 	struct remote_info *rinfo;
 	unsigned long flags;
 
@@ -277,7 +287,7 @@ static void avp_trpc_show(struct seq_file *s, struct trpc_endpoint *ep)
 		seq_printf(s, "    <unknown>\n");
 		goto out;
 	}
-	seq_printf(s, "    loc_id:0x%x\n    rem_id:0x%x\n",
+	seq_printf(s, "    loc_id:0x%x\n	rem_id:0x%x\n",
 		   rinfo->loc_id, rinfo->rem_id);
 out:
 	spin_unlock_irqrestore(&avp->state_lock, flags);
@@ -293,7 +303,7 @@ static inline u32 mbox_readl(void __iomem *mbox)
 	return readl(mbox);
 }
 
-static inline void msg_ack_remote(struct avp_info *avp, u32 cmd, u32 arg)
+static inline void msg_ack_remote(struct tegra_avp_info *avp, u32 cmd, u32 arg)
 {
 	struct msg_ack *ack = avp->msg_from_avp;
 
@@ -304,15 +314,15 @@ static inline void msg_ack_remote(struct avp_info *avp, u32 cmd, u32 arg)
 	wmb();
 }
 
-static inline u32 msg_recv_get_cmd(struct avp_info *avp)
+static inline u32 msg_recv_get_cmd(struct tegra_avp_info *avp)
 {
 	volatile u32 *cmd = avp->msg_from_avp;
 	rmb();
 	return *cmd;
 }
 
-static inline int __msg_write(struct avp_info *avp, void *hdr, size_t hdr_len,
-			      void *buf, size_t len)
+static inline int __msg_write(struct tegra_avp_info *avp, void *hdr,
+			      size_t hdr_len, void *buf, size_t len)
 {
 	memcpy(avp->msg_to_avp, hdr, hdr_len);
 	if (buf && len)
@@ -321,8 +331,8 @@ static inline int __msg_write(struct avp_info *avp, void *hdr, size_t hdr_len,
 	return 0;
 }
 
-static inline int msg_write(struct avp_info *avp, void *hdr, size_t hdr_len,
-			    void *buf, size_t len)
+static inline int msg_write(struct tegra_avp_info *avp, void *hdr,
+			    size_t hdr_len, void *buf, size_t len)
 {
 	/* rem_ack is a pointer into shared memory that the AVP modifies */
 	volatile u32 *rem_ack = avp->msg_to_avp;
@@ -341,7 +351,7 @@ static inline int msg_write(struct avp_info *avp, void *hdr, size_t hdr_len,
 	return 0;
 }
 
-static inline int msg_check_ack(struct avp_info *avp, u32 cmd, u32 *arg)
+static inline int msg_check_ack(struct tegra_avp_info *avp, u32 cmd, u32 *arg)
 {
 	struct msg_ack ack;
 
@@ -355,7 +365,7 @@ static inline int msg_check_ack(struct avp_info *avp, u32 cmd, u32 *arg)
 }
 
 /* XXX: add timeout */
-static int msg_wait_ack_locked(struct avp_info *avp, u32 cmd, u32 *arg)
+static int msg_wait_ack_locked(struct tegra_avp_info *avp, u32 cmd, u32 *arg)
 {
 	/* rem_ack is a pointer into shared memory that the AVP modifies */
 	volatile u32 *rem_ack = avp->msg_to_avp;
@@ -379,14 +389,14 @@ static int msg_wait_ack_locked(struct avp_info *avp, u32 cmd, u32 *arg)
 
 static int avp_trpc_send(struct trpc_endpoint *ep, void *buf, size_t len)
 {
-	struct avp_info *avp = tegra_avp;
+	struct tegra_avp_info *avp = tegra_avp;
 	struct remote_info *rinfo;
 	struct msg_port_data msg;
 	int ret;
 	unsigned long flags;
 
 	DBG(AVP_DBG_TRACE_TRPC_MSG, "%s: ep=%p priv=%p buf=%p len=%d\n",
-	    __func__, ep, trpc_priv(ep), buf, len);
+		__func__, ep, trpc_priv(ep), buf, len);
 
 	spin_lock_irqsave(&avp->state_lock, flags);
 	if (unlikely(avp->suspending && trpc_peer(ep) != avp->avp_ep)) {
@@ -413,7 +423,7 @@ static int avp_trpc_send(struct trpc_endpoint *ep, void *buf, size_t len)
 	mutex_unlock(&avp->to_avp_lock);
 
 	DBG(AVP_DBG_TRACE_TRPC_MSG, "%s: msg sent for %s (%x->%x) (%d)\n",
-	    __func__, trpc_name(ep), rinfo->loc_id, rinfo->rem_id, ret);
+		__func__, trpc_name(ep), rinfo->loc_id, rinfo->rem_id, ret);
 	rinfo_put(rinfo);
 	return ret;
 
@@ -422,7 +432,7 @@ err_state_locked:
 	return ret;
 }
 
-static int _send_disconnect(struct avp_info *avp, u32 port_id)
+static int _send_disconnect(struct tegra_avp_info *avp, u32 port_id)
 {
 	struct msg_disconnect msg;
 	int ret;
@@ -434,19 +444,19 @@ static int _send_disconnect(struct avp_info *avp, u32 port_id)
 	ret = msg_write(avp, &msg, sizeof(msg), NULL, 0);
 	if (ret) {
 		pr_err("%s: remote has not acked last message (%x)\n", __func__,
-		       port_id);
+			   port_id);
 		goto err_msg_write;
 	}
 
 	ret = msg_wait_ack_locked(avp, CMD_ACK, NULL);
 	if (ret) {
 		pr_err("%s: remote end won't respond for %x\n", __func__,
-		       port_id);
+			   port_id);
 		goto err_wait_ack;
 	}
 
 	DBG(AVP_DBG_TRACE_XPC_CONN, "%s: sent disconnect msg for %x\n",
-	    __func__, port_id);
+		__func__, port_id);
 
 err_wait_ack:
 err_msg_write:
@@ -471,7 +481,7 @@ static inline void remote_close(struct remote_info *rinfo)
 
 static void avp_trpc_close(struct trpc_endpoint *ep)
 {
-	struct avp_info *avp = tegra_avp;
+	struct tegra_avp_info *avp = tegra_avp;
 	struct remote_info *rinfo;
 	unsigned long flags;
 	int ret;
@@ -485,7 +495,7 @@ static void avp_trpc_close(struct trpc_endpoint *ep)
 	rinfo = validate_trpc_ep(avp, ep);
 	if (!rinfo) {
 		pr_err("%s: tried to close invalid port '%s' endpoint (%p)\n",
-		       __func__, trpc_name(ep), ep);
+			   __func__, trpc_name(ep), ep);
 		spin_unlock_irqrestore(&avp->state_lock, flags);
 		return;
 	}
@@ -494,18 +504,18 @@ static void avp_trpc_close(struct trpc_endpoint *ep)
 	spin_unlock_irqrestore(&avp->state_lock, flags);
 
 	DBG(AVP_DBG_TRACE_TRPC_CONN, "%s: closing '%s' (%x)\n", __func__,
-	    trpc_name(ep), rinfo->rem_id);
+		trpc_name(ep), rinfo->rem_id);
 
 	ret = _send_disconnect(avp, rinfo->rem_id);
 	if (ret)
 		pr_err("%s: error while closing remote port '%s' (%x)\n",
-		       __func__, trpc_name(ep), rinfo->rem_id);
+			   __func__, trpc_name(ep), rinfo->rem_id);
 	remote_close(rinfo);
 	rinfo_put(rinfo);
 }
 
 /* takes and holds avp->from_avp_lock */
-static void recv_msg_lock(struct avp_info *avp)
+static void recv_msg_lock(struct tegra_avp_info *avp)
 {
 	unsigned long flags;
 
@@ -516,7 +526,7 @@ static void recv_msg_lock(struct avp_info *avp)
 }
 
 /* MUST be called with avp->from_avp_lock held */
-static void recv_msg_unlock(struct avp_info *avp)
+static void recv_msg_unlock(struct tegra_avp_info *avp)
 {
 	unsigned long flags;
 
@@ -530,7 +540,7 @@ static int avp_node_try_connect(struct trpc_node *node,
 				struct trpc_node *src_node,
 				struct trpc_endpoint *from)
 {
-	struct avp_info *avp = tegra_avp;
+	struct tegra_avp_info *avp = tegra_avp;
 	const char *port_name = trpc_name(from);
 	struct remote_info *rinfo;
 	struct msg_connect msg;
@@ -539,14 +549,14 @@ static int avp_node_try_connect(struct trpc_node *node,
 	int len;
 
 	DBG(AVP_DBG_TRACE_TRPC_CONN, "%s: trying connect from %s\n", __func__,
-	    port_name);
+		port_name);
 
 	if (node != avp->rpc_node || node->priv != avp)
 		return -ENODEV;
 
 	len = strlen(port_name);
 	if (len > XPC_PORT_NAME_LEN) {
-		pr_err("%s: port name (%s) to long\n", __func__, port_name);
+		pr_err("%s: port name (%s) too long\n", __func__, port_name);
 		return -EINVAL;
 	}
 
@@ -595,7 +605,7 @@ static int avp_node_try_connect(struct trpc_node *node,
 	ret = msg_write(avp, &msg, sizeof(msg), NULL, 0);
 	if (ret) {
 		pr_err("%s: remote has not acked last message (%s)\n", __func__,
-		       port_name);
+			   port_name);
 		mutex_unlock(&avp->to_avp_lock);
 		goto err_msg_write;
 	}
@@ -604,7 +614,7 @@ static int avp_node_try_connect(struct trpc_node *node,
 
 	if (ret) {
 		pr_err("%s: remote end won't respond for '%s'\n", __func__,
-		       port_name);
+			   port_name);
 		goto err_wait_ack;
 	}
 	if (!rinfo->rem_id) {
@@ -614,10 +624,10 @@ static int avp_node_try_connect(struct trpc_node *node,
 	}
 
 	DBG(AVP_DBG_TRACE_TRPC_CONN, "%s: got conn ack '%s' (%x <-> %x)\n",
-	    __func__, port_name, rinfo->loc_id, rinfo->rem_id);
+		__func__, port_name, rinfo->loc_id, rinfo->rem_id);
 
 	rinfo->trpc_ep = trpc_create_peer(node, from, &remote_ep_ops,
-					       rinfo);
+						   rinfo);
 	if (!rinfo->trpc_ep) {
 		pr_err("%s: cannot create peer for %s\n", __func__, port_name);
 		ret = -EINVAL;
@@ -646,19 +656,19 @@ err_alloc_rinfo:
 	return ret;
 }
 
-static void process_disconnect_locked(struct avp_info *avp,
-				      struct msg_data *raw_msg)
+static void process_disconnect_locked(struct tegra_avp_info *avp,
+					  struct msg_data *raw_msg)
 {
 	struct msg_disconnect *disconn_msg = (struct msg_disconnect *)raw_msg;
 	unsigned long flags;
 	struct remote_info *rinfo;
 
 	DBG(AVP_DBG_TRACE_XPC_CONN, "%s: got disconnect (%x)\n", __func__,
-	    disconn_msg->port_id);
+		disconn_msg->port_id);
 
 	if (avp_debug_mask & AVP_DBG_TRACE_XPC_MSG)
 		print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, disconn_msg,
-				     sizeof(struct msg_disconnect));
+					 sizeof(struct msg_disconnect));
 
 	spin_lock_irqsave(&avp->state_lock, flags);
 	rinfo = remote_find(avp, disconn_msg->port_id);
@@ -678,7 +688,7 @@ ack:
 	msg_ack_remote(avp, CMD_ACK, 0);
 }
 
-static void process_connect_locked(struct avp_info *avp,
+static void process_connect_locked(struct tegra_avp_info *avp,
 				   struct msg_data *raw_msg)
 {
 	struct msg_connect *conn_msg = (struct msg_connect *)raw_msg;
@@ -690,10 +700,10 @@ static void process_connect_locked(struct avp_info *avp,
 	unsigned long flags;
 
 	DBG(AVP_DBG_TRACE_XPC_CONN, "%s: got connect (%x)\n", __func__,
-	    conn_msg->port_id);
+		conn_msg->port_id);
 	if (avp_debug_mask & AVP_DBG_TRACE_XPC_MSG)
 		print_hex_dump_bytes("", DUMP_PREFIX_OFFSET,
-				     conn_msg, sizeof(struct msg_connect));
+					 conn_msg, sizeof(struct msg_connect));
 
 	rinfo = rinfo_alloc(avp);
 	if (!rinfo) {
@@ -707,10 +717,10 @@ static void process_connect_locked(struct avp_info *avp,
 	memcpy(name, conn_msg->name, XPC_PORT_NAME_LEN);
 	name[XPC_PORT_NAME_LEN] = '\0';
 	trpc_ep = trpc_create_connect(avp->rpc_node, name, &remote_ep_ops,
-				      rinfo, 0);
+					  rinfo, 0);
 	if (IS_ERR(trpc_ep)) {
 		pr_err("%s: remote requested unknown port '%s' (%d)\n",
-		       __func__, name, (int)PTR_ERR(trpc_ep));
+			   __func__, name, (int)PTR_ERR(trpc_ep));
 		goto nack;
 	}
 	rinfo->trpc_ep = trpc_ep;
@@ -733,8 +743,8 @@ ack:
 	msg_ack_remote(avp, CMD_RESPONSE, local_port_id);
 }
 
-static int process_message(struct avp_info *avp, struct msg_data *raw_msg,
-			    gfp_t gfp_flags)
+static int process_message(struct tegra_avp_info *avp, struct msg_data *raw_msg,
+				gfp_t gfp_flags)
 {
 	struct msg_port_data *port_msg = (struct msg_port_data *)raw_msg;
 	struct remote_info *rinfo;
@@ -748,12 +758,12 @@ static int process_message(struct avp_info *avp, struct msg_data *raw_msg,
 		pr_info("%s: got message cmd=%x port=%x len=%d\n", __func__,
 			port_msg->cmd, port_msg->port_id, port_msg->msg_len);
 		print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, port_msg,
-				     sizeof(struct msg_port_data) + len);
+					 sizeof(struct msg_port_data) + len);
 	}
 
 	if (len != port_msg->msg_len)
 		pr_err("%s: message sent is too long (%d bytes)\n", __func__,
-		       port_msg->msg_len);
+			   port_msg->msg_len);
 
 	spin_lock_irqsave(&avp->state_lock, flags);
 	rinfo = remote_find(avp, port_msg->port_id);
@@ -776,8 +786,8 @@ static int process_message(struct avp_info *avp, struct msg_data *raw_msg,
 		goto no_ack;
 	} else if (ret) {
 		pr_err("%s: cannot queue message for port %s/%x (%d)\n",
-		       __func__, trpc_name(rinfo->trpc_ep), rinfo->loc_id,
-		       ret);
+			   __func__, trpc_name(rinfo->trpc_ep), rinfo->loc_id,
+			   ret);
 	} else {
 		DBG(AVP_DBG_TRACE_XPC_MSG, "%s: msg queued\n", __func__);
 	}
@@ -792,7 +802,8 @@ no_ack:
 
 static void process_avp_message(struct work_struct *work)
 {
-	struct avp_info *avp = container_of(work, struct avp_info, recv_work);
+	struct tegra_avp_info *avp = container_of(work, struct tegra_avp_info,
+						  recv_work);
 	struct msg_data *msg = avp->msg_from_avp;
 
 	mutex_lock(&avp->from_avp_lock);
@@ -816,7 +827,7 @@ static void process_avp_message(struct work_struct *work)
 
 static irqreturn_t avp_mbox_pending_isr(int irq, void *data)
 {
-	struct avp_info *avp = data;
+	struct tegra_avp_info *avp = data;
 	struct msg_data *msg = avp->msg_from_avp;
 	u32 mbox_msg;
 	unsigned long flags;
@@ -862,7 +873,7 @@ done:
 	return IRQ_HANDLED;
 }
 
-static int avp_reset(struct avp_info *avp, unsigned long reset_addr)
+static int avp_reset(struct tegra_avp_info *avp, unsigned long reset_addr)
 {
 	unsigned long stub_code_phys = virt_to_phys(_tegra_avp_boot_stub);
 	dma_addr_t stub_data_phys;
@@ -880,6 +891,9 @@ static int avp_reset(struct avp_info *avp, unsigned long reset_addr)
 
 	writel(stub_code_phys, TEGRA_AVP_RESET_VECTOR_ADDR);
 
+	pr_debug("%s: TEGRA_AVP_RESET_VECTOR=%x\n", __func__, readl(TEGRA_AVP_RESET_VECTOR_ADDR));
+	pr_info("%s: Resetting AVP: reset_addr=%lx\n", __func__, reset_addr);
+
 	tegra_periph_reset_assert(avp->cop_clk);
 	udelay(10);
 	tegra_periph_reset_deassert(avp->cop_clk);
@@ -890,12 +904,16 @@ static int avp_reset(struct avp_info *avp, unsigned long reset_addr)
 	 * starts, so a dead kernel can be detected by polling this value */
 	timeout = jiffies + msecs_to_jiffies(2000);
 	while (time_before(jiffies, timeout)) {
+		pr_debug("%s: TEGRA_AVP_RESET_VECTOR=%x\n", __func__, readl(TEGRA_AVP_RESET_VECTOR_ADDR));
 		if (readl(TEGRA_AVP_RESET_VECTOR_ADDR) != stub_code_phys)
 			break;
 		cpu_relax();
 	}
-	if (readl(TEGRA_AVP_RESET_VECTOR_ADDR) == stub_code_phys)
+	if (readl(TEGRA_AVP_RESET_VECTOR_ADDR) == stub_code_phys) {
+		pr_err("%s: Timed out waiting for AVP kernel to start\n", __func__);
 		ret = -EINVAL;
+	}
+	pr_debug("%s: TEGRA_AVP_RESET_VECTOR=%x\n", __func__, readl(TEGRA_AVP_RESET_VECTOR_ADDR));
 	WARN_ON(ret);
 	dma_unmap_single(NULL, stub_data_phys,
 			 sizeof(_tegra_avp_boot_stub_data),
@@ -903,7 +921,7 @@ static int avp_reset(struct avp_info *avp, unsigned long reset_addr)
 	return ret;
 }
 
-static void avp_halt(struct avp_info *avp)
+static void avp_halt(struct tegra_avp_info *avp)
 {
 	/* ensure the AVP is halted */
 	writel(FLOW_MODE_STOP, FLOW_CTRL_HALT_COP_EVENTS);
@@ -922,14 +940,15 @@ static void avp_halt(struct avp_info *avp)
  * of the char dev for receiving replies for managing remote
  * libraries/modules. */
 
-static int avp_init(struct avp_info *avp, const char *fw_file)
+static int avp_init(struct tegra_avp_info *avp)
 {
 	const struct firmware *avp_fw;
 	int ret;
 	struct trpc_endpoint *ep;
+	char fw_file[30];
 
 	avp->nvmap_libs = nvmap_create_client(nvmap_dev, "avp_libs");
-	if (IS_ERR(avp->nvmap_libs)) {
+	if (IS_ERR_OR_NULL(avp->nvmap_libs)) {
 		pr_err("%s: cannot create libs nvmap client\n", __func__);
 		ret = PTR_ERR(avp->nvmap_libs);
 		goto err_nvmap_create_libs_client;
@@ -939,19 +958,64 @@ static int avp_init(struct avp_info *avp, const char *fw_file)
 	 * to read out when its kernel boots. */
 	mbox_writel(avp->msg, MBOX_TO_AVP);
 
+#if defined(CONFIG_TEGRA_AVP_KERNEL_ON_MMU) /* Tegra2 with AVP MMU */
+	/* paddr is any address returned from nvmap_pin */
+	/* vaddr is AVP_KERNEL_VIRT_BASE */
+	pr_info("%s: Using AVP MMU to relocate AVP kernel\n", __func__);
+	sprintf(fw_file, "nvrm_avp.bin");
+	avp->reset_addr = AVP_KERNEL_VIRT_BASE;
+#elif defined(CONFIG_TEGRA_AVP_KERNEL_ON_SMMU) /* Tegra3 with SMMU */
+	/* paddr is any address behind SMMU */
+	/* vaddr is TEGRA_SMMU_BASE */
+	pr_info("%s: Using SMMU at %lx to load AVP kernel\n",
+		__func__, (unsigned long)avp->kernel_phys);
+	BUG_ON(avp->kernel_phys != 0xe0000000
+		&& avp->kernel_phys != 0x00001000);
+	sprintf(fw_file, "nvrm_avp_%08lx.bin", (unsigned long)avp->kernel_phys);
+	avp->reset_addr = avp->kernel_phys;
+#else /* nvmem= carveout */
+	/* paddr is found in nvmem= carveout */
+	/* vaddr is same as paddr */
+	/* Find nvmem carveout */
+	if (!pfn_valid(__phys_to_pfn(0x8e000000))) {
+		avp->kernel_phys = 0x8e000000;
+	}
+	else if (!pfn_valid(__phys_to_pfn(0x9e000000))) {
+		avp->kernel_phys = 0x9e000000;
+	}
+	else if (!pfn_valid(__phys_to_pfn(0xbe000000))) {
+		avp->kernel_phys = 0xbe000000;
+	}
+	else {
+		pr_err("Cannot find nvmem= carveout to load AVP kernel\n");
+		pr_err("Check kernel command line "
+			"to see if nvmem= is defined\n");
+		BUG();
+	}
+	pr_info("%s: Using nvmem= carveout at %lx to load AVP kernel\n",
+		__func__, avp->kernel_phys);
+	sprintf(fw_file, "nvrm_avp_%08lx.bin", avp->kernel_phys);
+	avp->reset_addr = avp->kernel_phys;
+	avp->kernel_data = ioremap(avp->kernel_phys, SZ_1M);
+#endif
+
 	ret = request_firmware(&avp_fw, fw_file, avp->misc_dev.this_device);
 	if (ret) {
 		pr_err("%s: Cannot read firmware '%s'\n", __func__, fw_file);
 		goto err_req_fw;
 	}
-	pr_info("%s: read firmware from '%s' (%d bytes)\n", __func__,
+	pr_info("%s: Reading firmware from '%s' (%d bytes)\n", __func__,
 		fw_file, avp_fw->size);
+
+	pr_info("%s: Loading AVP kernel at vaddr=%p paddr=%lx\n",
+		__func__, avp->kernel_data, (unsigned long)(avp->kernel_phys));
 	memcpy(avp->kernel_data, avp_fw->data, avp_fw->size);
 	memset(avp->kernel_data + avp_fw->size, 0, SZ_1M - avp_fw->size);
+
 	wmb();
 	release_firmware(avp_fw);
 
-	ret = avp_reset(avp, AVP_KERNEL_VIRT_BASE);
+	ret = avp_reset(avp, avp->reset_addr);
 	if (ret) {
 		pr_err("%s: cannot reset the AVP.. aborting..\n", __func__);
 		goto err_reset;
@@ -967,7 +1031,7 @@ static int avp_init(struct avp_info *avp, const char *fw_file)
 		goto err_avp_svc_start;
 
 	ep = trpc_create_connect(avp->rpc_node, "RPC_AVP_PORT", NULL,
-				      NULL, -1);
+					  NULL, -1);
 	if (IS_ERR(ep)) {
 		pr_err("%s: can't connect to RPC_AVP_PORT server\n", __func__);
 		ret = PTR_ERR(ep);
@@ -993,7 +1057,7 @@ err_nvmap_create_libs_client:
 	return ret;
 }
 
-static void avp_uninit(struct avp_info *avp)
+static void avp_uninit(struct tegra_avp_info *avp)
 {
 	unsigned long flags;
 	struct rb_node *n;
@@ -1038,7 +1102,8 @@ static void avp_uninit(struct avp_info *avp)
 }
 
 /* returns the remote lib handle in lib->handle */
-static int _load_lib(struct avp_info *avp, struct tegra_avp_lib *lib)
+static int _load_lib(struct tegra_avp_info *avp, struct tegra_avp_lib *lib,
+		     bool from_user)
 {
 	struct svc_lib_attach svc;
 	struct svc_lib_attach_resp resp;
@@ -1046,7 +1111,7 @@ static int _load_lib(struct avp_info *avp, struct tegra_avp_lib *lib)
 	void *args;
 	struct nvmap_handle_ref *lib_handle;
 	void *lib_data;
-	unsigned long lib_phys;
+	phys_addr_t lib_phys;
 	int ret;
 
 	DBG(AVP_DBG_TRACE_LIB, "avp_lib: loading library '%s'\n", lib->name);
@@ -1057,7 +1122,10 @@ static int _load_lib(struct avp_info *avp, struct tegra_avp_lib *lib)
 			lib->args_len);
 		return -ENOMEM;
 	}
-	if (copy_from_user(args, lib->args, lib->args_len)) {
+
+	if (!from_user)
+		memcpy(args, lib->args, lib->args_len);
+	else if (copy_from_user(args, lib->args, lib->args_len)) {
 		pr_err("avp_lib: can't copy lib args\n");
 		ret = -EFAULT;
 		goto err_cp_args;
@@ -1071,7 +1139,7 @@ static int _load_lib(struct avp_info *avp, struct tegra_avp_lib *lib)
 
 	lib_handle = nvmap_alloc(avp->nvmap_libs, fw->size, L1_CACHE_BYTES,
 				 NVMAP_HANDLE_WRITE_COMBINE);
-	if (IS_ERR(lib_handle)) {
+	if (IS_ERR_OR_NULL(lib_handle)) {
 		pr_err("avp_lib: can't nvmap alloc for lib '%s'\n", lib->name);
 		ret = PTR_ERR(lib_handle);
 		goto err_nvmap_alloc;
@@ -1085,7 +1153,7 @@ static int _load_lib(struct avp_info *avp, struct tegra_avp_lib *lib)
 	}
 
 	lib_phys = nvmap_pin(avp->nvmap_libs, lib_handle);
-	if (IS_ERR((void *)lib_phys)) {
+	if (IS_ERR_OR_NULL((void *)lib_phys)) {
 		pr_err("avp_lib: can't nvmap pin for lib '%s'\n", lib->name);
 		ret = PTR_ERR(lib_handle);
 		goto err_nvmap_pin;
@@ -1115,15 +1183,15 @@ static int _load_lib(struct avp_info *avp, struct tegra_avp_lib *lib)
 		goto err_recv_msg;
 	} else if (resp.err) {
 		pr_err("avp_lib: got remote error (%d) while loading lib %s\n",
-		       resp.err, lib->name);
+			   resp.err, lib->name);
 		ret = -EPROTO;
 		goto err_recv_msg;
 	}
 	lib->handle = resp.lib_id;
 	ret = 0;
 	DBG(AVP_DBG_TRACE_LIB,
-	    "avp_lib: Successfully loaded library %s (lib_id=%x)\n",
-	    lib->name, resp.lib_id);
+		"avp_lib: Successfully loaded library %s (lib_id=%x)\n",
+		lib->name, resp.lib_id);
 
 	/* We free the memory here because by this point the AVP has already
 	 * requested memory for the library for all the sections since it does
@@ -1146,8 +1214,8 @@ err_cp_args:
 	return ret;
 }
 
-static int send_unload_lib_msg(struct avp_info *avp, u32 handle,
-			       const char *name)
+static int send_unload_lib_msg(struct tegra_avp_info *avp, u32 handle,
+				   const char *name)
 {
 	struct svc_lib_detach svc;
 	struct svc_lib_detach_resp resp;
@@ -1161,26 +1229,33 @@ static int send_unload_lib_msg(struct avp_info *avp, u32 handle,
 				 GFP_KERNEL);
 	if (ret) {
 		pr_err("avp_lib: can't send unload message to avp for '%s'\n",
-		       name);
+			   name);
 		goto err;
 	}
 
+	/* Give it a few extra moments to unload. */
+	msleep(20);
+
 	ret = trpc_recv_msg(avp->rpc_node, avp->avp_ep, &resp,
 				 sizeof(resp), -1);
 	if (ret != sizeof(resp)) {
 		pr_err("avp_lib: Couldn't get unload reply for '%s' (%d)\n",
-		       name, ret);
+			   name, ret);
 	} else if (resp.err) {
 		pr_err("avp_lib: remote error (%d) while unloading lib %s\n",
-		       resp.err, name);
+			   resp.err, name);
 		ret = -EPROTO;
-	} else
+	} else {
+		pr_info("avp_lib: Successfully unloaded '%s'\n",
+			   name);
 		ret = 0;
+	}
+
 err:
 	return ret;
 }
 
-static struct lib_item *_find_lib_locked(struct avp_info *avp, u32 handle)
+static struct lib_item *_find_lib_locked(struct tegra_avp_info *avp, u32 handle)
 {
 	struct lib_item *item;
 
@@ -1191,7 +1266,8 @@ static struct lib_item *_find_lib_locked(struct avp_info *avp, u32 handle)
 	return NULL;
 }
 
-static int _insert_lib_locked(struct avp_info *avp, u32 handle, char *name)
+static int _insert_lib_locked(struct tegra_avp_info *avp, u32 handle,
+			      char *name)
 {
 	struct lib_item *item;
 
@@ -1204,17 +1280,19 @@ static int _insert_lib_locked(struct avp_info *avp, u32 handle, char *name)
 	return 0;
 }
 
-static void _delete_lib_locked(struct avp_info *avp, struct lib_item *item)
+static void _delete_lib_locked(struct tegra_avp_info *avp,
+			       struct lib_item *item)
 {
 	list_del(&item->list);
 	kfree(item);
 }
 
-static int handle_load_lib_ioctl(struct avp_info *avp, unsigned long arg)
+static int handle_load_lib_ioctl(struct tegra_avp_info *avp, unsigned long arg)
 {
 	struct tegra_avp_lib lib;
 	int ret;
 
+	pr_debug("%s: ioctl\n", __func__);
 	if (copy_from_user(&lib, (void __user *)arg, sizeof(lib)))
 		return -EFAULT;
 	lib.name[TEGRA_AVP_LIB_MAX_NAME - 1] = '\0';
@@ -1226,7 +1304,7 @@ static int handle_load_lib_ioctl(struct avp_info *avp, unsigned long arg)
 	}
 
 	mutex_lock(&avp->libs_lock);
-	ret = _load_lib(avp, &lib);
+	ret = _load_lib(avp, &lib, true);
 	if (ret)
 		goto err_load_lib;
 
@@ -1253,33 +1331,7 @@ err_load_lib:
 	return ret;
 }
 
-static int handle_unload_lib_ioctl(struct avp_info *avp, unsigned long arg)
-{
-	struct lib_item *item;
-	int ret;
-
-	mutex_lock(&avp->libs_lock);
-	item = _find_lib_locked(avp, (u32)arg);
-	if (!item) {
-		pr_err("avp_lib: avp lib with handle 0x%x not found\n",
-		       (u32)arg);
-		ret = -ENOENT;
-		goto err_find;
-	}
-	ret = send_unload_lib_msg(avp, item->handle, item->name);
-	if (!ret)
-		DBG(AVP_DBG_TRACE_LIB, "avp_lib: unloaded '%s'\n", item->name);
-	else
-		pr_err("avp_lib: can't unload lib '%s'/0x%x (%d)\n", item->name,
-		       item->handle, ret);
-	_delete_lib_locked(avp, item);
-
-err_find:
-	mutex_unlock(&avp->libs_lock);
-	return ret;
-}
-
-static void libs_cleanup(struct avp_info *avp)
+static void libs_cleanup(struct tegra_avp_info *avp)
 {
 	struct lib_item *lib;
 	struct lib_item *lib_tmp;
@@ -1295,14 +1347,14 @@ static void libs_cleanup(struct avp_info *avp)
 }
 
 static long tegra_avp_ioctl(struct file *file, unsigned int cmd,
-			    unsigned long arg)
+				unsigned long arg)
 {
-	struct avp_info *avp = tegra_avp;
+	struct tegra_avp_info *avp = tegra_avp;
 	int ret;
 
 	if (_IOC_TYPE(cmd) != TEGRA_AVP_IOCTL_MAGIC ||
-	    _IOC_NR(cmd) < TEGRA_AVP_IOCTL_MIN_NR ||
-	    _IOC_NR(cmd) > TEGRA_AVP_IOCTL_MAX_NR)
+		_IOC_NR(cmd) < TEGRA_AVP_IOCTL_MIN_NR ||
+		_IOC_NR(cmd) > TEGRA_AVP_IOCTL_MAX_NR)
 		return -ENOTTY;
 
 	switch (cmd) {
@@ -1310,7 +1362,7 @@ static long tegra_avp_ioctl(struct file *file, unsigned int cmd,
 		ret = handle_load_lib_ioctl(avp, arg);
 		break;
 	case TEGRA_AVP_IOCTL_UNLOAD_LIB:
-		ret = handle_unload_lib_ioctl(avp, arg);
+		ret = tegra_avp_unload_lib(avp, arg);
 		break;
 	default:
 		pr_err("avp_lib: Unknown tegra_avp ioctl 0x%x\n", _IOC_NR(cmd));
@@ -1320,50 +1372,70 @@ static long tegra_avp_ioctl(struct file *file, unsigned int cmd,
 	return ret;
 }
 
-static int tegra_avp_open(struct inode *inode, struct file *file)
+int tegra_avp_open(struct tegra_avp_info **avp)
 {
-	struct avp_info *avp = tegra_avp;
+	struct tegra_avp_info *new_avp = tegra_avp;
 	int ret = 0;
 
-	nonseekable_open(inode, file);
+	pr_debug("%s: open\n", __func__);
+	mutex_lock(&new_avp->open_lock);
 
-	mutex_lock(&avp->open_lock);
-	/* only one userspace client at a time */
-	if (avp->opened) {
-		pr_err("%s: already have client, aborting\n", __func__);
-		ret = -EBUSY;
+	if (!new_avp->refcount)
+		ret = avp_init(new_avp);
+
+	if (ret < 0) {
+		mutex_unlock(&new_avp->open_lock);
+		new_avp = 0;
 		goto out;
 	}
 
-	ret = avp_init(avp, TEGRA_AVP_KERNEL_FW);
-	avp->opened = !ret;
+	new_avp->refcount++;
+
+	mutex_unlock(&new_avp->open_lock);
 out:
-	mutex_unlock(&avp->open_lock);
+	*avp = new_avp;
 	return ret;
 }
 
-static int tegra_avp_release(struct inode *inode, struct file *file)
+static int tegra_avp_open_fops(struct inode *inode, struct file *file)
+{
+	struct tegra_avp_info *avp;
+
+	nonseekable_open(inode, file);
+	return tegra_avp_open(&avp);
+}
+
+int tegra_avp_release(struct tegra_avp_info *avp)
 {
-	struct avp_info *avp = tegra_avp;
 	int ret = 0;
 
-	pr_info("%s: release\n", __func__);
+	pr_info("%s: WORKAROUND: ignoring AVP release\n", __func__);
+	return 0;
+
+	pr_debug("%s: close\n", __func__);
 	mutex_lock(&avp->open_lock);
-	if (!avp->opened) {
+	if (!avp->refcount) {
 		pr_err("%s: releasing while in invalid state\n", __func__);
 		ret = -EINVAL;
 		goto out;
 	}
+	if (avp->refcount > 0)
+		avp->refcount--;
+	if (!avp->refcount)
+		avp_uninit(avp);
 
-	avp_uninit(avp);
-
-	avp->opened = false;
 out:
 	mutex_unlock(&avp->open_lock);
 	return ret;
 }
 
-static int avp_enter_lp0(struct avp_info *avp)
+static int tegra_avp_release_fops(struct inode *inode, struct file *file)
+{
+	struct tegra_avp_info *avp = tegra_avp;
+	return tegra_avp_release(avp);
+}
+
+static int avp_enter_lp0(struct tegra_avp_info *avp)
 {
 	volatile u32 *avp_suspend_done =
 		avp->iram_backup_data + TEGRA_IRAM_SIZE;
@@ -1408,7 +1480,7 @@ err:
 
 static int tegra_avp_suspend(struct platform_device *pdev, pm_message_t state)
 {
-	struct avp_info *avp = tegra_avp;
+	struct tegra_avp_info *avp = tegra_avp;
 	unsigned long flags;
 	int ret;
 
@@ -1450,7 +1522,7 @@ err:
 
 static int tegra_avp_resume(struct platform_device *pdev)
 {
-	struct avp_info *avp = tegra_avp;
+	struct tegra_avp_info *avp = tegra_avp;
 	int ret = 0;
 
 	pr_info("%s()+\n", __func__);
@@ -1474,9 +1546,9 @@ out:
 
 static const struct file_operations tegra_avp_fops = {
 	.owner		= THIS_MODULE,
-	.open		= tegra_avp_open,
-	.release	= tegra_avp_release,
-	.unlocked_ioctl	= tegra_avp_ioctl,
+	.open		= tegra_avp_open_fops,
+	.release	= tegra_avp_release_fops,
+	.unlocked_ioctl = tegra_avp_ioctl,
 };
 
 static struct trpc_node avp_trpc_node = {
@@ -1485,12 +1557,51 @@ static struct trpc_node avp_trpc_node = {
 	.try_connect	= avp_node_try_connect,
 };
 
+static struct nvmap_client *avp_early_nvmap_drv;
+static struct nvmap_handle_ref *avp_early_kernel_handle;
+static void *avp_early_kernel_data;
+static phys_addr_t avp_early_kernel_phys;
+
+void avp_early_init(void)
+{
+	int ret;
+
+	avp_early_nvmap_drv = nvmap_create_client(nvmap_dev, "avp_early");
+	if (IS_ERR_OR_NULL(avp_early_nvmap_drv))
+		pr_crit("%s: nvmap_create_client error\n", __func__);
+
+	avp_early_kernel_handle =
+		nvmap_create_handle(avp_early_nvmap_drv, SZ_1M);
+	if (IS_ERR_OR_NULL(avp_early_kernel_handle))
+		pr_crit("%s: nvmap_create_handle error\n", __func__);
+
+	ret = nvmap_alloc_handle_id(avp_early_nvmap_drv,
+				nvmap_ref_to_id(avp_early_kernel_handle),
+				NVMAP_HEAP_IOVMM, PAGE_SIZE,
+				NVMAP_HANDLE_WRITE_COMBINE);
+	if (ret)
+		pr_crit("%s: nvmap_alloc_handle_id error\n", __func__);
+
+	avp_early_kernel_data = nvmap_mmap(avp_early_kernel_handle);
+	if (!avp_early_kernel_data)
+		pr_crit("%s: nvmap_mmap error\n", __func__);
+
+	avp_early_kernel_phys =
+		nvmap_pin(avp_early_nvmap_drv, avp_early_kernel_handle);
+	if (IS_ERR_OR_NULL((void *)avp_early_kernel_phys))
+		pr_crit("%s: nvmap_pin error\n", __func__);
+
+	pr_info("%s: allocated memory at %x for AVP kernel\n",
+		__func__, avp_early_kernel_phys);
+}
+
 static int tegra_avp_probe(struct platform_device *pdev)
 {
 	void *msg_area;
-	struct avp_info *avp;
+	struct tegra_avp_info *avp;
 	int ret = 0;
 	int irq;
+	unsigned int heap_mask;
 
 	irq = platform_get_irq_byname(pdev, "mbox_from_avp_pending");
 	if (irq < 0) {
@@ -1498,39 +1609,60 @@ static int tegra_avp_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	avp = kzalloc(sizeof(struct avp_info), GFP_KERNEL);
+	avp = kzalloc(sizeof(struct tegra_avp_info), GFP_KERNEL);
 	if (!avp) {
-		pr_err("%s: cannot allocate avp_info\n", __func__);
+		pr_err("%s: cannot allocate tegra_avp_info\n", __func__);
 		return -ENOMEM;
 	}
 
 	avp->nvmap_drv = nvmap_create_client(nvmap_dev, "avp_core");
-	if (IS_ERR(avp->nvmap_drv)) {
+	if (IS_ERR_OR_NULL(avp->nvmap_drv)) {
 		pr_err("%s: cannot create drv nvmap client\n", __func__);
 		ret = PTR_ERR(avp->nvmap_drv);
 		goto err_nvmap_create_drv_client;
 	}
 
-	avp->kernel_handle = nvmap_alloc(avp->nvmap_drv, SZ_1M, SZ_1M,
-					 NVMAP_HANDLE_WRITE_COMBINE);
-	if (IS_ERR(avp->kernel_handle)) {
-		pr_err("%s: cannot create handle\n", __func__);
-		ret = PTR_ERR(avp->kernel_handle);
-		goto err_nvmap_alloc;
-	}
+#if defined(CONFIG_TEGRA_AVP_KERNEL_ON_MMU) /* Tegra2 with AVP MMU */
+	heap_mask = NVMAP_HEAP_CARVEOUT_GENERIC;
+#elif defined(CONFIG_TEGRA_AVP_KERNEL_ON_SMMU) /* Tegra3 with SMMU */
+	heap_mask = NVMAP_HEAP_IOVMM;
+#else /* nvmem= carveout */
+	heap_mask = 0;
+#endif
+
+	if (heap_mask == NVMAP_HEAP_IOVMM) {
+		avp->nvmap_drv = avp_early_nvmap_drv;
+		avp->kernel_handle = avp_early_kernel_handle;
+		avp->kernel_data = avp_early_kernel_data;
+		avp->kernel_phys = avp_early_kernel_phys;
+	}
+
+	if (heap_mask == NVMAP_HEAP_CARVEOUT_GENERIC) {
+		avp->kernel_handle = nvmap_alloc(avp->nvmap_drv, SZ_1M, SZ_1M,
+						NVMAP_HANDLE_WRITE_COMBINE);
+		if (IS_ERR_OR_NULL(avp->kernel_handle)) {
+			pr_err("%s: cannot create handle\n", __func__);
+			ret = PTR_ERR(avp->kernel_handle);
+			goto err_nvmap_alloc;
+		}
 
-	avp->kernel_data = nvmap_mmap(avp->kernel_handle);
-	if (!avp->kernel_data) {
-		pr_err("%s: cannot map kernel handle\n", __func__);
-		ret = -ENOMEM;
-		goto err_nvmap_mmap;
-	}
+		avp->kernel_data = nvmap_mmap(avp->kernel_handle);
+		if (!avp->kernel_data) {
+			pr_err("%s: cannot map kernel handle\n", __func__);
+			ret = -ENOMEM;
+			goto err_nvmap_mmap;
+		}
 
-	avp->kernel_phys = nvmap_pin(avp->nvmap_drv, avp->kernel_handle);
-	if (IS_ERR((void *)avp->kernel_phys)) {
-		pr_err("%s: cannot pin kernel handle\n", __func__);
-		ret = PTR_ERR((void *)avp->kernel_phys);
-		goto err_nvmap_pin;
+		avp->kernel_phys = nvmap_pin(avp->nvmap_drv,
+					avp->kernel_handle);
+		if (IS_ERR_OR_NULL((void *)avp->kernel_phys)) {
+			pr_err("%s: cannot pin kernel handle\n", __func__);
+			ret = PTR_ERR((void *)avp->kernel_phys);
+			goto err_nvmap_pin;
+		}
+
+		pr_info("%s: allocated carveout memory at %lx for AVP kernel\n",
+			__func__, (unsigned long)avp->kernel_phys);
 	}
 
 	/* allocate an extra 4 bytes at the end which AVP uses to signal to
@@ -1538,8 +1670,8 @@ static int tegra_avp_probe(struct platform_device *pdev)
 	 */
 	avp->iram_backup_handle =
 		nvmap_alloc(avp->nvmap_drv, TEGRA_IRAM_SIZE + 4,
-			    L1_CACHE_BYTES, NVMAP_HANDLE_WRITE_COMBINE);
-	if (IS_ERR(avp->iram_backup_handle)) {
+				L1_CACHE_BYTES, NVMAP_HANDLE_WRITE_COMBINE);
+	if (IS_ERR_OR_NULL(avp->iram_backup_handle)) {
 		pr_err("%s: cannot create handle for iram backup\n", __func__);
 		ret = PTR_ERR(avp->iram_backup_handle);
 		goto err_iram_nvmap_alloc;
@@ -1552,7 +1684,7 @@ static int tegra_avp_probe(struct platform_device *pdev)
 	}
 	avp->iram_backup_phys = nvmap_pin(avp->nvmap_drv,
 					  avp->iram_backup_handle);
-	if (IS_ERR((void *)avp->iram_backup_phys)) {
+	if (IS_ERR_OR_NULL((void *)avp->iram_backup_phys)) {
 		pr_err("%s: cannot pin iram backup handle\n", __func__);
 		ret = PTR_ERR((void *)avp->iram_backup_phys);
 		goto err_iram_nvmap_pin;
@@ -1570,7 +1702,7 @@ static int tegra_avp_probe(struct platform_device *pdev)
 	INIT_LIST_HEAD(&avp->libs);
 
 	avp->recv_wq = alloc_workqueue("avp-msg-recv",
-				       WQ_NON_REENTRANT | WQ_HIGHPRI, 1);
+					   WQ_NON_REENTRANT | WQ_HIGHPRI, 1);
 	if (!avp->recv_wq) {
 		pr_err("%s: can't create recve workqueue\n", __func__);
 		ret = -ENOMEM;
@@ -1578,14 +1710,14 @@ static int tegra_avp_probe(struct platform_device *pdev)
 	}
 
 	avp->cop_clk = clk_get(&pdev->dev, "cop");
-	if (IS_ERR(avp->cop_clk)) {
+	if (IS_ERR_OR_NULL(avp->cop_clk)) {
 		pr_err("%s: Couldn't get cop clock\n", TEGRA_AVP_NAME);
 		ret = -ENOENT;
 		goto err_get_cop_clk;
 	}
 
 	msg_area = dma_alloc_coherent(&pdev->dev, AVP_MSG_AREA_SIZE * 2,
-				      &avp->msg_area_addr, GFP_KERNEL);
+					  &avp->msg_area_addr, GFP_KERNEL);
 	if (!msg_area) {
 		pr_err("%s: cannot allocate msg_area\n", __func__);
 		ret = -ENOMEM;
@@ -1608,7 +1740,7 @@ static int tegra_avp_probe(struct platform_device *pdev)
 	avp->rpc_node = &avp_trpc_node;
 
 	avp->avp_svc = avp_svc_init(pdev, avp->rpc_node);
-	if (IS_ERR(avp->avp_svc)) {
+	if (IS_ERR_OR_NULL(avp->avp_svc)) {
 		pr_err("%s: Cannot initialize avp_svc\n", __func__);
 		ret = PTR_ERR(avp->avp_svc);
 		goto err_avp_svc_init;
@@ -1633,8 +1765,7 @@ static int tegra_avp_probe(struct platform_device *pdev)
 
 	tegra_avp = avp;
 
-	pr_info("%s: driver registered, kernel %lx(%p), msg area %lx/%lx\n",
-		__func__, avp->kernel_phys, avp->kernel_data,
+	pr_info("%s: message area %lx/%lx\n", __func__,
 		(unsigned long)avp->msg_area_addr,
 		(unsigned long)avp->msg_area_addr + AVP_MSG_AREA_SIZE);
 
@@ -1675,18 +1806,17 @@ err_nvmap_create_drv_client:
 
 static int tegra_avp_remove(struct platform_device *pdev)
 {
-	struct avp_info *avp = tegra_avp;
+	struct tegra_avp_info *avp = tegra_avp;
 
 	if (!avp)
 		return 0;
 
 	mutex_lock(&avp->open_lock);
-	if (avp->opened) {
+	/* ensure that noone can open while we tear down */
+	if (avp->refcount) {
 		mutex_unlock(&avp->open_lock);
 		return -EBUSY;
 	}
-	/* ensure that noone can open while we tear down */
-	avp->opened = true;
 	mutex_unlock(&avp->open_lock);
 
 	misc_deregister(&avp->misc_dev);
@@ -1711,12 +1841,85 @@ static int tegra_avp_remove(struct platform_device *pdev)
 	return 0;
 }
 
+int tegra_avp_load_lib(struct tegra_avp_info *avp, struct tegra_avp_lib *lib)
+{
+	int ret;
+
+	if (!avp)
+		return -ENODEV;
+
+	if (!lib)
+		return -EFAULT;
+
+	lib->name[TEGRA_AVP_LIB_MAX_NAME - 1] = '\0';
+
+	if (lib->args_len > TEGRA_AVP_LIB_MAX_ARGS) {
+		pr_err("%s: library args too long (%d)\n", __func__,
+			lib->args_len);
+		return -E2BIG;
+	}
+
+	mutex_lock(&avp->libs_lock);
+	ret = _load_lib(avp, lib, false);
+	if (ret)
+		goto err_load_lib;
+
+	ret = _insert_lib_locked(avp, lib->handle, lib->name);
+	if (ret) {
+		pr_err("%s: can't insert lib (%d)\n", __func__, ret);
+		goto err_insert_lib;
+	}
+
+	mutex_unlock(&avp->libs_lock);
+	return 0;
+
+err_insert_lib:
+	ret = send_unload_lib_msg(avp, lib->handle, lib->name);
+	if (!ret)
+		DBG(AVP_DBG_TRACE_LIB, "avp_lib: unloaded '%s'\n", lib->name);
+	else
+		pr_err("avp_lib: can't unload lib '%s' (%d)\n", lib->name, ret);
+	lib->handle = 0;
+err_load_lib:
+	mutex_unlock(&avp->libs_lock);
+	return ret;
+}
+
+int tegra_avp_unload_lib(struct tegra_avp_info *avp, unsigned long handle)
+{
+	struct lib_item *item;
+	int ret;
+
+	if (!avp)
+		return -ENODEV;
+
+	mutex_lock(&avp->libs_lock);
+	item = _find_lib_locked(avp, handle);
+	if (!item) {
+		pr_err("avp_lib: avp lib with handle 0x%x not found\n",
+			   (u32)handle);
+		ret = -ENOENT;
+		goto err_find;
+	}
+	ret = send_unload_lib_msg(avp, item->handle, item->name);
+	if (!ret)
+		DBG(AVP_DBG_TRACE_LIB, "avp_lib: unloaded '%s'\n", item->name);
+	else
+		pr_err("avp_lib: can't unload lib '%s'/0x%x (%d)\n", item->name,
+			   item->handle, ret);
+	_delete_lib_locked(avp, item);
+
+err_find:
+	mutex_unlock(&avp->libs_lock);
+	return ret;
+}
+
 static struct platform_driver tegra_avp_driver = {
 	.probe		= tegra_avp_probe,
 	.remove		= tegra_avp_remove,
 	.suspend	= tegra_avp_suspend,
 	.resume		= tegra_avp_resume,
-	.driver	= {
+	.driver = {
 		.name	= TEGRA_AVP_NAME,
 		.owner	= THIS_MODULE,
 	},
diff --git a/drivers/media/video/tegra/avp/avp_svc.c b/drivers/media/video/tegra/avp/avp_svc.c
index 2eed2891e556..385ac5eadacf 100644
--- a/drivers/media/video/tegra/avp/avp_svc.c
+++ b/drivers/media/video/tegra/avp/avp_svc.c
@@ -2,6 +2,8 @@
  * Copyright (C) 2010 Google, Inc.
  * Author: Dima Zavin <dima@android.com>
  *
+ * Copyright (C) 2010-2011 NVIDIA Corporation
+ *
  * This software is licensed under the terms of the GNU General Public
  * License version 2, as published by the Free Software Foundation, and
  * may be copied, distributed, and modified under those terms.
@@ -38,7 +40,7 @@ enum {
 	AVP_DBG_TRACE_SVC		= 1U << 0,
 };
 
-static u32 debug_mask = 0;
+static u32 debug_mask;
 module_param_named(debug_mask, debug_mask, uint, S_IWUSR | S_IRUGO);
 
 #define DBG(flag, args...) \
@@ -200,7 +202,7 @@ static void do_svc_nvmap_pin(struct avp_svc_info *avp_svc,
 	struct svc_nvmap_pin *msg = (struct svc_nvmap_pin *)_msg;
 	struct svc_nvmap_pin_resp resp;
 	struct nvmap_handle_ref *handle;
-	unsigned long addr = ~0UL;
+	phys_addr_t addr = ~0UL;
 	unsigned long id = msg->handle_id;
 	int err;
 
@@ -686,6 +688,7 @@ struct avp_svc_info *avp_svc_init(struct platform_device *pdev,
 		ret = -ENOENT;
 		goto err_get_clks;
 	}
+	clk_set_rate(avp_svc->sclk, ULONG_MAX);
 
 	avp_svc->emcclk = clk_get(&pdev->dev, "emc");
 	if (IS_ERR(avp_svc->emcclk)) {
diff --git a/drivers/media/video/tegra/avp/headavp.S b/drivers/media/video/tegra/avp/headavp.S
index 5304067f0d83..c1f8e9fea1cb 100644
--- a/drivers/media/video/tegra/avp/headavp.S
+++ b/drivers/media/video/tegra/avp/headavp.S
@@ -48,12 +48,14 @@
 ENTRY(_tegra_avp_boot_stub)
 	adr	r4, _tegra_avp_boot_stub_data
 	ldmia	r4, {r0-r3}
+#ifdef CONFIG_TEGRA_AVP_KERNEL_ON_MMU
 	str	r2, [r0, #PTE0_COMPARE]
 	bic	r3, r3, #0xff0
 	bic	r3, r3, #0x00f
 	orr	r3, r3, #TRANSLATE_OPT
 	orr	r3, r3, #TRANSLATE_EN
 	str	r3, [r0, #PTE0_TRANSLATE]
+#endif
 	bx	r1
 	b	.
 ENDPROC(_tegra_avp_boot_stub)
diff --git a/drivers/media/video/tegra/avp/nvavp.h b/drivers/media/video/tegra/avp/nvavp.h
new file mode 100644
index 000000000000..dbc62b485882
--- /dev/null
+++ b/drivers/media/video/tegra/avp/nvavp.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2011 Nvidia Corp
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef __MEDIA_VIDEO_TEGRA_NVAVP_H
+#define __MEDIA_VIDEO_TEGRA_NVAVP_H
+
+#include <linux/tegra_avp.h>
+
+struct tegra_avp_info;
+
+int tegra_avp_open(struct tegra_avp_info **avp);
+int tegra_avp_release(struct tegra_avp_info *avp);
+int tegra_avp_load_lib(struct tegra_avp_info *avp, struct tegra_avp_lib *lib);
+int tegra_avp_unload_lib(struct tegra_avp_info *avp, unsigned long handle);
+
+
+#include <linux/tegra_sema.h>
+
+struct tegra_sema_info;
+
+int tegra_sema_open(struct tegra_sema_info **sema);
+int tegra_sema_release(struct tegra_sema_info *sema);
+int tegra_sema_wait(struct tegra_sema_info *sema, long* timeout);
+int tegra_sema_signal(struct tegra_sema_info *sema);
+
+
+#include <linux/tegra_rpc.h>
+
+struct tegra_rpc_info;
+
+int tegra_rpc_open(struct tegra_rpc_info **rpc);
+int tegra_rpc_release(struct tegra_rpc_info *rpc);
+int tegra_rpc_port_create(struct tegra_rpc_info *rpc, char *name,
+			  struct tegra_sema_info *sema);
+int tegra_rpc_get_name(struct tegra_rpc_info *rpc, char* name);
+int tegra_rpc_port_connect(struct tegra_rpc_info *rpc, long timeout);
+int tegra_rpc_port_listen(struct tegra_rpc_info *rpc, long timeout);
+int tegra_rpc_write(struct tegra_rpc_info *rpc, u8* buf, size_t size);
+int tegra_rpc_read(struct tegra_rpc_info *rpc, u8 *buf, size_t max);
+
+
+#endif
diff --git a/drivers/media/video/tegra/avp/tegra_rpc.c b/drivers/media/video/tegra/avp/tegra_rpc.c
index 6110d0bd066c..a0fd1dc999f4 100644
--- a/drivers/media/video/tegra/avp/tegra_rpc.c
+++ b/drivers/media/video/tegra/avp/tegra_rpc.c
@@ -70,7 +70,7 @@ enum {
 	TRPC_TRACE_PORT	= 1U << 2,
 };
 
-static u32 trpc_debug_mask = 0;
+static u32 trpc_debug_mask;
 module_param_named(debug_mask, trpc_debug_mask, uint, S_IWUSR | S_IRUGO);
 
 #define DBG(flag, args...) \
@@ -724,7 +724,7 @@ static int trpc_debug_ports_show(struct seq_file *s, void *data)
 		for (i = 0; i < ARRAY_SIZE(port->peers); i++) {
 			struct trpc_endpoint *ep = &port->peers[i];
 			seq_printf(s, "  peer%d: %s\n    ready:%s\n", i,
-				   ep->owner ? ep->owner->name: "<none>",
+				   ep->owner ? ep->owner->name : "<none>",
 				   ep->ready ? "yes" : "no");
 			if (ep->ops && ep->ops->show)
 				ep->ops->show(s, ep);
@@ -741,7 +741,7 @@ static int trpc_debug_ports_open(struct inode *inode, struct file *file)
 	return single_open(file, trpc_debug_ports_show, inode->i_private);
 }
 
-static struct file_operations trpc_debug_ports_fops = {
+static const struct file_operations trpc_debug_ports_fops = {
 	.open = trpc_debug_ports_open,
 	.read = seq_read,
 	.llseek = seq_lseek,
diff --git a/drivers/media/video/tegra/avp/trpc_local.c b/drivers/media/video/tegra/avp/trpc_local.c
index 5a941a78fc40..77692e094385 100644
--- a/drivers/media/video/tegra/avp/trpc_local.c
+++ b/drivers/media/video/tegra/avp/trpc_local.c
@@ -33,10 +33,11 @@
 
 #include "trpc.h"
 #include "trpc_sema.h"
+#include "nvavp.h"
 
-struct rpc_info {
+struct tegra_rpc_info {
 	struct trpc_endpoint	*rpc_ep;
-	struct file		*sema_file;
+	struct tegra_sema_info  *sema;
 };
 
 /* ports names reserved for system functions, i.e. communicating with the
@@ -55,26 +56,39 @@ static struct trpc_ep_ops ep_ops = {
 };
 
 static struct trpc_node rpc_node = {
-	.name	= "local",
-	.type	= TRPC_NODE_LOCAL,
+	.name   = "local",
+	.type   = TRPC_NODE_LOCAL,
 };
 
 static void rpc_notify_recv(struct trpc_endpoint *ep)
 {
-	struct rpc_info *info = trpc_priv(ep);
+	struct tegra_rpc_info *info = trpc_priv(ep);
 
 	if (WARN_ON(!info))
 		return;
-	if (info->sema_file)
-		trpc_sema_signal(info->sema_file);
+	if (info->sema)
+		tegra_sema_signal(info->sema);
+}
+
+int tegra_rpc_open(struct tegra_rpc_info **info)
+{
+	struct tegra_rpc_info *new_info;
+
+	new_info = kzalloc(sizeof(struct tegra_rpc_info), GFP_KERNEL);
+	if (!new_info)
+		return -ENOMEM;
+
+	*info = new_info;
+	return 0;
 }
 
 static int local_rpc_open(struct inode *inode, struct file *file)
 {
-	struct rpc_info *info;
+	struct tegra_rpc_info *info;
+	int ret = 0;
 
-	info = kzalloc(sizeof(struct rpc_info), GFP_KERNEL);
-	if (!info)
+	ret = tegra_rpc_open(&info);
+	if (ret < 0)
 		return -ENOMEM;
 
 	nonseekable_open(inode, file);
@@ -82,30 +96,23 @@ static int local_rpc_open(struct inode *inode, struct file *file)
 	return 0;
 }
 
-static int local_rpc_release(struct inode *inode, struct file *file)
+int tegra_rpc_release(struct tegra_rpc_info *info)
 {
-	struct rpc_info *info = file->private_data;
-
 	if (info->rpc_ep)
 		trpc_close(info->rpc_ep);
-	if (info->sema_file)
-		fput(info->sema_file);
+	if (info->sema)
+		trpc_sema_put(info->sema);
 	kfree(info);
-	file->private_data = NULL;
 	return 0;
 }
+EXPORT_SYMBOL(tegra_rpc_release);
 
-static int __get_port_desc(struct tegra_rpc_port_desc *desc,
-			   unsigned int cmd, unsigned long arg)
+static int local_rpc_release(struct inode *inode, struct file *file)
 {
-	unsigned int size = _IOC_SIZE(cmd);
-
-	if (size != sizeof(struct tegra_rpc_port_desc))
-		return -EINVAL;
-	if (copy_from_user(desc, (void __user *)arg, sizeof(*desc)))
-		return -EFAULT;
+	struct tegra_rpc_info *info = file->private_data;
 
-	desc->name[TEGRA_RPC_MAX_NAME_LEN - 1] = '\0';
+	tegra_rpc_release(info);
+	file->private_data = NULL;
 	return 0;
 }
 
@@ -138,55 +145,98 @@ static int _validate_port_name(const char *name)
 	return 0;
 }
 
+int tegra_rpc_port_create(struct tegra_rpc_info *info, char *name,
+			  struct tegra_sema_info *sema)
+{
+	struct trpc_endpoint *ep;
+	int ret = 0;
+
+	if (info->rpc_ep) {
+		ret = -EINVAL;
+		goto err;
+	}
+
+	name[TEGRA_RPC_MAX_NAME_LEN - 1] = '\0';
+	if (name[0]) {
+		ret = _validate_port_name(name);
+		if (ret)
+			goto err;
+	} else {
+		_gen_port_name(name);
+	}
+	ep = trpc_create(&rpc_node, name, &ep_ops, info);
+	if (IS_ERR(ep)) {
+		ret = PTR_ERR(ep);
+		goto err;
+	}
+	info->rpc_ep = ep;
+	info->sema = sema;
+	return 0;
+
+err:
+	return ret;
+}
+
+int tegra_rpc_get_name(struct tegra_rpc_info *info, char* name)
+{
+	if (!info->rpc_ep)
+		return -EINVAL;
+
+	strcpy(name, trpc_name(info->rpc_ep));
+	return 0;
+}
+
+int tegra_rpc_port_connect(struct tegra_rpc_info *info, long timeout)
+{
+	if (!info->rpc_ep)
+		return -EINVAL;
+
+	return trpc_connect(info->rpc_ep, timeout);
+
+}
+
+int tegra_rpc_port_listen(struct tegra_rpc_info *info, long timeout)
+{
+	if (!info->rpc_ep)
+		return -EINVAL;
+
+	return trpc_wait_peer(info->rpc_ep, timeout);
+}
+
 static long local_rpc_ioctl(struct file *file, unsigned int cmd,
-			    unsigned long arg)
+				unsigned long arg)
 {
-	struct rpc_info *info = file->private_data;
+	struct tegra_rpc_info *info = file->private_data;
 	struct tegra_rpc_port_desc desc;
-	struct trpc_endpoint *ep;
+	struct tegra_sema_info *sema = NULL;
 	int ret = 0;
 
 	if (_IOC_TYPE(cmd) != TEGRA_RPC_IOCTL_MAGIC ||
-	    _IOC_NR(cmd) < TEGRA_RPC_IOCTL_MIN_NR ||
-	    _IOC_NR(cmd) > TEGRA_RPC_IOCTL_MAX_NR) {
+		_IOC_NR(cmd) < TEGRA_RPC_IOCTL_MIN_NR ||
+		_IOC_NR(cmd) > TEGRA_RPC_IOCTL_MAX_NR) {
 		ret = -ENOTTY;
 		goto err;
 	}
 
 	switch (cmd) {
 	case TEGRA_RPC_IOCTL_PORT_CREATE:
-		if (info->rpc_ep) {
-			ret = -EINVAL;
-			goto err;
-		}
-		ret = __get_port_desc(&desc, cmd, arg);
-		if (ret)
-			goto err;
-		if (desc.name[0]) {
-			ret = _validate_port_name(desc.name);
-			if (ret)
-				goto err;
-		} else {
-			_gen_port_name(desc.name);
-		}
+
+		if (_IOC_SIZE(cmd) != sizeof(struct tegra_rpc_port_desc))
+			return -EINVAL;
+		if (copy_from_user(&desc, (void __user *)arg, sizeof(desc)))
+			return -EFAULT;
 		if (desc.notify_fd != -1) {
-			/* grab a reference to the trpc_sema fd */
-			info->sema_file = trpc_sema_get_from_fd(desc.notify_fd);
-			if (IS_ERR(info->sema_file)) {
-				ret = PTR_ERR(info->sema_file);
-				info->sema_file = NULL;
+			sema = trpc_sema_get_from_fd(desc.notify_fd);
+			if (IS_ERR(sema)) {
+				ret = PTR_ERR(sema);
 				goto err;
 			}
 		}
-		ep = trpc_create(&rpc_node, desc.name, &ep_ops, info);
-		if (IS_ERR(ep)) {
-			ret = PTR_ERR(ep);
-			if (info->sema_file)
-				fput(info->sema_file);
-			info->sema_file = NULL;
+
+		ret = tegra_rpc_port_create(info, desc.name, sema);
+		if (ret < 0)
 			goto err;
-		}
-		info->rpc_ep = ep;
+
 		break;
 	case TEGRA_RPC_IOCTL_PORT_GET_NAME:
 		if (!info->rpc_ep) {
@@ -208,7 +258,7 @@ static long local_rpc_ioctl(struct file *file, unsigned int cmd,
 		ret = trpc_connect(info->rpc_ep, (long)arg);
 		if (ret) {
 			pr_err("%s: can't connect to '%s' (%d)\n", __func__,
-			       trpc_name(info->rpc_ep), ret);
+				   trpc_name(info->rpc_ep), ret);
 			goto err;
 		}
 		break;
@@ -220,7 +270,7 @@ static long local_rpc_ioctl(struct file *file, unsigned int cmd,
 		ret = trpc_wait_peer(info->rpc_ep, (long)arg);
 		if (ret) {
 			pr_err("%s: error waiting for peer for '%s' (%d)\n",
-			       __func__, trpc_name(info->rpc_ep), ret);
+				   __func__, trpc_name(info->rpc_ep), ret);
 			goto err;
 		}
 		break;
@@ -235,14 +285,31 @@ static long local_rpc_ioctl(struct file *file, unsigned int cmd,
 err:
 	if (ret && ret != -ERESTARTSYS)
 		pr_err("tegra_rpc: pid=%d ioctl=%x/%lx (%x) ret=%d\n",
-		       current->pid, cmd, arg, _IOC_NR(cmd), ret);
+			   current->pid, cmd, arg, _IOC_NR(cmd), ret);
 	return (long)ret;
 }
 
+int tegra_rpc_write(struct tegra_rpc_info *info, u8* buf, size_t size)
+{
+	int ret;
+
+	if (!info->rpc_ep)
+		return -EINVAL;
+
+	if (TEGRA_RPC_MAX_MSG_LEN < size)
+		return -EINVAL;
+
+	ret = trpc_send_msg(&rpc_node, info->rpc_ep, buf, size,
+						GFP_KERNEL);
+	if (ret)
+		return ret;
+	return size;
+}
+
 static ssize_t local_rpc_write(struct file *file, const char __user *buf,
-			       size_t count, loff_t *ppos)
+				   size_t count, loff_t *ppos)
 {
-	struct rpc_info *info = file->private_data;
+	struct tegra_rpc_info *info = file->private_data;
 	u8 data[TEGRA_RPC_MAX_MSG_LEN];
 	int ret;
 
@@ -255,16 +322,35 @@ static ssize_t local_rpc_write(struct file *file, const char __user *buf,
 		return -EFAULT;
 
 	ret = trpc_send_msg(&rpc_node, info->rpc_ep, data, count,
-			    GFP_KERNEL);
+				GFP_KERNEL);
 	if (ret)
 		return ret;
 	return count;
 }
 
+int tegra_rpc_read(struct tegra_rpc_info *info, u8 *buf, size_t max)
+{
+	int ret;
+
+	if (max > TEGRA_RPC_MAX_MSG_LEN)
+		return -EINVAL;
+
+	ret = trpc_recv_msg(&rpc_node, info->rpc_ep, buf,
+				TEGRA_RPC_MAX_MSG_LEN, 0);
+	if (ret == 0)
+		return 0;
+	else if (ret < 0)
+		return ret;
+	else if (ret > max)
+		return -ENOSPC;
+
+	return ret;
+}
+
 static ssize_t local_rpc_read(struct file *file, char __user *buf, size_t max,
-			      loff_t *ppos)
+				  loff_t *ppos)
 {
-	struct rpc_info *info = file->private_data;
+	struct tegra_rpc_info *info = file->private_data;
 	int ret;
 	u8 data[TEGRA_RPC_MAX_MSG_LEN];
 
@@ -272,7 +358,7 @@ static ssize_t local_rpc_read(struct file *file, char __user *buf, size_t max,
 		return -EINVAL;
 
 	ret = trpc_recv_msg(&rpc_node, info->rpc_ep, data,
-			    TEGRA_RPC_MAX_MSG_LEN, 0);
+				TEGRA_RPC_MAX_MSG_LEN, 0);
 	if (ret == 0)
 		return 0;
 	else if (ret < 0)
@@ -295,9 +381,9 @@ static const struct file_operations local_rpc_misc_fops = {
 };
 
 static struct miscdevice local_rpc_misc_device = {
-	.minor	= MISC_DYNAMIC_MINOR,
-	.name	= "tegra_rpc",
-	.fops	= &local_rpc_misc_fops,
+	.minor  = MISC_DYNAMIC_MINOR,
+	.name   = "tegra_rpc",
+	.fops   = &local_rpc_misc_fops,
 };
 
 int __init rpc_local_init(void)
diff --git a/drivers/media/video/tegra/avp/trpc_sema.c b/drivers/media/video/tegra/avp/trpc_sema.c
index b8772573d956..cd717a1a0ca3 100644
--- a/drivers/media/video/tegra/avp/trpc_sema.c
+++ b/drivers/media/video/tegra/avp/trpc_sema.c
@@ -29,7 +29,8 @@
 
 #include "trpc_sema.h"
 
-struct trpc_sema {
+struct tegra_sema_info {
+	struct file		*file;
 	wait_queue_head_t	wq;
 	spinlock_t		lock;
 	int			count;
@@ -46,7 +47,7 @@ static inline bool is_trpc_sema_file(struct file *file)
 	return false;
 }
 
-struct file *trpc_sema_get_from_fd(int fd)
+struct tegra_sema_info *trpc_sema_get_from_fd(int fd)
 {
 	struct file *file;
 
@@ -62,12 +63,17 @@ struct file *trpc_sema_get_from_fd(int fd)
 		return ERR_PTR(-EINVAL);
 	}
 
-	return file;
+	return file->private_data;
 }
 
-int trpc_sema_signal(struct file *file)
+void trpc_sema_put(struct tegra_sema_info *info)
+{
+	if (info->file)
+		fput(info->file);
+}
+
+int tegra_sema_signal(struct tegra_sema_info *info)
 {
-	struct trpc_sema *info = file->private_data;
 	unsigned long flags;
 
 	if (!info)
@@ -80,26 +86,25 @@ int trpc_sema_signal(struct file *file)
 	return 0;
 }
 
-static int trpc_sema_wait(struct trpc_sema *info, long *timeleft)
+int tegra_sema_wait(struct tegra_sema_info *info, long *timeout)
 {
 	unsigned long flags;
 	int ret = 0;
 	unsigned long endtime;
-	long timeout = *timeleft;
-
-	*timeleft = 0;
-	if (timeout < 0) {
-		timeout = MAX_SCHEDULE_TIMEOUT;
-	} else if (timeout > 0) {
-		timeout = msecs_to_jiffies(timeout);
-		endtime = jiffies + timeout;
-	}
+	long timeleft = *timeout;
+
+	*timeout = 0;
+	if (timeleft < 0)
+		timeleft = MAX_SCHEDULE_TIMEOUT;
+
+	timeleft = msecs_to_jiffies(timeleft);
+	endtime = jiffies + timeleft;
 
 again:
-	if (timeout)
+	if (timeleft)
 		ret = wait_event_interruptible_timeout(info->wq,
 						       info->count > 0,
-						       timeout);
+						       timeleft);
 	spin_lock_irqsave(&info->lock, flags);
 	if (info->count > 0) {
 		info->count--;
@@ -108,15 +113,15 @@ again:
 		ret = -ETIMEDOUT;
 	} else if (ret < 0) {
 		ret = -EINTR;
-		if (timeout != MAX_SCHEDULE_TIMEOUT &&
+		if (timeleft != MAX_SCHEDULE_TIMEOUT &&
 		    time_before(jiffies, endtime))
-			*timeleft = jiffies_to_msecs(endtime - jiffies);
+			*timeout = jiffies_to_msecs(endtime - jiffies);
 		else
-			*timeleft = 0;
+			*timeout = 0;
 	} else {
 		/* we woke up but someone else got the semaphore and we have
 		 * time left, try again */
-		timeout = ret;
+		timeleft = ret;
 		spin_unlock_irqrestore(&info->lock, flags);
 		goto again;
 	}
@@ -124,34 +129,53 @@ again:
 	return ret;
 }
 
-static int trpc_sema_open(struct inode *inode, struct file *file)
+int tegra_sema_open(struct tegra_sema_info **sema)
 {
-	struct trpc_sema *info;
-
-	info = kzalloc(sizeof(struct trpc_sema), GFP_KERNEL);
+	struct tegra_sema_info *info;
+	info = kzalloc(sizeof(struct tegra_sema_info), GFP_KERNEL);
 	if (!info)
 		return -ENOMEM;
 
-	nonseekable_open(inode, file);
 	init_waitqueue_head(&info->wq);
 	spin_lock_init(&info->lock);
+	*sema = info;
+	return 0;
+}
+
+static int trpc_sema_open(struct inode *inode, struct file *file)
+{
+	struct tegra_sema_info *info;
+	int ret;
+
+	ret = tegra_sema_open(&info);
+	if (ret < 0)
+		return ret;
+
+	info->file = file;
+	nonseekable_open(inode, file);
 	file->private_data = info;
 	return 0;
 }
 
+int tegra_sema_release(struct tegra_sema_info *sema)
+{
+	kfree(sema);
+	return 0;
+}
+
 static int trpc_sema_release(struct inode *inode, struct file *file)
 {
-	struct trpc_sema *info = file->private_data;
+	struct tegra_sema_info *info = file->private_data;
 
 	file->private_data = NULL;
-	kfree(info);
+	tegra_sema_release(info);
 	return 0;
 }
 
 static long trpc_sema_ioctl(struct file *file, unsigned int cmd,
 			    unsigned long arg)
 {
-	struct trpc_sema *info = file->private_data;
+	struct tegra_sema_info *info = file->private_data;
 	int ret;
 	long timeout;
 
@@ -166,14 +190,14 @@ static long trpc_sema_ioctl(struct file *file, unsigned int cmd,
 	case TEGRA_SEMA_IOCTL_WAIT:
 		if (copy_from_user(&timeout, (void __user *)arg, sizeof(long)))
 			return -EFAULT;
-		ret = trpc_sema_wait(info, &timeout);
+		ret = tegra_sema_wait(info, &timeout);
 		if (ret != -EINTR)
 			break;
 		if (copy_to_user((void __user *)arg, &timeout, sizeof(long)))
 			ret = -EFAULT;
 		break;
 	case TEGRA_SEMA_IOCTL_SIGNAL:
-		ret = trpc_sema_signal(file);
+		ret = tegra_sema_signal(info);
 		break;
 	default:
 		pr_err("%s: Unknown tegra_sema ioctl 0x%x\n", __func__,
diff --git a/drivers/media/video/tegra/avp/trpc_sema.h b/drivers/media/video/tegra/avp/trpc_sema.h
index 566bbdbe739e..2a7c42245b7f 100644
--- a/drivers/media/video/tegra/avp/trpc_sema.h
+++ b/drivers/media/video/tegra/avp/trpc_sema.h
@@ -21,8 +21,10 @@
 #include <linux/types.h>
 #include <linux/fs.h>
 
-struct file *trpc_sema_get_from_fd(int fd);
-int trpc_sema_signal(struct file *file);
+struct tegra_sema_info;
+
+struct tegra_sema_info *trpc_sema_get_from_fd(int fd);
+void trpc_sema_put(struct tegra_sema_info *sema);
 int __init trpc_sema_init(void);
 
 #endif
diff --git a/drivers/media/video/tegra/mediaserver/Kconfig b/drivers/media/video/tegra/mediaserver/Kconfig
new file mode 100644
index 000000000000..9e60a5b49cd3
--- /dev/null
+++ b/drivers/media/video/tegra/mediaserver/Kconfig
@@ -0,0 +1,10 @@
+config TEGRA_MEDIASERVER
+bool "Tegra Media Server support"
+depends on ARCH_TEGRA && TEGRA_RPC
+default y
+help
+  Enables support for the multiple OpenMAX clients.  Exports the
+  interface on the device node /dev/tegra_mediaserver.
+
+  If unsure, say Y
+
diff --git a/drivers/media/video/tegra/mediaserver/Makefile b/drivers/media/video/tegra/mediaserver/Makefile
new file mode 100644
index 000000000000..82e056f5faf5
--- /dev/null
+++ b/drivers/media/video/tegra/mediaserver/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_TEGRA_MEDIASERVER)	+= tegra_mediaserver.o
+
diff --git a/drivers/media/video/tegra/mediaserver/tegra_mediaserver.c b/drivers/media/video/tegra/mediaserver/tegra_mediaserver.c
new file mode 100644
index 000000000000..e25e1926d99e
--- /dev/null
+++ b/drivers/media/video/tegra/mediaserver/tegra_mediaserver.c
@@ -0,0 +1,554 @@
+/*
+ * Copyright (C) 2011 NVIDIA Corp.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/mm.h>
+
+#include <linux/tegra_mediaserver.h>
+#include "../avp/nvavp.h"
+#include "../../../../video/tegra/nvmap/nvmap.h"
+
+#define CHECK_STATUS(e, tag) \
+	do { if (e < 0) goto tag; } while (0)
+
+#define CHECK_NULL(ptr, tag) \
+	do { if (!ptr) goto tag; } while (0)
+
+#define CHECK_CONDITION(c, tag) \
+	do { if (c) goto tag; } while (0)
+
+struct tegra_mediasrv_block {
+	struct list_head entry;
+	struct tegra_mediaserver_block_info block;
+};
+
+struct tegra_mediasrv_iram {
+	struct list_head entry;
+	struct tegra_mediaserver_iram_info iram;
+};
+
+struct tegra_mediasrv_node {
+	struct tegra_mediasrv_info *mediasrv;
+	struct list_head blocks;
+	int nr_iram_shared;
+};
+
+struct tegra_mediasrv_manager {
+	struct tegra_avp_lib   lib;
+	struct tegra_rpc_info  *rpc;
+	struct tegra_sema_info *sema;
+};
+
+struct tegra_mediasrv_info {
+	int minor;
+	struct mutex lock;
+	struct nvmap_client *nvmap;
+	struct tegra_avp_info *avp;
+	struct tegra_mediasrv_manager manager;
+	int nr_nodes;
+	int nr_blocks;
+	struct tegra_mediaserver_iram_info iram; /* only one supported */
+	int nr_iram_shared;
+};
+
+static struct tegra_mediasrv_info *mediasrv_info;
+
+
+/*
+ * File entry points
+ */
+static int mediasrv_open(struct inode *inode, struct file *file)
+{
+	struct tegra_mediasrv_info *mediasrv = mediasrv_info;
+	struct tegra_mediasrv_node *node = NULL;
+	struct tegra_mediasrv_manager *manager = &mediasrv->manager;
+	struct tegra_avp_lib *lib = &manager->lib;
+	int e;
+
+	node = kzalloc(sizeof(struct tegra_mediasrv_node), GFP_KERNEL);
+	CHECK_NULL(node, node_alloc_fail);
+	INIT_LIST_HEAD(&node->blocks);
+	node->mediasrv = mediasrv;
+
+	mutex_lock(&mediasrv->lock);
+	nonseekable_open(inode, file);
+
+	if (!mediasrv->nr_nodes) {
+		e = tegra_sema_open(&manager->sema);
+		CHECK_STATUS(e, fail);
+
+		e = tegra_rpc_open(&manager->rpc);
+		CHECK_STATUS(e, fail);
+
+		e = tegra_rpc_port_create(manager->rpc, "NVMM_MANAGER_SRV",
+					  manager->sema);
+		CHECK_STATUS(e, fail);
+
+		e = tegra_avp_open(&mediasrv->avp);
+		CHECK_STATUS(e, fail);
+
+		memcpy(lib->name, "nvmm_manager.axf\0",
+			   strlen("nvmm_manager.axf") + 1);
+		lib->args = &mediasrv;
+		lib->args_len = sizeof(unsigned long);
+		e = tegra_avp_load_lib(mediasrv->avp, lib);
+		CHECK_STATUS(e, fail);
+
+		e = tegra_rpc_port_connect(manager->rpc, 50000);
+		CHECK_STATUS(e, fail);
+	}
+
+	mediasrv->nr_nodes++;
+	try_module_get(THIS_MODULE);
+
+	mutex_unlock(&mediasrv->lock);
+
+	file->private_data = node;
+
+	return 0;
+
+fail:
+	if (lib->handle) {
+		tegra_avp_unload_lib(mediasrv->avp, lib->handle);
+		lib->handle = 0;
+	}
+
+	if (mediasrv->avp) {
+		tegra_avp_release(mediasrv->avp);
+		mediasrv->avp = NULL;
+	}
+
+	if (manager->rpc) {
+		tegra_rpc_release(manager->rpc);
+		manager->rpc = NULL;
+	}
+	if (manager->sema) {
+		tegra_sema_release(manager->sema);
+		manager->sema = NULL;
+	}
+
+	kfree(node);
+
+	mutex_unlock(&mediasrv->lock);
+	return e;
+
+node_alloc_fail:
+	e = -ENOMEM;
+	return e;
+}
+
+static int mediasrv_release(struct inode *inode, struct file *file)
+{
+	struct tegra_mediasrv_info *mediasrv = mediasrv_info;
+	struct tegra_mediasrv_node *node = file->private_data;
+	struct tegra_mediasrv_block *block;
+	struct list_head *entry;
+	struct list_head *temp;
+	u32 message[2];
+	int e;
+
+	mutex_lock(&mediasrv->lock);
+
+	list_for_each_safe(entry, temp, &node->blocks) {
+		block = list_entry(entry, struct tegra_mediasrv_block, entry);
+
+		pr_info("Improperly closed block found!");
+		pr_info("  NVMM Block Handle: 0x%08x\n",
+			block->block.nvmm_block_handle);
+		pr_info("  AVP Block Handle: 0x%08x\n",
+			block->block.avp_block_handle);
+
+		message[0] = 1; /* NvmmManagerMsgType_AbnormalTerm */
+		message[1] = block->block.avp_block_handle;
+
+		e = tegra_rpc_write(mediasrv->manager.rpc, (u8 *)message,
+				    sizeof(u32) * 2);
+		pr_info("Abnormal termination message result: %d\n", e);
+
+		if (block->block.avp_block_library_handle) {
+			e = tegra_avp_unload_lib(mediasrv->avp,
+				block->block.avp_block_library_handle);
+			pr_info("Unload block (0x%08x) result: %d\n",
+				block->block.avp_block_library_handle, e);
+		}
+
+		if (block->block.service_library_handle) {
+			e = tegra_avp_unload_lib(mediasrv->avp,
+				block->block.service_library_handle);
+			pr_info("Unload service (0x%08x) result: %d\n",
+				block->block.service_library_handle, e);
+		}
+
+		mediasrv->nr_blocks--;
+		list_del(entry);
+		kfree(block);
+	}
+
+	mediasrv->nr_iram_shared -= node->nr_iram_shared;
+	if (mediasrv->iram.rm_handle && !mediasrv->nr_iram_shared) {
+		pr_info("Improperly freed shared iram found!");
+		nvmap_unpin_ids(mediasrv->nvmap, 1, &mediasrv->iram.rm_handle);
+		nvmap_free_handle_id(mediasrv->nvmap, mediasrv->iram.rm_handle);
+		mediasrv->iram.rm_handle = 0;
+		mediasrv->iram.physical_address = 0;
+	}
+
+	kfree(node);
+	mediasrv->nr_nodes--;
+	if (!mediasrv->nr_nodes) {
+		struct tegra_mediasrv_manager *manager = &mediasrv->manager;
+
+		tegra_avp_unload_lib(mediasrv->avp, manager->lib.handle);
+		manager->lib.handle = 0;
+
+		tegra_avp_release(mediasrv->avp);
+		mediasrv->avp = NULL;
+
+		tegra_rpc_release(manager->rpc);
+		manager->rpc = NULL;
+
+		tegra_sema_release(manager->sema);
+		manager->sema = NULL;
+	}
+
+	mutex_unlock(&mediasrv->lock);
+	module_put(THIS_MODULE);
+	return 0;
+}
+
+static int mediasrv_alloc(struct tegra_mediasrv_node *node,
+			  union tegra_mediaserver_alloc_info *in,
+			  union tegra_mediaserver_alloc_info *out)
+{
+	struct tegra_mediasrv_info *mediasrv = node->mediasrv;
+	int e;
+
+	switch (in->in.tegra_mediaserver_resource_type) {
+	case TEGRA_MEDIASERVER_RESOURCE_BLOCK:
+	{
+		struct tegra_mediasrv_block *block;
+
+		block = kzalloc(sizeof(struct tegra_mediasrv_node),
+				GFP_KERNEL);
+		CHECK_NULL(block, block_alloc_fail);
+
+		block->block = in->in.u.block;
+		list_add(&block->entry, &node->blocks);
+		goto block_done;
+
+block_alloc_fail:
+		e = -ENOMEM;
+	    goto fail;
+
+block_done:
+		mediasrv->nr_blocks++;
+		out->out.u.block.count = mediasrv->nr_blocks;
+	}
+	break;
+
+	case TEGRA_MEDIASERVER_RESOURCE_IRAM:
+	{
+		if (in->in.u.iram.tegra_mediaserver_iram_type ==
+		    TEGRA_MEDIASERVER_IRAM_SHARED) {
+			if (!mediasrv->nr_iram_shared) {
+				size_t align, size;
+				struct nvmap_handle_ref *r = NULL;
+				unsigned long id, physical_address;
+
+				size = PAGE_ALIGN(in->in.u.iram.size);
+				r = nvmap_create_handle(mediasrv->nvmap, size);
+				CHECK_CONDITION((r < 0),
+						iram_shared_handle_fail);
+
+				id = nvmap_ref_to_id(r);
+
+				align = max_t(size_t, in->in.u.iram.alignment,
+					      PAGE_SIZE);
+				e = nvmap_alloc_handle_id(mediasrv->nvmap, id,
+					NVMAP_HEAP_CARVEOUT_IRAM, align,
+					NVMAP_HANDLE_WRITE_COMBINE);
+				CHECK_STATUS(e, iram_shared_alloc_fail);
+
+				physical_address =
+					nvmap_pin_ids(mediasrv->nvmap, 1, &id);
+				CHECK_CONDITION((physical_address < 0),
+						iram_shared_pin_fail);
+
+				mediasrv->iram.rm_handle = id;
+				mediasrv->iram.physical_address =
+					physical_address;
+				goto iram_shared_done;
+
+iram_shared_pin_fail:
+				e = physical_address;
+iram_shared_alloc_fail:
+				nvmap_free_handle_id(mediasrv->nvmap, id);
+iram_shared_handle_fail:
+				goto fail;
+			}
+
+iram_shared_done:
+			out->out.u.iram.rm_handle = mediasrv->iram.rm_handle;
+			out->out.u.iram.physical_address =
+				mediasrv->iram.physical_address;
+			mediasrv->nr_iram_shared++;
+			node->nr_iram_shared++;
+		} else if (in->in.u.iram.tegra_mediaserver_iram_type ==
+			   TEGRA_MEDIASERVER_IRAM_SCRATCH) {
+			e = -EINVAL;
+			goto fail;
+		}
+	}
+	break;
+
+	default:
+	{
+		e = -EINVAL;
+		goto fail;
+	}
+	break;
+	}
+
+	return 0;
+
+fail:
+	return e;
+}
+
+static void mediasrv_free(struct tegra_mediasrv_node *node,
+			  union tegra_mediaserver_free_info *in)
+{
+	struct tegra_mediasrv_info *mediasrv = node->mediasrv;
+
+	switch (in->in.tegra_mediaserver_resource_type) {
+	case TEGRA_MEDIASERVER_RESOURCE_BLOCK:
+	{
+		struct tegra_mediasrv_block *block;
+		struct tegra_mediasrv_block *temp;
+		struct list_head *entry;
+
+		list_for_each(entry, &node->blocks) {
+			temp = list_entry(entry, struct tegra_mediasrv_block,
+					  entry);
+			if (temp->block.nvmm_block_handle !=
+			    in->in.u.nvmm_block_handle)
+				continue;
+
+			block = temp;
+			break;
+		}
+
+		CHECK_NULL(block, done);
+		list_del(&block->entry);
+		kfree(block);
+	}
+	break;
+
+	case TEGRA_MEDIASERVER_RESOURCE_IRAM:
+	{
+		if (in->in.u.iram_rm_handle == mediasrv->iram.rm_handle &&
+		    node->nr_iram_shared) {
+			node->nr_iram_shared--;
+			mediasrv->nr_iram_shared--;
+
+			if (!mediasrv->nr_iram_shared) {
+				nvmap_unpin_ids(mediasrv->nvmap, 1,
+						&mediasrv->iram.rm_handle);
+				nvmap_free_handle_id(mediasrv->nvmap,
+						     mediasrv->iram.rm_handle);
+				mediasrv->iram.rm_handle = 0;
+				mediasrv->iram.physical_address = 0;
+			}
+		}
+
+		else
+			goto done;
+	}
+	break;
+	}
+
+done:
+	return;
+}
+
+static int mediasrv_update_block_info(
+	struct tegra_mediasrv_node *node,
+	union tegra_mediaserver_update_block_info *in
+)
+{
+	struct tegra_mediasrv_block *entry;
+	struct tegra_mediasrv_block *block;
+	int e;
+
+	list_for_each_entry(entry, &node->blocks, entry) {
+		if (entry->block.nvmm_block_handle != in->in.nvmm_block_handle)
+			continue;
+
+		block = entry;
+		break;
+	}
+
+	CHECK_NULL(block, fail);
+
+	block->block = in->in;
+	return 0;
+
+fail:
+	e = -EINVAL;
+	return e;
+}
+
+static long mediasrv_unlocked_ioctl(struct file *file, unsigned int cmd,
+				    unsigned long arg)
+{
+	struct tegra_mediasrv_info *mediasrv = mediasrv_info;
+	struct tegra_mediasrv_node *node = file->private_data;
+	int e = -ENODEV;
+
+	mutex_lock(&mediasrv->lock);
+
+	switch (cmd) {
+	case TEGRA_MEDIASERVER_IOCTL_ALLOC:
+	{
+		union tegra_mediaserver_alloc_info in, out;
+		e = copy_from_user(&in, (void __user *)arg, sizeof(in));
+		CHECK_CONDITION(e, copy_fail);
+		e = mediasrv_alloc(node, &in, &out);
+		CHECK_STATUS(e, fail);
+		e = copy_to_user((void __user *)arg, &out, sizeof(out));
+		CHECK_CONDITION(e, copy_fail);
+	}
+	break;
+
+	case TEGRA_MEDIASERVER_IOCTL_FREE:
+	{
+		union tegra_mediaserver_free_info in;
+		e = copy_from_user(&in, (void __user *)arg, sizeof(in));
+		CHECK_CONDITION(e, copy_fail);
+		mediasrv_free(node, &in);
+	}
+	break;
+
+	case TEGRA_MEDIASERVER_IOCTL_UPDATE_BLOCK_INFO:
+	{
+		union tegra_mediaserver_update_block_info in;
+		e = copy_from_user(&in, (void __user *)arg, sizeof(in));
+		CHECK_CONDITION(e, copy_fail);
+		e = mediasrv_update_block_info(node, &in);
+		CHECK_CONDITION(e, fail);
+	}
+	break;
+
+	default:
+	{
+		e = -ENODEV;
+		goto fail;
+	}
+	break;
+	}
+
+	mutex_unlock(&mediasrv->lock);
+	return 0;
+
+copy_fail:
+	e = -EFAULT;
+fail:
+	return e;
+}
+
+/*
+ * Kernel structures and entry points
+ */
+static const struct file_operations mediaserver_fops = {
+	.owner			= THIS_MODULE,
+	.open			= mediasrv_open,
+	.release		= mediasrv_release,
+	.unlocked_ioctl = mediasrv_unlocked_ioctl,
+};
+
+static struct miscdevice mediaserver_misc_device = {
+	.minor	= MISC_DYNAMIC_MINOR,
+	.name	= "tegra_mediaserver",
+	.fops	= &mediaserver_fops,
+};
+
+static int __init tegra_mediaserver_init(void)
+{
+	struct tegra_mediasrv_info *mediasrv;
+	int e = 0;
+
+	CHECK_NULL(!mediasrv_info, busy);
+
+	mediasrv = kzalloc(sizeof(struct tegra_mediasrv_info), GFP_KERNEL);
+	CHECK_NULL(mediasrv, alloc_fail);
+
+	mediasrv->nvmap = nvmap_create_client(nvmap_dev, "tegra_mediaserver");
+	CHECK_NULL(mediasrv, nvmap_create_fail);
+
+	e = misc_register(&mediaserver_misc_device);
+	CHECK_STATUS(e, register_fail);
+
+	mediasrv->nr_nodes = 0;
+	mutex_init(&mediasrv->lock);
+
+	mediasrv_info = mediasrv;
+	goto done;
+
+nvmap_create_fail:
+	e = -ENOMEM;
+	kfree(mediasrv);
+	goto done;
+
+register_fail:
+	nvmap_client_put(mediasrv->nvmap);
+	kfree(mediasrv);
+	goto done;
+
+alloc_fail:
+	e = -ENOMEM;
+	goto done;
+
+busy:
+	e = -EBUSY;
+	goto done;
+
+done:
+	return e;
+}
+
+void __exit tegra_mediaserver_cleanup(void)
+{
+	struct tegra_mediasrv_info *mediasrv = mediasrv_info;
+	int e;
+
+	e = misc_deregister(&mediaserver_misc_device);
+	CHECK_STATUS(e, fail);
+
+	nvmap_client_put(mediasrv->nvmap);
+	kfree(mediasrv);
+	mediasrv_info = NULL;
+
+fail:
+	return;
+}
+
+module_init(tegra_mediaserver_init);
+module_exit(tegra_mediaserver_cleanup);
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/media/video/tegra/ov2710.c b/drivers/media/video/tegra/ov2710.c
new file mode 100644
index 000000000000..5497b6cf213d
--- /dev/null
+++ b/drivers/media/video/tegra/ov2710.c
@@ -0,0 +1,693 @@
+/*
+ * ov2710.c - ov2710 sensor driver
+ *
+ * Copyright (c) 2011, NVIDIA, All Rights Reserved.
+ *
+ * Contributors:
+ *      erik lilliebjerg <elilliebjerg@nvidia.com>
+ *
+ * Leverage OV5650.c
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ */
+
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <media/ov2710.h>
+
+struct ov2710_reg {
+	u16 addr;
+	u16 val;
+};
+
+struct ov2710_info {
+	int mode;
+	struct i2c_client *i2c_client;
+	struct ov2710_platform_data *pdata;
+};
+
+#define OV2710_TABLE_WAIT_MS 0
+#define OV2710_TABLE_END 1
+#define OV2710_MAX_RETRIES 3
+
+static struct ov2710_reg mode_start[] = {
+	{0x3008, 0x82}, /* reset registers pg 72 */
+	{OV2710_TABLE_WAIT_MS, 5},
+	{0x3008, 0x42}, /* register power down pg 72 */
+	{OV2710_TABLE_WAIT_MS, 5},
+	{0x3103, 0x93}, /* power up system clock from PLL page 77 */
+	{0x3017, 0xff}, /* PAD output enable page 100 */
+	{0x3018, 0xfc}, /* PAD output enable page 100 */
+
+	{0x3600, 0x50}, /* analog pg 108 */
+	{0x3601, 0x0d}, /* analog pg 108 */
+	{0x3604, 0x50}, /* analog pg 108 */
+	{0x3605, 0x04}, /* analog pg 108 */
+	{0x3606, 0x3f}, /* analog pg 108 */
+	{0x3612, 0x1a}, /* analog pg 108 */
+	{0x3630, 0x22}, /* analog pg 108 */
+	{0x3631, 0x22}, /* analog pg 108 */
+	{0x3702, 0x3a}, /* analog pg 108 */
+	{0x3704, 0x18}, /* analog pg 108 */
+	{0x3705, 0xda}, /* analog pg 108 */
+	{0x3706, 0x41}, /* analog pg 108 */
+	{0x370a, 0x80}, /* analog pg 108 */
+	{0x370b, 0x40}, /* analog pg 108 */
+	{0x370e, 0x00}, /* analog pg 108 */
+	{0x3710, 0x28}, /* analog pg 108 */
+	{0x3712, 0x13}, /* analog pg 108 */
+	{0x3830, 0x50}, /* manual exposure gain bit [0] */
+	{0x3a18, 0x00}, /* AEC gain ceiling bit 8 pg 114 */
+	{0x3a19, 0xf8}, /* AEC gain ceiling pg 114 */
+	{0x3a00, 0x38}, /* AEC control 0 debug mode band low
+			   limit mode band func pg 112 */
+
+	{0x3603, 0xa7}, /* analog pg 108 */
+	{0x3615, 0x50}, /* analog pg 108 */
+	{0x3620, 0x56}, /* analog pg 108 */
+	{0x3810, 0x00}, /* TIMING HVOFFS both are zero pg 80 */
+	{0x3836, 0x00}, /* TIMING HVPAD both are zero pg 82 */
+	{0x3a1a, 0x06}, /* DIFF MAX an AEC register??? pg 114 */
+	{0x4000, 0x01}, /* BLC enabled pg 120 */
+	{0x401c, 0x48}, /* reserved pg 120 */
+	{0x401d, 0x28}, /* BLC control pg 120 */
+	{0x5000, 0x00}, /* ISP control00 features are disabled. pg 132 */
+	{0x5001, 0x00}, /* ISP control01 awb disabled. pg 132 */
+	{0x5002, 0x00}, /* ISP control02 debug mode disabled pg 132 */
+	{0x503d, 0x00}, /* ISP control3D features disabled pg 133 */
+	{0x5046, 0x00}, /* ISP control isp disable awbg disable pg 133 */
+
+	{0x300f, 0x8f}, /* PLL control00 R_SELD5 [7:6] div by 4 R_DIVL [2]
+			   two lane div 1 SELD2P5 [1:0] div 2.5 pg 99 */
+	{0x3010, 0x10}, /* PLL control01 DIVM [3:0] DIVS [7:4] div 1 pg 99 */
+	{0x3011, 0x14}, /* PLL control02 R_DIVP [5:0] div 20 pg 99 */
+	{0x3012, 0x02}, /* PLL CTR 03, default */
+	{0x3815, 0x82}, /* PCLK to SCLK ratio bit[4:0] is set to 2 pg 81 */
+	{0x3503, 0x33}, /* AEC auto AGC auto gain has no latch delay. pg 38 */
+	/*	{FAST_SETMODE_START, 0}, */
+	{0x3613, 0x44}, /* analog pg 108 */
+	{OV2710_TABLE_END, 0x0},
+};
+
+static struct ov2710_reg mode_1920x1080[] = {
+	{0x3103, 0x93},
+	{0x3008, 0x82},
+	{0x3017, 0x7f},
+	{0x3018, 0xfc},
+	{0x3706, 0x61},
+	{0x3712, 0x0c},
+	{0x3630, 0x6d},
+	{0x3801, 0xb4},
+
+	{0x3621, 0x04},
+	{0x3604, 0x60},
+	{0x3603, 0xa7},
+	{0x3631, 0x26},
+	{0x3600, 0x04},
+	{0x3620, 0x37},
+	{0x3623, 0x00},
+	{0x3702, 0x9e},
+	{0x3703, 0x5c},
+	{0x3704, 0x40},
+	{0x370d, 0x0f},
+	{0x3713, 0x9f},
+	{0x3714, 0x4c},
+	{0x3710, 0x9e},
+	{0x3801, 0xc4},
+	{0x3605, 0x05},
+	{0x3606, 0x3f},
+	{0x302d, 0x90},
+	{0x370b, 0x40},
+	{0x3716, 0x31},
+	{0x380d, 0x74},
+	{0x5181, 0x20},
+	{0x518f, 0x00},
+	{0x4301, 0xff},
+	{0x4303, 0x00},
+	{0x3a00, 0x78},
+	{0x3a18, 0x00}, /* AEC gain ceiling bit 8 pg 51 */
+	{0x3a19, 0xf8}, /* AEC gain ceiling pg 51 */
+	{0x300f, 0x88},
+	{0x3011, 0x28},
+	{0x3a1a, 0x06},
+	{0x3a18, 0x00},
+	{0x3a19, 0x7a},
+	{0x3a13, 0x54},
+	{0x382e, 0x0f},
+	{0x381a, 0x1a},
+	{0x401d, 0x02},
+	{0x5688, 0x03},
+	{0x5684, 0x07},
+	{0x5685, 0xa0},
+	{0x5686, 0x04},
+	{0x5687, 0x43},
+	{0x3011, 0x0a},
+	{0x300f, 0x8a},
+	{0x3017, 0x00},
+	{0x3018, 0x00},
+	{0x300e, 0x04},
+	{0x4801, 0x0f},
+	{0x300f, 0xc3},
+	{0x3a0f, 0x40},
+	{0x3a10, 0x38},
+	{0x3a1b, 0x48},
+	{0x3a1e, 0x30},
+	{0x3a11, 0x90},
+	{0x3a1f, 0x10},
+
+	{0x350c, 0xff}, /* peak VTS reg, set to highest limit */
+	{0x350d, 0xff}, /* peak VTS reg, set to highest limit */
+	{0x3503, 0x07}, /* enable manual gain and manual exposure */
+	{0x3500, 0x00}, /* write default to AEC PK EXPO */
+	{0x3501, 0x00}, /* write default to AEC PK EXPO */
+	{0x3502, 0x02}, /* write default to AEC PK EXPO */
+	{0x350a, 0x00}, /* write default to manual gain reg */
+	{0x350b, 0x10}, /* write default to manual gain reg */
+
+	{OV2710_TABLE_END, 0x0000}
+};
+
+static struct ov2710_reg mode_1280x720[] = {
+	{0x3008, 0x82},
+	{OV2710_TABLE_WAIT_MS, 5},
+	{0x3008, 0x02},
+	{OV2710_TABLE_WAIT_MS, 5},
+	{0x3103, 0x93},
+	{0x3017, 0x7f},
+	{0x3018, 0xfc},
+
+	{0x3706, 0x61},
+	{0x3712, 0x0c},
+	{0x3630, 0x6d},
+	{0x3801, 0xb4},
+	{0x3621, 0x04},
+	{0x3604, 0x60},
+	{0x3603, 0xa7},
+	{0x3631, 0x26},
+	{0x3600, 0x04},
+	{0x3620, 0x37},
+	{0x3623, 0x00},
+	{0x3702, 0x9e},
+	{0x3703, 0x5c},
+	{0x3704, 0x40},
+	{0x370d, 0x0f},
+	{0x3713, 0x9f},
+	{0x3714, 0x4c},
+	{0x3710, 0x9e},
+	{0x3801, 0xc4},
+	{0x3605, 0x05},
+	{0x3606, 0x3f},
+	{0x302d, 0x90},
+	{0x370b, 0x40},
+	{0x3716, 0x31},
+	{0x380d, 0x74},
+	{0x5181, 0x20},
+	{0x518f, 0x00},
+	{0x4301, 0xff},
+	{0x4303, 0x00},
+	{0x3a00, 0x78},
+	{0x3a18, 0x00}, /* AEC gain ceiling bit 8 pg 51 */
+	{0x3a19, 0xf8}, /* AEC gain ceiling pg 51 */
+	{0x300f, 0x88},
+	{0x3011, 0x28},
+	{0x3a1a, 0x06},
+	{0x3a18, 0x00},
+	{0x3a19, 0x7a},
+	{0x3a13, 0x54},
+	{0x382e, 0x0f},
+	{0x381a, 0x1a},
+	{0x401d, 0x02},
+	{0x381c, 0x10},
+	{0x381d, 0xb8},
+	{0x381e, 0x02},
+	{0x381f, 0xdc},
+	{0x3820, 0x0a},
+	{0x3821, 0x29},
+	{0x3804, 0x05},
+	{0x3805, 0x00},
+	{0x3806, 0x02},
+	{0x3807, 0xd0},
+	{0x3808, 0x05},
+	{0x3809, 0x00},
+	{0x380a, 0x02},
+	{0x380b, 0xd0},
+	{0x380e, 0x02},
+	{0x380f, 0xe8},
+	{0x380c, 0x07},
+	{0x380d, 0x00},
+	{0x5688, 0x03},
+	{0x5684, 0x05},
+	{0x5685, 0x00},
+	{0x5686, 0x02},
+	{0x5687, 0xd0},
+	{0x3a08, 0x1b},
+	{0x3a09, 0xe6},
+	{0x3a0a, 0x17},
+	{0x3a0b, 0x40},
+	{0x3a0e, 0x01},
+	{0x3a0d, 0x02},
+	{0x3011, 0x0a},
+	{0x300f, 0x8a},
+	{0x3017, 0x00},
+	{0x3018, 0x00},
+
+	{0x4803, 0x50}, /* MIPI CTRL3 pg 91 */
+	{0x4800, 0x24}, /* MIPI CTRl0 idle and short line pg 89 */
+
+	{0x300e, 0x04},
+	{0x4801, 0x0f},
+	{0x300f, 0xc3},
+	{0x3a0f, 0x40},
+	{0x3a10, 0x38},
+	{0x3a1b, 0x48},
+	{0x3a1e, 0x30},
+	{0x3a11, 0x90},
+	{0x3a1f, 0x10},
+
+	{0x3010, 0x10},
+	{0x3a0e, 0x02},
+	{0x3a0d, 0x03},
+	{0x3a08, 0x0d},
+	{0x3a09, 0xf3},
+	{0x3a0a, 0x0b},
+	{0x3a0b, 0xa0},
+
+	{0x350c, 0xff}, /* peak VTS reg, set to highest limit */
+	{0x350d, 0xff}, /* peak VTS reg, set to highest limit */
+	{0x3503, 0x07}, /* enable manual gain and manual exposure */
+	{0x3500, 0x00}, /* write default to AEC PK EXPO */
+	{0x3501, 0x00}, /* write default to AEC PK EXPO */
+	{0x3502, 0x02}, /* write default to AEC PK EXPO */
+	{0x350a, 0x00}, /* write default to manual gain reg */
+	{0x350b, 0x10}, /* write default to manual gain reg */
+
+	{OV2710_TABLE_END, 0x0000}
+};
+
+static struct ov2710_reg mode_end[] = {
+	{0x3212, 0x00}, /* SRM_GROUP_ACCESS (group hold begin) */
+	{0x3003, 0x01}, /* reset DVP pg 97 */
+	{0x3212, 0x10}, /* SRM_GROUP_ACCESS (group hold end) */
+	{0x3212, 0xa0}, /* SRM_GROUP_ACCESS (group hold launch) */
+	{0x3008, 0x02}, /* SYSTEM_CTRL0 mipi suspend mask pg 98 */
+
+	/*	{FAST_SETMODE_END, 0}, */
+	{OV2710_TABLE_END, 0x0000}
+};
+
+enum {
+	OV2710_MODE_1920x1080,
+	OV2710_MODE_1280x720,
+};
+
+static struct ov2710_reg *mode_table[] = {
+	[OV2710_MODE_1920x1080] = mode_1920x1080,
+	[OV2710_MODE_1280x720] = mode_1280x720,
+};
+
+/* 2 regs to program frame length */
+static inline void ov2710_get_frame_length_regs(struct ov2710_reg *regs,
+						u32 frame_length)
+{
+	regs->addr = 0x380e;
+	regs->val = (frame_length >> 8) & 0xff;
+	(regs + 1)->addr = 0x380f;
+	(regs + 1)->val = (frame_length) & 0xff;
+}
+
+/* 3 regs to program coarse time */
+static inline void ov2710_get_coarse_time_regs(struct ov2710_reg *regs,
+					       u32 coarse_time)
+{
+	regs->addr = 0x3500;
+	regs->val = (coarse_time >> 12) & 0xff;
+	(regs + 1)->addr = 0x3501;
+	(regs + 1)->val = (coarse_time >> 4) & 0xff;
+	(regs + 2)->addr = 0x3502;
+	(regs + 2)->val = (coarse_time & 0xf) << 4;
+}
+
+/* 1 reg to program gain */
+static inline void ov2710_get_gain_reg(struct ov2710_reg *regs, u16 gain)
+{
+	regs->addr = 0x350b;
+	regs->val = gain;
+}
+
+static int ov2710_read_reg(struct i2c_client *client, u16 addr, u8 *val)
+{
+	int err;
+	struct i2c_msg msg[2];
+	unsigned char data[3];
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 2;
+	msg[0].buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8) (addr >> 8);;
+	data[1] = (u8) (addr & 0xff);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 1;
+	msg[1].buf = data + 2;
+
+	err = i2c_transfer(client->adapter, msg, 2);
+
+	if (err != 2)
+		return -EINVAL;
+
+	*val = data[2];
+
+	return 0;
+}
+
+static int ov2710_write_reg(struct i2c_client *client, u16 addr, u8 val)
+{
+	int err;
+	struct i2c_msg msg;
+	unsigned char data[3];
+	int retry = 0;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data[0] = (u8) (addr >> 8);;
+	data[1] = (u8) (addr & 0xff);
+	data[2] = (u8) (val & 0xff);
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 3;
+	msg.buf = data;
+
+	do {
+		err = i2c_transfer(client->adapter, &msg, 1);
+		if (err == 1)
+			return 0;
+		retry++;
+		pr_err("ov2710: i2c transfer failed, retrying %x %x\n",
+		       addr, val);
+		msleep(3);
+	} while (retry <= OV2710_MAX_RETRIES);
+
+	return err;
+}
+
+static int ov2710_write_table(struct i2c_client *client,
+			      const struct ov2710_reg table[],
+			      const struct ov2710_reg override_list[],
+			      int num_override_regs)
+{
+	int err;
+	const struct ov2710_reg *next;
+	int i;
+	u16 val;
+
+	for (next = table; next->addr != OV2710_TABLE_END; next++) {
+		if (next->addr == OV2710_TABLE_WAIT_MS) {
+			msleep(next->val);
+			continue;
+		}
+
+		val = next->val;
+
+		/* When an override list is passed in, replace the reg */
+		/* value to write if the reg is in the list            */
+		if (override_list) {
+			for (i = 0; i < num_override_regs; i++) {
+				if (next->addr == override_list[i].addr) {
+					val = override_list[i].val;
+					break;
+				}
+			}
+		}
+
+		err = ov2710_write_reg(client, next->addr, val);
+		if (err)
+			return err;
+	}
+	return 0;
+}
+
+static int ov2710_set_mode(struct ov2710_info *info, struct ov2710_mode *mode)
+{
+	int sensor_mode;
+	int err;
+	struct ov2710_reg reg_list[6];
+
+	pr_info("%s: xres %u yres %u framelength %u coarsetime %u gain %u\n",
+		__func__, mode->xres, mode->yres, mode->frame_length,
+		mode->coarse_time, mode->gain);
+	if (mode->xres == 1920 && mode->yres == 1080)
+		sensor_mode = OV2710_MODE_1920x1080;
+	else if (mode->xres == 1264 && mode->yres == 704)
+		sensor_mode = OV2710_MODE_1280x720;
+	else {
+		pr_err("%s: invalid resolution supplied to set mode %d %d\n",
+		       __func__, mode->xres, mode->yres);
+		return -EINVAL;
+	}
+
+	/* get a list of override regs for the asking frame length, */
+	/* coarse integration time, and gain.                       */
+	ov2710_get_frame_length_regs(reg_list, mode->frame_length);
+	ov2710_get_coarse_time_regs(reg_list + 2, mode->coarse_time);
+	ov2710_get_gain_reg(reg_list + 5, mode->gain);
+
+	err = ov2710_write_table(info->i2c_client, mode_table[sensor_mode],
+	NULL, 0);
+	if (err)
+		return err;
+
+	info->mode = sensor_mode;
+	return 0;
+}
+
+static int ov2710_set_frame_length(struct ov2710_info *info, u32 frame_length)
+{
+	struct ov2710_reg reg_list[2];
+	int i = 0;
+	int ret;
+
+	ov2710_get_frame_length_regs(reg_list, frame_length);
+
+	for (i = 0; i < 2; i++)	{
+		ret = ov2710_write_reg(info->i2c_client, reg_list[i].addr,
+			reg_list[i].val);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int ov2710_set_coarse_time(struct ov2710_info *info, u32 coarse_time)
+{
+	int ret;
+
+	struct ov2710_reg reg_list[3];
+	int i = 0;
+
+	ov2710_get_coarse_time_regs(reg_list, coarse_time);
+
+	ret = ov2710_write_reg(info->i2c_client, 0x3212, 0x01);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < 3; i++)	{
+		ret = ov2710_write_reg(info->i2c_client, reg_list[i].addr,
+			reg_list[i].val);
+		if (ret)
+			return ret;
+	}
+
+	ret = ov2710_write_reg(info->i2c_client, 0x3212, 0x11);
+	if (ret)
+		return ret;
+
+	ret = ov2710_write_reg(info->i2c_client, 0x3212, 0xa1);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int ov2710_set_gain(struct ov2710_info *info, u16 gain)
+{
+	int ret;
+	struct ov2710_reg reg_list;
+
+	ov2710_get_gain_reg(&reg_list, gain);
+
+	ret = ov2710_write_reg(info->i2c_client, reg_list.addr, reg_list.val);
+
+	return ret;
+}
+
+static int ov2710_get_status(struct ov2710_info *info, u8 *status)
+{
+	int err;
+
+	*status = 0;
+	err = ov2710_read_reg(info->i2c_client, 0x002, status);
+	return err;
+}
+
+
+static long ov2710_ioctl(struct file *file,
+			 unsigned int cmd, unsigned long arg)
+{
+	int err;
+	struct ov2710_info *info = file->private_data;
+
+	switch (cmd) {
+	case OV2710_IOCTL_SET_MODE:
+	{
+		struct ov2710_mode mode;
+		if (copy_from_user(&mode,
+				   (const void __user *)arg,
+				   sizeof(struct ov2710_mode))) {
+			return -EFAULT;
+		}
+
+		return ov2710_set_mode(info, &mode);
+	}
+	case OV2710_IOCTL_SET_FRAME_LENGTH:
+		return ov2710_set_frame_length(info, (u32)arg);
+	case OV2710_IOCTL_SET_COARSE_TIME:
+		return ov2710_set_coarse_time(info, (u32)arg);
+	case OV2710_IOCTL_SET_GAIN:
+		return ov2710_set_gain(info, (u16)arg);
+	case OV2710_IOCTL_GET_STATUS:
+	{
+		u8 status;
+
+		err = ov2710_get_status(info, &status);
+		if (err)
+			return err;
+		if (copy_to_user((void __user *)arg, &status,
+				 2)) {
+			return -EFAULT;
+		}
+		return 0;
+	}
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static struct ov2710_info *info;
+
+static int ov2710_open(struct inode *inode, struct file *file)
+{
+	u8 status;
+
+	file->private_data = info;
+	if (info->pdata && info->pdata->power_on)
+		info->pdata->power_on();
+	ov2710_get_status(info, &status);
+	return 0;
+}
+
+int ov2710_release(struct inode *inode, struct file *file)
+{
+	if (info->pdata && info->pdata->power_off)
+		info->pdata->power_off();
+	file->private_data = NULL;
+	return 0;
+}
+
+
+static const struct file_operations ov2710_fileops = {
+	.owner = THIS_MODULE,
+	.open = ov2710_open,
+	.unlocked_ioctl = ov2710_ioctl,
+	.release = ov2710_release,
+};
+
+static struct miscdevice ov2710_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "ov2710",
+	.fops = &ov2710_fileops,
+};
+
+static int ov2710_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int err;
+
+	pr_info("ov2710: probing sensor.\n");
+
+	info = kzalloc(sizeof(struct ov2710_info), GFP_KERNEL);
+	if (!info) {
+		pr_err("ov2710: Unable to allocate memory!\n");
+		return -ENOMEM;
+	}
+
+	err = misc_register(&ov2710_device);
+	if (err) {
+		pr_err("ov2710: Unable to register misc device!\n");
+		kfree(info);
+		return err;
+	}
+
+	info->pdata = client->dev.platform_data;
+	info->i2c_client = client;
+
+	i2c_set_clientdata(client, info);
+	return 0;
+}
+
+static int ov2710_remove(struct i2c_client *client)
+{
+	struct ov2710_info *info;
+	info = i2c_get_clientdata(client);
+	misc_deregister(&ov2710_device);
+	kfree(info);
+	return 0;
+}
+
+static const struct i2c_device_id ov2710_id[] = {
+	{ "ov2710", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, ov2710_id);
+
+static struct i2c_driver ov2710_i2c_driver = {
+	.driver = {
+		.name = "ov2710",
+		.owner = THIS_MODULE,
+	},
+	.probe = ov2710_probe,
+	.remove = ov2710_remove,
+	.id_table = ov2710_id,
+};
+
+static int __init ov2710_init(void)
+{
+	pr_info("ov2710 sensor driver loading\n");
+	return i2c_add_driver(&ov2710_i2c_driver);
+}
+
+static void __exit ov2710_exit(void)
+{
+	i2c_del_driver(&ov2710_i2c_driver);
+}
+
+module_init(ov2710_init);
+module_exit(ov2710_exit);
+
diff --git a/drivers/media/video/tegra/ov5650.c b/drivers/media/video/tegra/ov5650.c
new file mode 100644
index 000000000000..9f329d375f26
--- /dev/null
+++ b/drivers/media/video/tegra/ov5650.c
@@ -0,0 +1,975 @@
+/*
+ * ov5650.c - ov5650 sensor driver
+ *
+ * Copyright (C) 2011 Google Inc.
+ *
+ * Contributors:
+ *      Rebecca Schultz Zavin <rebecca@android.com>
+ *
+ * Leverage OV9640.c
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ */
+
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <media/ov5650.h>
+
+struct ov5650_reg {
+	u16 addr;
+	u16 val;
+};
+
+enum StereoCameraMode{
+	/* Sets the default camera to Main */
+	Main,
+	/* Sets the stereo camera to stereo mode. */
+	Stereo,
+	/* Only the sensor on the left is on. */
+	LeftOnly,
+	/* Only the sensor on the right is on. */
+	RightOnly,
+	/* Ignore -- Forces compilers to make 32-bit enums. */
+	StereoCameraMode_Force32 = 0x7FFFFFFF
+};
+
+struct ov5650_sensor {
+	struct i2c_client *i2c_client;
+	struct ov5650_platform_data *pdata;
+};
+
+struct ov5650_info {
+	int mode;
+	enum StereoCameraMode camera_mode;
+	struct ov5650_sensor left;
+	struct ov5650_sensor right;
+};
+
+static struct ov5650_info *info;
+
+#define OV5650_TABLE_WAIT_MS 0
+#define OV5650_TABLE_END 1
+#define OV5650_MAX_RETRIES 3
+
+static struct ov5650_reg tp_none_seq[] = {
+	{0x5046, 0x00}, /* isp_off */
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg tp_cbars_seq[] = {
+	{0x503D, 0xC0},
+	{0x503E, 0x00},
+	{0x5046, 0x01}, /* isp_on */
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg tp_checker_seq[] = {
+	{0x503D, 0xC0},
+	{0x503E, 0x0A},
+	{0x5046, 0x01}, /* isp_on */
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg *test_pattern_modes[] = {
+	tp_none_seq,
+	tp_cbars_seq,
+	tp_checker_seq,
+};
+
+static struct ov5650_reg reset_seq[] = {
+	{0x3008, 0x82}, /* reset registers pg 72 */
+	{OV5650_TABLE_WAIT_MS, 5},
+	{0x3008, 0x42}, /* register power down pg 72 */
+	{OV5650_TABLE_WAIT_MS, 5},
+	{OV5650_TABLE_END, 0x0000},
+};
+
+static struct ov5650_reg mode_start[] = {
+	{0x3103, 0x93}, /* power up system clock from PLL page 77 */
+	{0x3017, 0xff}, /* PAD output enable page 100 */
+	{0x3018, 0xfc}, /* PAD output enable page 100 */
+
+	{0x3600, 0x50}, /* analog pg 108 */
+	{0x3601, 0x0d}, /* analog pg 108 */
+	{0x3604, 0x50}, /* analog pg 108 */
+	{0x3605, 0x04}, /* analog pg 108 */
+	{0x3606, 0x3f}, /* analog pg 108 */
+	{0x3612, 0x1a}, /* analog pg 108 */
+	{0x3630, 0x22}, /* analog pg 108 */
+	{0x3631, 0x22}, /* analog pg 108 */
+	{0x3702, 0x3a}, /* analog pg 108 */
+	{0x3704, 0x18}, /* analog pg 108 */
+	{0x3705, 0xda}, /* analog pg 108 */
+	{0x3706, 0x41}, /* analog pg 108 */
+	{0x370a, 0x80}, /* analog pg 108 */
+	{0x370b, 0x40}, /* analog pg 108 */
+	{0x370e, 0x00}, /* analog pg 108 */
+	{0x3710, 0x28}, /* analog pg 108 */
+	{0x3712, 0x13}, /* analog pg 108 */
+	{0x3830, 0x50}, /* manual exposure gain bit [0] */
+	{0x3a18, 0x00}, /* AEC gain ceiling bit 8 pg 114 */
+	{0x3a19, 0xf8}, /* AEC gain ceiling pg 114 */
+	{0x3a00, 0x38}, /* AEC control 0 debug mode band low
+			   limit mode band func pg 112 */
+
+	{0x3603, 0xa7}, /* analog pg 108 */
+	{0x3615, 0x50}, /* analog pg 108 */
+	{0x3620, 0x56}, /* analog pg 108 */
+	{0x3810, 0x00}, /* TIMING HVOFFS both are zero pg 80 */
+	{0x3836, 0x00}, /* TIMING HVPAD both are zero pg 82 */
+	{0x3a1a, 0x06}, /* DIFF MAX an AEC register??? pg 114 */
+	{0x4000, 0x01}, /* BLC enabled pg 120 */
+	{0x401c, 0x48}, /* reserved pg 120 */
+	{0x401d, 0x28}, /* BLC control pg 120 */
+	{0x5000, 0x00}, /* ISP control00 features are disabled. pg 132 */
+	{0x5001, 0x00}, /* ISP control01 awb disabled. pg 132 */
+	{0x5002, 0x00}, /* ISP control02 debug mode disabled pg 132 */
+	{0x503d, 0x00}, /* ISP control3D features disabled pg 133 */
+	{0x5046, 0x00}, /* ISP control isp disable awbg disable pg 133 */
+
+	{0x300f, 0x8f}, /* PLL control00 R_SELD5 [7:6] div by 4 R_DIVL [2]
+			   two lane div 1 SELD2P5 [1:0] div 2.5 pg 99 */
+	{0x3010, 0x10}, /* PLL control01 DIVM [3:0] DIVS [7:4] div 1 pg 99 */
+	{0x3011, 0x14}, /* PLL control02 R_DIVP [5:0] div 20 pg 99 */
+	{0x3012, 0x02}, /* PLL CTR 03, default */
+	{0x3815, 0x82}, /* PCLK to SCLK ratio bit[4:0] is set to 2 pg 81 */
+	{0x3503, 0x33}, /* AEC auto AGC auto gain has no latch delay. pg 38 */
+	/*	{FAST_SETMODE_START, 0}, */
+	{0x3613, 0x44}, /* analog pg 108 */
+	{OV5650_TABLE_END, 0x0},
+};
+
+static struct ov5650_reg mode_2592x1944[] = {
+	{0x3621, 0x2f}, /* analog horizontal binning/sampling not enabled.
+			   pg 108 */
+	{0x3632, 0x55}, /* analog pg 108 */
+	{0x3703, 0xe6}, /* analog pg 108 */
+	{0x370c, 0xa0}, /* analog pg 108 */
+	{0x370d, 0x04}, /* analog pg 108 */
+	{0x3713, 0x2f}, /* analog pg 108 */
+	{0x3800, 0x02}, /* HREF start point higher 4 bits [3:0] pg 108 */
+	{0x3801, 0x58}, /* HREF start point lower  8 bits [7:0] pg 108 */
+	{0x3802, 0x00}, /* VREF start point higher 4 bits [3:0] pg 108 */
+	{0x3803, 0x0c}, /* VREF start point [7:0] pg 108 */
+	{0x3804, 0x0a}, /* HREF width  higher 4 bits [3:0] pg 108 */
+	{0x3805, 0x20}, /* HREF width  lower  8 bits [7:0] pg 108 */
+	{0x3806, 0x07}, /* VREF height higher 4 bits [3:0] pg 109 */
+	{0x3807, 0xa0}, /* VREF height lower  8 bits [7:0] pg 109 */
+	{0x3808, 0x0a}, /* DVP horizontal output size higher 4 bits [3:0]
+			   pg 109 */
+	{0x3809, 0x20}, /* DVP horizontal output size lower  8 bits [7:0]
+			   pg 109 */
+	{0x380a, 0x07}, /* DVP vertical   output size higher 4 bits [3:0]
+			   pg 109 */
+	{0x380b, 0xa0}, /* DVP vertical   output size lower  8 bits [7:0]
+			   pg 109 */
+	{0x380c, 0x0c}, /* total horizontal size higher 5 bits [4:0] pg 109,
+			   line length */
+	{0x380d, 0xb4}, /* total horizontal size lower  8 bits [7:0] pg 109,
+			   line length */
+	{0x380e, 0x07}, /* total vertical   size higher 5 bits [4:0] pg 109,
+			   frame length */
+	{0x380f, 0xb0}, /* total vertical   size lower  8 bits [7:0] pg 109,
+			   frame length */
+	{0x3818, 0xc0}, /* timing control reg18 mirror & dkhf pg 110 */
+	{0x381a, 0x3c}, /* HS mirror adjustment pg 110 */
+	{0x3a0d, 0x06}, /* b60 max pg 113 */
+	{0x3c01, 0x00}, /* 5060HZ_CTRL01 pg 116 */
+	{0x3007, 0x3f}, /* clock enable03 pg 98 */
+	{0x5059, 0x80}, /* => NOT found */
+	{0x3003, 0x03}, /* reset MIPI and DVP pg 97 */
+	{0x3500, 0x00}, /* long exp 1/3 in unit of 1/16 line, pg 38 */
+	{0x3501, 0x7a}, /* long exp 2/3 in unit of 1/16 line, pg 38,
+			   note frame length start with 0x7b0,
+			   and SENSOR_BAYER_DEFAULT_MAX_COARSE_DIFF=3 */
+	{0x3502, 0xd0}, /* long exp 3/3 in unit of 1/16 line, pg 38.
+			   Two lines of integration time. */
+	{0x350a, 0x00}, /* gain output to sensor, pg 38 */
+	{0x350b, 0x00}, /* gain output to sensor, pg 38 */
+	{0x4801, 0x0f}, /* MIPI control01 pg 125 */
+	{0x300e, 0x0c}, /* SC_MIPI_SC_CTRL0 pg 73 */
+	{0x4803, 0x50}, /* MIPI CTRL3 pg 91 */
+	{0x4800, 0x34}, /* MIPI CTRl0 idle and short line pg 89 */
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg mode_1296x972[] = {
+	{0x3621, 0xaf}, /* analog horizontal binning/sampling not enabled.
+			   pg 108 */
+	{0x3632, 0x5a}, /* analog pg 108 */
+	{0x3703, 0xb0}, /* analog pg 108 */
+	{0x370c, 0xc5}, /* analog pg 108 */
+	{0x370d, 0x42}, /* analog pg 108 */
+	{0x3713, 0x2f}, /* analog pg 108 */
+	{0x3800, 0x03}, /* HREF start point higher 4 bits [3:0] pg 108 */
+	{0x3801, 0x3c}, /* HREF start point lower  8 bits [7:0] pg 108 */
+	{0x3802, 0x00}, /* VREF start point higher 4 bits [3:0] pg 108 */
+	{0x3803, 0x06}, /* VREF start point [7:0] pg 108 */
+	{0x3804, 0x05}, /* HREF width  higher 4 bits [3:0] pg 108 */
+	{0x3805, 0x10}, /* HREF width  lower  8 bits [7:0] pg 108 */
+	{0x3806, 0x03}, /* VREF height higher 4 bits [3:0] pg 109 */
+	{0x3807, 0xd0}, /* VREF height lower  8 bits [7:0] pg 109 */
+	{0x3808, 0x05}, /* DVP horizontal output size higher 4 bits [3:0]
+			   pg 109 */
+	{0x3809, 0x10}, /* DVP horizontal output size lower  8 bits [7:0]
+			   pg 109 */
+	{0x380a, 0x03}, /* DVP vertical   output size higher 4 bits [3:0]
+			   pg 109 */
+	{0x380b, 0xd0}, /* DVP vertical   output size lower  8 bits [7:0]
+			   pg 109 */
+	{0x380c, 0x08}, /* total horizontal size higher 5 bits [4:0]
+			   pg 109, line length */
+	{0x380d, 0xa8}, /* total horizontal size lower  8 bits [7:0] pg 109,
+			   line length */
+	{0x380e, 0x05}, /* total vertical   size higher 5 bits [4:0] pg 109,
+			   frame length */
+	{0x380f, 0xa4}, /* total horizontal size lower  8 bits [7:0] pg 109,
+			   frame length */
+	{0x3818, 0xc1}, /* timing control reg18 mirror & dkhf pg 110 */
+	{0x381a, 0x00}, /* HS mirror adjustment pg 110 */
+	{0x3a0d, 0x08}, /* b60 max pg 113 */
+	{0x3c01, 0x00}, /* 5060HZ_CTRL01 pg 116 */
+	{0x3007, 0x3b}, /* clock enable03 pg 98 */
+	{0x5059, 0x80}, /* => NOT found. added */
+	{0x3003, 0x03}, /* reset MIPI and DVP pg 97 */
+	{0x3500, 0x00}, /* long exp 1/3 in unit of 1/16 line, pg 38,
+			   note frame length is from 0x5a4,
+			   and SENSOR_BAYER_DEFAULT_MAX_COARSE_DIFF=3 */
+	{0x3501, 0x5a}, /* long exp 2/3 in unit of 1/16 line, pg 38 */
+	{0x3502, 0x10}, /* long exp 3/3 in unit of 1/16 line, pg 38 */
+	{0x350a, 0x00}, /* gain output to sensor, pg 38 */
+	{0x350b, 0x10}, /* gain output to sensor, pg 38 */
+	{0x4801, 0x0f}, /* MIPI control01 pg 125 */
+	{0x300e, 0x0c}, /* SC_MIPI_SC_CTRL0 pg 73 */
+	{0x4803, 0x50}, /* MIPI CTRL3 pg 91 */
+	{0x4800, 0x34}, /* MIPI CTRl0 idle and short line pg 89 */
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg mode_2080x1164[] = {
+	{0x3103, 0x93}, // power up system clock from PLL page 77
+	{0x3007, 0x3b}, // clock enable03 pg 98
+	{0x3017, 0xff}, // PAD output enable page 100
+	{0x3018, 0xfc}, // PAD output enable page 100
+
+	{0x3600, 0x54}, // analog pg 108
+	{0x3601, 0x05}, // analog pg 108
+	{0x3603, 0xa7}, // analog pg 108
+	{0x3604, 0x40}, // analog pg 108
+	{0x3605, 0x04}, // analog pg 108
+	{0x3606, 0x3f}, // analog pg 108
+	{0x3612, 0x1a}, // analog pg 108
+	{0x3613, 0x44}, // analog pg 108
+	{0x3615, 0x52}, // analog pg 108
+	{0x3620, 0x56}, // analog pg 108
+	{0x3623, 0x01}, // analog pg 108
+	{0x3630, 0x22}, // analog pg 108
+	{0x3631, 0x36}, // analog pg 108
+	{0x3632, 0x5f}, // analog pg 108
+	{0x3633, 0x24}, // analog pg 108
+
+	{0x3702, 0x3a}, // analog pg 108
+	{0x3704, 0x18}, // analog pg 108
+	{0x3706, 0x41}, // analog pg 108
+	{0x370b, 0x40}, // analog pg 108
+	{0x370e, 0x00}, // analog pg 108
+	{0x3710, 0x28}, // analog pg 108
+	{0x3711, 0x24},
+	{0x3712, 0x13}, // analog pg 108
+
+	{0x3810, 0x00}, // TIMING HVOFFS both are zero pg 80
+	{0x3815, 0x82}, // PCLK to SCLK ratio bit[4:0] is set to 2 pg 81
+	{0x3830, 0x50}, // manual exposure gain bit [0]
+	{0x3836, 0x00}, // TIMING HVPAD both are zero pg 82
+
+	{0x3a1a, 0x06}, // DIFF MAX an AEC register??? pg 114
+	{0x3a18, 0x00}, // AEC gain ceiling bit 8 pg 114
+	{0x3a19, 0xf8}, // AEC gain ceiling pg 114
+	{0x3a00, 0x38}, // AEC control 0 debug mode band low limit mode band func pg 112
+	{0x3a0d, 0x06}, // b60 max pg 113
+	{0x3c01, 0x34}, // 5060HZ_CTRL01 pg 116
+
+	{0x401f, 0x03}, // BLC enabled pg 120
+	{0x4000, 0x05}, // BLC enabled pg 120
+	{0x401d, 0x08}, // reserved pg 120
+	{0x4001, 0x02}, // BLC control pg 120
+
+	{0x5000, 0x00}, // ISP control00 features are disabled. pg 132
+	{0x5001, 0x00}, // ISP control01 awb disabled. pg 132
+	{0x5002, 0x00}, // ISP control02 debug mode disabled pg 132
+	{0x503d, 0x00}, // ISP control3D features disabled pg 133
+	{0x5046, 0x00}, // ISP control isp disable awbg disable pg 133
+
+	{0x300f, 0x8f}, // PLL control00 R_SELD5 [7:6] div by 4 R_DIVL [2] two lane div 1 SELD2P5 [1:0] div 2.5 pg 99
+	{0x3010, 0x10}, // PLL control01 DIVM [3:0] DIVS [7:4] div 1 pg 99
+	{0x3011, 0x14}, // PLL control02 R_DIVP [5:0] div 20 pg 99
+	{0x3012, 0x02}, // PLL CTR 03, default
+	{0x3503, 0x33}, // AEC auto AGC auto gain has delay of 2 frames. pg 38
+
+	{0x3621, 0x2f}, // analog horizontal binning/sampling not enabled. pg 108
+	{0x3703, 0xe6}, // analog pg 108
+	{0x370c, 0x00}, // analog pg 108
+	{0x370d, 0x04}, // analog pg 108
+	{0x3713, 0x22}, // analog pg 108
+	{0x3714, 0x27},
+	{0x3705, 0xda},
+	{0x370a, 0x80},
+
+	{0x3800, 0x02}, // HREF start point higher 4 bits [3:0] pg 108
+	{0x3801, 0x12}, // HREF start point lower  8 bits [7:0] pg 108
+	{0x3802, 0x00}, // VREF start point higher 4 bits [3:0] pg 108
+	{0x3803, 0x0a}, // VREF start point [7:0] pg 108
+	{0x3804, 0x08}, // HREF width  higher 4 bits [3:0] pg 108
+	{0x3805, 0x20}, // HREF width  lower  8 bits [7:0] pg 108
+	{0x3806, 0x04}, // VREF height higher 4 bits [3:0] pg 109
+	{0x3807, 0x92}, // VREF height lower  8 bits [7:0] pg 109
+	{0x3808, 0x08}, // DVP horizontal output size higher 4 bits [3:0] pg 109
+	{0x3809, 0x20}, // DVP horizontal output size lower  8 bits [7:0] pg 109
+	{0x380a, 0x04}, // DVP vertical   output size higher 4 bits [3:0] pg 109
+	{0x380b, 0x92}, // DVP vertical   output size lower  8 bits [7:0] pg 109
+	{0x380c, 0x0a}, // total horizontal size higher 5 bits [4:0] pg 109, line length
+	{0x380d, 0x96}, // total horizontal size lower  8 bits [7:0] pg 109, line length
+	{0x380e, 0x04}, // total vertical   size higher 5 bits [4:0] pg 109, frame length
+	{0x380f, 0x9e}, // total vertical   size lower  8 bits [7:0] pg 109, frame length
+	{0x3818, 0xc0}, // timing control reg18 mirror & dkhf pg 110
+	{0x381a, 0x3c}, // HS mirror adjustment pg 110
+	{0x381c, 0x31},
+	{0x381d, 0x8e},
+	{0x381e, 0x04},
+	{0x381f, 0x92},
+	{0x3820, 0x04},
+	{0x3821, 0x19},
+	{0x3824, 0x01},
+	{0x3827, 0x0a},
+	{0x401c, 0x46},
+
+	{0x3003, 0x03}, // reset MIPI and DVP pg 97
+	{0x3500, 0x00}, // long exp 1/3 in unit of 1/16 line, pg 38
+	{0x3501, 0x49}, // long exp 2/3 in unit of 1/16 line, pg 38
+	{0x3502, 0xa0}, // long exp 3/3 in unit of 1/16 line, pg 38
+	{0x350a, 0x00}, // gain output to sensor, pg 38
+	{0x350b, 0x00}, // gain output to sensor, pg 38
+	{0x4801, 0x0f}, // MIPI control01 pg 125
+	{0x300e, 0x0c}, // SC_MIPI_SC_CTRL0 pg 73
+	{0x4803, 0x50}, // MIPI CTRL3 pg 91
+	{0x4800, 0x34}, // MIPI CTRl0 idle and short line pg 89
+
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg mode_1264x704[] = {
+	{0x3600, 0x54}, /* analog pg 108 */
+	{0x3601, 0x05}, /* analog pg 108 */
+	{0x3604, 0x40}, /* analog pg 108 */
+	{0x3705, 0xdb}, /* analog pg 108 */
+	{0x370a, 0x81}, /* analog pg 108 */
+	{0x3615, 0x52}, /* analog pg 108 */
+	{0x3810, 0x40}, /* TIMING HVOFFS both are zero pg 80 */
+	{0x3836, 0x41}, /* TIMING HVPAD both are zero pg 82 */
+	{0x4000, 0x05}, /* BLC enabled pg 120 */
+	{0x401c, 0x42}, /* reserved pg 120 */
+	{0x5046, 0x09}, /* ISP control isp disable awbg disable pg 133 */
+	{0x3010, 0x00}, /* PLL control01 DIVM [3:0] DIVS [7:4] div 1 pg 99 */
+	{0x3503, 0x00}, /* AEC auto AGC auto gain has no latch delay. pg 38 */
+	{0x3613, 0xc4}, /* analog pg 108 */
+
+	{0x3621, 0xaf}, /* analog horizontal binning/sampling not enabled.
+			   pg 108 */
+	{0x3632, 0x55}, /* analog pg 108 */
+	{0x3703, 0x9a}, /* analog pg 108 */
+	{0x370c, 0x00}, /* analog pg 108 */
+	{0x370d, 0x42}, /* analog pg 108 */
+	{0x3713, 0x22}, /* analog pg 108 */
+	{0x3800, 0x02}, /* HREF start point higher 4 bits [3:0] pg 108 */
+	{0x3801, 0x54}, /* HREF start point lower  8 bits [7:0] pg 108 */
+	{0x3802, 0x00}, /* VREF start point higher 4 bits [3:0] pg 108 */
+	{0x3803, 0x0c}, /* VREF start point [7:0] pg 108 */
+	{0x3804, 0x05}, /* HREF width  higher 4 bits [3:0] pg 108 */
+	{0x3805, 0x00}, /* HREF width  lower  8 bits [7:0] pg 108 */
+	{0x3806, 0x02}, /* VREF height higher 4 bits [3:0] pg 109 */
+	{0x3807, 0xd0}, /* VREF height lower  8 bits [7:0] pg 109 */
+	{0x3808, 0x05}, /* DVP horizontal output size higher 4 bits [3:0]
+			   pg 109 */
+	{0x3809, 0x00}, /* DVP horizontal output size lower  8 bits [7:0]
+			   pg 109 */
+	{0x380a, 0x02}, /* DVP vertical   output size higher 4 bits [3:0]
+			   pg 109 */
+	{0x380b, 0xd0}, /* DVP vertical   output size lower  8 bits [7:0]
+			   pg 109 */
+	{0x380c, 0x08}, /* total horizontal size higher 5 bits [4:0] pg 109,
+			   line length */
+	{0x380d, 0x72}, /* total horizontal size lower  8 bits [7:0] pg 109,
+			   line length */
+	{0x380e, 0x02}, /* total vertical size higher 5 bits [4:0] pg 109,
+			   frame length */
+	{0x380f, 0xe4}, /* total vertical size lower  8 bits [7:0] pg 109,
+			   frame length */
+	{0x3818, 0xc1}, /* timing control reg18 mirror & dkhf pg 110 */
+	{0x381a, 0x3c}, /* HS mirror adjustment pg 110 */
+	{0x3a0d, 0x06}, /* b60 max pg 113 */
+	{0x3c01, 0x34}, /* 5060HZ_CTRL01 pg 116 */
+	{0x3007, 0x3b}, /* clock enable03 pg 98 */
+	{0x5059, 0x80}, /* => NOT found */
+	{0x3003, 0x03}, /* reset MIPI and DVP pg 97 */
+	{0x3500, 0x04}, /* long exp 1/3 in unit of 1/16 line, pg 38 */
+	{0x3501, 0xa5}, /* long exp 2/3 in unit of 1/16 line, pg 38,
+			   note frame length start with 0x7b0,
+			   and SENSOR_BAYER_DEFAULT_MAX_COARSE_DIFF=3 */
+	{0x3502, 0x10}, /* long exp 3/3 in unit of 1/16 line, pg 38.
+			   Two lines of integration time. */
+	{0x350a, 0x00}, /* gain output to sensor, pg 38 */
+	{0x350b, 0x00}, /* gain output to sensor, pg 38 */
+	{0x4801, 0x0f}, /* MIPI control01 pg 125 */
+	{0x300e, 0x0c}, /* SC_MIPI_SC_CTRL0 pg 73 */
+	{0x4803, 0x50}, /* MIPI CTRL3 pg 91 */
+	{0x4800, 0x24}, /* MIPI CTRl0 idle and short line pg 89 */
+	{0x300f, 0x8b}, /* PLL control00 R_SELD5 [7:6] div by 4 R_DIVL [2]
+			   two lane div 1 SELD2P5 [1:0] div 2.5 pg 99 */
+
+	{0x3711, 0x24},
+	{0x3713, 0x92},
+	{0x3714, 0x17},
+	{0x381c, 0x10},
+	{0x381d, 0x82},
+	{0x381e, 0x05},
+	{0x381f, 0xc0},
+	{0x3821, 0x20},
+	{0x3824, 0x23},
+	{0x3825, 0x2c},
+	{0x3826, 0x00},
+	{0x3827, 0x0c},
+	{0x3623, 0x01},
+	{0x3633, 0x24},
+	{0x3632, 0x5f},
+	{0x401f, 0x03},
+
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg mode_end[] = {
+	{0x3212, 0x00}, /* SRM_GROUP_ACCESS (group hold begin) */
+	{0x3003, 0x01}, /* reset DVP pg 97 */
+	{0x3212, 0x10}, /* SRM_GROUP_ACCESS (group hold end) */
+	{0x3212, 0xa0}, /* SRM_GROUP_ACCESS (group hold launch) */
+	{0x3008, 0x02}, /* SYSTEM_CTRL0 mipi suspend mask pg 98 */
+
+	/*	{FAST_SETMODE_END, 0}, */
+	{OV5650_TABLE_END, 0x0000}
+};
+
+enum {
+	OV5650_MODE_2592x1944,
+	OV5650_MODE_1296x972,
+	OV5650_MODE_2080x1164,
+	OV5650_MODE_1264x704
+};
+
+static struct ov5650_reg *mode_table[] = {
+	[OV5650_MODE_2592x1944] = mode_2592x1944,
+	[OV5650_MODE_1296x972] = mode_1296x972,
+	[OV5650_MODE_2080x1164] = mode_2080x1164,
+	[OV5650_MODE_1264x704] = mode_1264x704
+};
+
+/* 2 regs to program frame length */
+static inline void ov5650_get_frame_length_regs(struct ov5650_reg *regs,
+						u32 frame_length)
+{
+	regs->addr = 0x380e;
+	regs->val = (frame_length >> 8) & 0xff;
+	(regs + 1)->addr = 0x380f;
+	(regs + 1)->val = (frame_length) & 0xff;
+}
+
+/* 3 regs to program coarse time */
+static inline void ov5650_get_coarse_time_regs(struct ov5650_reg *regs,
+                                               u32 coarse_time)
+{
+	regs->addr = 0x3500;
+	regs->val = (coarse_time >> 12) & 0xff;
+	(regs + 1)->addr = 0x3501;
+	(regs + 1)->val = (coarse_time >> 4) & 0xff;
+	(regs + 2)->addr = 0x3502;
+	(regs + 2)->val = (coarse_time & 0xf) << 4;
+}
+
+/* 1 reg to program gain */
+static inline void ov5650_get_gain_reg(struct ov5650_reg *regs, u16 gain)
+{
+	regs->addr = 0x350b;
+	regs->val = gain;
+}
+
+static int ov5650_write_reg(struct i2c_client *client, u16 addr, u8 val)
+{
+	int err;
+	struct i2c_msg msg;
+	unsigned char data[3];
+	int retry = 0;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data[0] = (u8) (addr >> 8);;
+	data[1] = (u8) (addr & 0xff);
+	data[2] = (u8) (val & 0xff);
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 3;
+	msg.buf = data;
+
+	do {
+		err = i2c_transfer(client->adapter, &msg, 1);
+		if (err == 1)
+			return 0;
+		retry++;
+		pr_err("ov5650: i2c transfer failed, retrying %x %x\n",
+			addr, val);
+		msleep(3);
+	} while (retry <= OV5650_MAX_RETRIES);
+
+	return err;
+}
+
+static int ov5650_write_reg_helper(struct ov5650_info *info,
+					u16 addr, u8 val)
+{
+	int ret;
+	switch (info->camera_mode) {
+	case Main:
+	case LeftOnly:
+		ret = ov5650_write_reg(info->left.i2c_client, addr, val);
+		break;
+	case Stereo:
+		ret = ov5650_write_reg(info->left.i2c_client, addr, val);
+		if (ret)
+			break;
+		ret = ov5650_write_reg(info->right.i2c_client, addr, val);
+		break;
+	case RightOnly:
+		ret = ov5650_write_reg(info->right.i2c_client, addr, val);
+		break;
+	default:
+		return -1;
+	}
+	return ret;
+}
+
+static int ov5650_write_table(struct ov5650_info *info,
+				const struct ov5650_reg table[],
+				const struct ov5650_reg override_list[],
+				int num_override_regs)
+{
+	int err;
+	const struct ov5650_reg *next;
+	int i;
+	u16 val;
+
+	for (next = table; next->addr != OV5650_TABLE_END; next++) {
+		if (next->addr == OV5650_TABLE_WAIT_MS) {
+			msleep(next->val);
+			continue;
+		}
+
+		val = next->val;
+
+		/* When an override list is passed in, replace the reg */
+		/* value to write if the reg is in the list            */
+		if (override_list) {
+			for (i = 0; i < num_override_regs; i++) {
+				if (next->addr == override_list[i].addr) {
+					val = override_list[i].val;
+					break;
+				}
+			}
+		}
+
+		err = ov5650_write_reg_helper(info, next->addr, val);
+		if (err)
+			return err;
+	}
+	return 0;
+}
+
+static int ov5650_set_mode(struct ov5650_info *info, struct ov5650_mode *mode)
+{
+	int sensor_mode;
+	int err;
+	struct ov5650_reg reg_list[6];
+
+	pr_info("%s: xres %u yres %u framelength %u coarsetime %u gain %u\n",
+		__func__, mode->xres, mode->yres, mode->frame_length,
+		mode->coarse_time, mode->gain);
+	if (mode->xres == 2592 && mode->yres == 1944)
+		sensor_mode = OV5650_MODE_2592x1944;
+	else if (mode->xres == 1296 && mode->yres == 972)
+		sensor_mode = OV5650_MODE_1296x972;
+	else if (mode->xres == 2080 && mode->yres == 1164)
+		sensor_mode = OV5650_MODE_2080x1164;
+	else if (mode->xres == 1264 && mode->yres == 704)
+		sensor_mode = OV5650_MODE_1264x704;
+	else {
+		pr_err("%s: invalid resolution supplied to set mode %d %d\n",
+		       __func__, mode->xres, mode->yres);
+		return -EINVAL;
+	}
+
+	/* get a list of override regs for the asking frame length, */
+	/* coarse integration time, and gain.                       */
+	ov5650_get_frame_length_regs(reg_list, mode->frame_length);
+	ov5650_get_coarse_time_regs(reg_list + 2, mode->coarse_time);
+	ov5650_get_gain_reg(reg_list + 5, mode->gain);
+
+	err = ov5650_write_table(info, reset_seq, NULL, 0);
+	if (err)
+		return err;
+
+	err = ov5650_write_table(info, mode_start, NULL, 0);
+	if (err)
+		return err;
+
+	err = ov5650_write_table(info, mode_table[sensor_mode],
+		reg_list, 6);
+	if (err)
+		return err;
+
+	err = ov5650_write_table(info, mode_end, NULL, 0);
+	if (err)
+		return err;
+
+	info->mode = sensor_mode;
+	return 0;
+}
+
+static int ov5650_set_frame_length(struct ov5650_info *info, u32 frame_length)
+{
+	struct ov5650_reg reg_list[2];
+	int i = 0;
+	int ret;
+
+	ov5650_get_frame_length_regs(reg_list, frame_length);
+
+	for (i = 0; i < 2; i++)	{
+		ret = ov5650_write_reg_helper(info, reg_list[i].addr,
+			reg_list[i].val);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int ov5650_set_coarse_time(struct ov5650_info *info, u32 coarse_time)
+{
+	int ret;
+
+	struct ov5650_reg reg_list[3];
+	int i = 0;
+
+	ov5650_get_coarse_time_regs(reg_list, coarse_time);
+
+	ret = ov5650_write_reg_helper(info, 0x3212, 0x01);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < 3; i++)	{
+		ret = ov5650_write_reg_helper(info, reg_list[i].addr,
+			reg_list[i].val);
+		if (ret)
+			return ret;
+	}
+
+	ret = ov5650_write_reg_helper(info, 0x3212, 0x11);
+	if (ret)
+		return ret;
+
+	ret = ov5650_write_reg_helper(info, 0x3212, 0xa1);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int ov5650_set_gain(struct ov5650_info *info, u16 gain)
+{
+	int ret;
+	struct ov5650_reg reg_list;
+
+	ov5650_get_gain_reg(&reg_list, gain);
+
+	ret = ov5650_write_reg_helper(info, reg_list.addr, reg_list.val);
+
+	return ret;
+}
+
+static int ov5650_test_pattern(struct ov5650_info *info,
+			       enum ov5650_test_pattern pattern)
+{
+	if (pattern >= ARRAY_SIZE(test_pattern_modes))
+		return -EINVAL;
+
+	return ov5650_write_table(info,
+				  test_pattern_modes[pattern],
+				  NULL, 0);
+}
+
+static int ov5650_set_power(int powerLevel)
+{
+	pr_info("%s: powerLevel=%d camera mode=%d\n", __func__, powerLevel,
+			info->camera_mode);
+
+	switch (info->camera_mode) {
+	case Main:
+	case LeftOnly:
+		if (info->left.pdata) {
+			if (powerLevel && info->left.pdata->power_on)
+				info->left.pdata->power_on();
+			else if (info->left.pdata->power_off)
+				info->left.pdata->power_off();
+		}
+		break;
+
+	case Stereo:
+		if (info->left.pdata) {
+			if (powerLevel && info->left.pdata->power_on)
+				info->left.pdata->power_on();
+			else if (info->left.pdata->power_off)
+				info->left.pdata->power_off();
+		}
+		if (info->right.pdata) {
+			if (powerLevel && info->right.pdata->power_on)
+				info->right.pdata->power_on();
+			else if (info->right.pdata->power_off)
+				info->right.pdata->power_off();
+		}
+		break;
+
+	case RightOnly:
+		if (info->right.pdata) {
+			if (powerLevel && info->right.pdata->power_on)
+				info->right.pdata->power_on();
+			else if (info->right.pdata->power_off)
+				info->right.pdata->power_off();
+		}
+		break;
+
+	default:
+		return -1;
+	}
+	return 0;
+}
+
+static long ov5650_ioctl(struct file *file,
+			 unsigned int cmd, unsigned long arg)
+{
+	int err;
+	struct ov5650_info *info = file->private_data;
+
+	switch (cmd) {
+	case OV5650_IOCTL_SET_CAMERA_MODE:
+	{
+		if (info->camera_mode != arg) {
+			err = ov5650_set_power(0);
+			if (err) {
+				pr_info("%s %d\n", __func__, __LINE__);
+				return err;
+			}
+			info->camera_mode = arg;
+			err = ov5650_set_power(1);
+			if (err)
+				return err;
+		}
+		return 0;
+	}
+	case OV5650_IOCTL_SYNC_SENSORS:
+		if (info->right.pdata->synchronize_sensors)
+			info->right.pdata->synchronize_sensors();
+		return 0;
+	case OV5650_IOCTL_SET_MODE:
+	{
+		struct ov5650_mode mode;
+		if (copy_from_user(&mode,
+				   (const void __user *)arg,
+				   sizeof(struct ov5650_mode))) {
+			pr_info("%s %d\n", __func__, __LINE__);
+			return -EFAULT;
+		}
+
+		return ov5650_set_mode(info, &mode);
+	}
+	case OV5650_IOCTL_SET_FRAME_LENGTH:
+		return ov5650_set_frame_length(info, (u32)arg);
+	case OV5650_IOCTL_SET_COARSE_TIME:
+		return ov5650_set_coarse_time(info, (u32)arg);
+	case OV5650_IOCTL_SET_GAIN:
+		return ov5650_set_gain(info, (u16)arg);
+	case OV5650_IOCTL_GET_STATUS:
+	{
+		u16 status = 0;
+		if (copy_to_user((void __user *)arg, &status,
+				 2)) {
+			pr_info("%s %d\n", __func__, __LINE__);
+			return -EFAULT;
+		}
+		return 0;
+	}
+	case OV5650_IOCTL_TEST_PATTERN:
+	{
+		err = ov5650_test_pattern(info, (enum ov5650_test_pattern) arg);
+		if (err)
+			pr_err("%s %d %d\n", __func__, __LINE__, err);
+		return err;
+	}
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int ov5650_open(struct inode *inode, struct file *file)
+{
+	pr_info("%s\n", __func__);
+	file->private_data = info;
+	ov5650_set_power(1);
+	return 0;
+}
+
+int ov5650_release(struct inode *inode, struct file *file)
+{
+	ov5650_set_power(0);
+	file->private_data = NULL;
+	return 0;
+}
+
+
+static const struct file_operations ov5650_fileops = {
+	.owner = THIS_MODULE,
+	.open = ov5650_open,
+	.unlocked_ioctl = ov5650_ioctl,
+	.release = ov5650_release,
+};
+
+static struct miscdevice ov5650_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "ov5650",
+	.fops = &ov5650_fileops,
+};
+
+static int left_ov5650_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int err;
+	pr_info("%s: probing sensor.\n", __func__);
+
+	if (!info) {
+		info = kzalloc(sizeof(struct ov5650_info), GFP_KERNEL);
+		if (!info) {
+			pr_err("ov5650: Unable to allocate memory!\n");
+			return -ENOMEM;
+		}
+	}
+
+	err = misc_register(&ov5650_device);
+	if (err) {
+		pr_err("ov5650: Unable to register misc device!\n");
+		kfree(info);
+		return err;
+	}
+
+	info->left.pdata = client->dev.platform_data;
+	info->left.i2c_client = client;
+
+	return 0;
+}
+
+static int left_ov5650_remove(struct i2c_client *client)
+{
+	misc_deregister(&ov5650_device);
+	kfree(info);
+	return 0;
+}
+
+static const struct i2c_device_id left_ov5650_id[] = {
+	{ "ov5650", 0 },
+	{ "ov5650L", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, left_ov5650_id);
+
+static struct i2c_driver left_ov5650_i2c_driver = {
+	.driver = {
+		.name = "ov5650",
+		.owner = THIS_MODULE,
+	},
+	.probe = left_ov5650_probe,
+	.remove = left_ov5650_remove,
+	.id_table = left_ov5650_id,
+};
+
+static int right_ov5650_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	pr_info("%s: probing sensor.\n", __func__);
+	if (!info) {
+		info = kzalloc(sizeof(struct ov5650_info), GFP_KERNEL);
+		if (!info) {
+			pr_err("ov5650_right: Unable to allocate memory!\n");
+			return -ENOMEM;
+		}
+	}
+
+	info->right.pdata = client->dev.platform_data;
+	info->right.i2c_client = client;
+
+	return 0;
+}
+
+static int right_ov5650_remove(struct i2c_client *client)
+{
+	return 0;
+}
+
+static const struct i2c_device_id right_ov5650_id[] = {
+	{ "ov5650R", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, right_ov5650_id);
+
+static struct i2c_driver right_ov5650_i2c_driver = {
+	.driver = {
+		.name = "ov5650R",
+		.owner = THIS_MODULE,
+	},
+	.probe = right_ov5650_probe,
+	.remove = right_ov5650_remove,
+	.id_table = right_ov5650_id,
+};
+
+static int __init ov5650_init(void)
+{
+	int ret;
+	pr_info("ov5650 sensor driver loading\n");
+	ret = i2c_add_driver(&left_ov5650_i2c_driver);
+	if (ret)
+		return ret;
+	return i2c_add_driver(&right_ov5650_i2c_driver);
+}
+
+static void __exit ov5650_exit(void)
+{
+	i2c_del_driver(&right_ov5650_i2c_driver);
+	i2c_del_driver(&left_ov5650_i2c_driver);
+}
+
+module_init(ov5650_init);
+module_exit(ov5650_exit);
+
diff --git a/drivers/media/video/tegra/sh532u.c b/drivers/media/video/tegra/sh532u.c
new file mode 100644
index 000000000000..7a707dff21e8
--- /dev/null
+++ b/drivers/media/video/tegra/sh532u.c
@@ -0,0 +1,642 @@
+/*
+ * SH532U focuser driver.
+ *
+ * Copyright (C) 2011 NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
+ * 02111-1307, USA
+ */
+
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <media/sh532u.h>
+
+#include <asm/traps.h>
+
+#define POS_LOW (0xA000)
+#define POS_HIGH (0x6000)
+#define SETTLETIME_MS (7)
+#define FOCAL_LENGTH 0x408d70a4 /* (4.42f) */
+#define FNUMBER 0x40333333 /* (2.8f) */
+
+
+struct sh532u_info {
+	struct i2c_client *i2c_client;
+	struct sh532u_config config;
+	struct sh532u_platform_data sh532u_pdata;
+};
+
+static struct sh532u_info *info;
+
+static int sh532u_read_u8(u8 dev, u8 addr, u8 *val)
+{
+	struct i2c_client *client = info->i2c_client;
+	struct i2c_msg msg[2];
+	unsigned char data[3];
+
+	if (dev)
+		msg[0].addr = dev;
+	else
+		msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = data;
+
+	data[0] = (u8)addr;
+
+	if (dev)
+		msg[1].addr = dev;
+	else
+		msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 1;
+	msg[1].buf = data + 2;
+
+	if (i2c_transfer(client->adapter, msg, 2) != 2)
+		return -1;
+	*val = data[2];
+	return 0;
+}
+
+static int sh532u_read_u16(u8 addr, u16 *val)
+{
+	struct i2c_client *client = info->i2c_client;
+	struct i2c_msg msg[2];
+	u8 buf[4];
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = &buf[0];
+
+	/* high byte goes out first */
+	buf[0] = (u8) (addr);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 2;
+	msg[1].buf = &buf[1];
+
+	if (i2c_transfer(client->adapter, msg, 2) != 2)
+		return -1;
+	*val = (((u16)buf[1] << 8) | (u16)buf[2]);
+	return 0;
+}
+
+static int eeprom_read_u32(u8 addr, u32 *val)
+{
+	struct i2c_client *client = info->i2c_client;
+	struct i2c_msg msg[2];
+	union {
+		u8   dataU8[8];
+		u32  dataU32[2];
+	} buffer;
+
+	msg[0].addr = 0x50;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = &(buffer.dataU8[0]);
+
+	/* high byte goes out first */
+	buffer.dataU8[0] = (u8) (addr);
+	buffer.dataU8[1] = (u8) (0);
+
+	msg[1].addr = 0x50;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 4;
+	msg[1].buf = (u8 *)&(buffer.dataU32[1]);
+
+	if (i2c_transfer(client->adapter, msg, 2) != 2)
+		return -1;
+	*val = buffer.dataU32[1];
+	return 0;
+}
+
+static int sh532u_write_u8(u16 addr, u8 val)
+{
+	struct i2c_client *client = info->i2c_client;
+	struct i2c_msg msg;
+	unsigned char data[2];
+
+	data[0] = (u8) (addr & 0xff);
+	data[1] = (u8) (val & 0xff);
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 2;
+	msg.buf = data;
+
+	if (i2c_transfer(client->adapter, &msg, 1) != 1)
+		return -1;
+
+	return 0;
+}
+
+static int sh532u_write_u16(u16 addr, u16 val)
+{
+	struct i2c_client *client = info->i2c_client;
+	struct i2c_msg msg;
+	unsigned char data[3];
+
+	data[0] = (u8) (addr & 0xff);
+	data[1] = (u8) (val >> 8);
+	data[2] = (u8) (val & 0xff);
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 3;
+	msg.buf = data;
+
+	if (i2c_transfer(client->adapter, &msg, 1) != 1)
+		return -1;
+	return 0;
+}
+
+static void move_driver(s16 tarPos)
+{
+	s16 curPos, moveStep;
+	u16 moveDistance;
+	int err;
+
+	/* Read Current Position */
+	err = sh532u_read_u16(RZ_211H, &curPos);
+	if (err)
+		goto move_driver_error;
+	/* Check move distance to Target Position */
+	moveDistance = abs((int)curPos - (int)tarPos);
+
+	/* if move distance is shorter than MS1Z12(=Step width) */
+	if (moveDistance <= STMV_SIZE) {
+		err = sh532u_write_u8(MSSET_211, (INI_MSSET_211 | 0x01));
+		err = err | sh532u_write_u16(MS1Z22_211H, tarPos);
+		if (err)
+			goto move_driver_error;
+	} else {
+		if (curPos < tarPos)
+			moveStep = STMV_SIZE;
+		else
+			moveStep = -STMV_SIZE;
+
+		/* Set StepMove Target Positon */
+		err = sh532u_write_u16(MS1Z12_211H, moveStep);
+		err = err | sh532u_write_u16(STMVENDH_211, tarPos);
+		/* Start StepMove */
+		err = err |
+		      sh532u_write_u8(
+			STMVEN_211,
+			(STMCHTG_ON | STMSV_ON | STMLFF_OFF | STMVEN_ON));
+		if (err)
+			goto move_driver_error;
+	}
+
+	return;
+move_driver_error:
+	pr_err("Focuser: %s failed!\n", __func__);
+}
+
+static void wait_for_move(void)
+{
+	u16 usSmvFin;
+	u8 moveTime, ucParMod, tmp;
+	int err;
+
+	moveTime = 0;
+	do {
+		mdelay(1);
+		err = sh532u_read_u8(0, STMVEN_211, &ucParMod);
+		err = err | sh532u_read_u16(RZ_211H, &usSmvFin);
+		if (err)
+			goto wait_for_move_error;
+		/* StepMove Error Handling, Unexpected Position */
+		if ((usSmvFin == 0x7FFF) || (usSmvFin == 0x8001)) {
+			/* Stop StepMove Operation */
+			err = sh532u_write_u8(STMVEN_211, ucParMod & 0xFE);
+			if (err)
+				goto wait_for_move_error;
+		}
+		moveTime++;
+		/* Wait StepMove operation end */
+	} while ((ucParMod & STMVEN_ON) && (moveTime < 50));
+
+	moveTime = 0;
+	if ((ucParMod & 0x08) == STMCHTG_ON) {
+		mdelay(5);
+		do {
+			mdelay(1);
+			moveTime++;
+			err = sh532u_read_u8(0, MSSET_211, &tmp);
+			if (err)
+				goto wait_for_move_error;
+		} while ((tmp & CHTGST_ON) && (moveTime < 15));
+	}
+
+	return;
+wait_for_move_error:
+	pr_err("Focuser: %s failed!\n", __func__);
+}
+
+static void lens_move_pulse(s16 position)
+{
+	move_driver(position);
+	wait_for_move();
+}
+
+static void get_rom_info(void)
+{
+	u8 tmp;
+	int err;
+
+	/* Get Inf1, Mac1
+	Inf1 and Mac1 are the mechanical limit position.
+	Inf1     : Bottom limit.
+	Mac1 : Top limit. */
+	err = sh532u_read_u8(0x50, addrMac1, &tmp);
+	if (err)
+		goto get_rom_info_error;
+	info->config.limit_low = (tmp<<8) & 0xff00;
+	err = sh532u_read_u8(0x50, addrInf1, &tmp);
+	if (err)
+		goto get_rom_info_error;
+	info->config.limit_high = (tmp<<8) & 0xff00;
+
+	/* Get Inf2, Mac2
+	Inf2 and Mac2 are the calibration data for SEMCO AF lens.
+	Inf2: Best focus (lens position) when object distance is 1.2M.
+	Mac2: Best focus (lens position) when object distance is 10cm. */
+	err = sh532u_read_u8(0x50, addrMac2, &tmp);
+	if (err)
+		goto get_rom_info_error;
+	info->config.pos_low = (tmp << 8) & 0xff00;
+	err = sh532u_read_u8(0x50, addrInf2, &tmp);
+	if (err)
+		goto get_rom_info_error;
+	info->config.pos_high = (tmp << 8) & 0xff00;
+
+	return;
+get_rom_info_error:
+	pr_err("Focuser: %s failed!\n", __func__);
+	info->config.limit_high = POS_HIGH;
+	info->config.limit_low = POS_LOW;
+	info->config.pos_high = POS_HIGH;
+	info->config.pos_low = POS_LOW;
+}
+
+static void init_hvca_pos(void)
+{
+	short sBottomLimit, sTopLimit;
+
+	get_rom_info();
+	sBottomLimit = (((int)info->config.limit_low * 5) >> 3) & 0xFFC0;
+	lens_move_pulse(sBottomLimit);
+	sTopLimit = (((int)info->config.limit_high * 5) >> 3) & 0xFFC0;
+	lens_move_pulse(sTopLimit);
+	lens_move_pulse(info->config.pos_high);
+}
+
+static unsigned int a2buf[] = {
+	0x0018019c,
+	0x0018019d,
+	0x0000019e,
+	0x007f0192,
+	0x00000194,
+	0x00f00184,
+	0x00850187,
+	0x0000018a,
+	0x00fd7187,
+	0x007f7183,
+	0x0008025a,
+	0x05042218,
+	0x80010216,
+	0x000601a0,
+	0x00808183,
+	0xffffffff
+};
+
+/* Write 1 byte data to the HVCA Drive IC by data type */
+static void sh532u_hvca_wr1(u8 ep_type, u8 ep_data1, u8 ep_addr)
+{
+	int err = 0;
+	u8 us_data;
+
+	switch (ep_type & 0xF0) {
+	case DIRECT_MODE:
+		us_data = ep_data1;
+		break;
+
+	case INDIRECT_EEPROM:
+		err = sh532u_read_u8(0x50, ep_data1, &us_data);
+		break;
+
+	case INDIRECT_HVCA:
+		err = sh532u_read_u8(0, (u16)ep_data1, &us_data);
+		break;
+
+	case MASK_AND:
+		err = sh532u_read_u8(0, (u16)ep_addr, &us_data);
+		us_data = us_data & ep_data1;
+		break;
+
+	case MASK_OR:
+		err = sh532u_read_u8(0, (u16)ep_addr, &us_data);
+		us_data = us_data | ep_data1;
+		break;
+
+	default:
+		err = 1;
+	}
+	if (!err)
+		err = sh532u_write_u8((u16)ep_addr, us_data);
+	if (err)
+		pr_err("Focuser: Failed to init!\n");
+}
+
+/* Write 2 byte data to the HVCA Drive IC by data type */
+static void sh532u_hvca_wr2(u8 ep_type, u8 ep_data1, u8 ep_data2, u8 ep_addr)
+{
+	int err = 0;
+	u8 uc_data1;
+	u8 uc_data2;
+	u16 us_data;
+
+	switch (ep_type & 0xF0) {
+	case DIRECT_MODE:
+		us_data = (((u16)ep_data1 << 8) & 0xFF00) |
+			((u16)ep_data2 & 0x00FF);
+		break;
+
+	case INDIRECT_EEPROM:
+		err = sh532u_read_u8(0x50, (u16)ep_data1, &uc_data1);
+		err = err | sh532u_read_u8(0x50, (u16)ep_data2, &uc_data2);
+		us_data = (((u16)uc_data1 << 8) & 0xFF00) |
+			((u16)uc_data2 & 0x00FF);
+		break;
+
+	case INDIRECT_HVCA:
+		err = sh532u_read_u8(0, (u16)ep_data1, &uc_data1);
+		err = err | sh532u_read_u8(0, (u16)ep_data2, &uc_data2);
+		us_data = (((u16)uc_data1 << 8) & 0xFF00) |
+			((u16)uc_data2 & 0x00FF);
+		break;
+
+	case MASK_AND:
+		err = sh532u_read_u16((u16)ep_addr, &us_data);
+		us_data = us_data & ((((u16)ep_data1 << 8) & 0xFF00) |
+			((u16)ep_data2 & 0x00FF));
+		break;
+
+	case MASK_OR:
+		err = sh532u_read_u16((u16)ep_addr, &us_data);
+		us_data = us_data | ((((u16)ep_data1 << 8) & 0xFF00) |
+			((u16)ep_data2 & 0x00FF));
+		break;
+
+	default:
+		err = 1;
+	}
+	if (!err)
+		err = sh532u_write_u16((u16)ep_addr, us_data);
+	if (err)
+		pr_err("Focuser: Failed to init!\n");
+}
+
+static void init_driver(void)
+{
+	int eeprom_addr;
+	unsigned int eeprom_data = 0;
+	u8 ep_addr, ep_type, ep_data1, ep_data2;
+
+	for (eeprom_addr = 0x30; eeprom_addr <= 0x013C; eeprom_addr += 4) {
+		if (eeprom_addr > 0xff) {
+			/* use hardcoded data instead */
+			eeprom_data = a2buf[(eeprom_addr & 0xFF) / 4];
+		} else {
+			if (eeprom_read_u32(eeprom_addr & 0xFF, &eeprom_data))
+				pr_info("sh532u: cannot read eeprom\n");
+		}
+
+		/* HVCA Address to write eeprom Data1,Data2 by the Data type */
+		ep_addr = (u8)(eeprom_data & 0x000000ff);
+		ep_type = (u8)((eeprom_data & 0x0000ff00) >> 8);
+		ep_data1 = (u8)((eeprom_data & 0x00ff0000) >> 16);
+		ep_data2 = (u8)((eeprom_data & 0xff000000) >> 24);
+
+		if (ep_addr == 0xFF)
+			break;
+
+		if (ep_addr == 0xDD) {
+			mdelay((unsigned int)((ep_data1 << 8) | ep_data2));
+		} else {
+			if ((ep_type & 0x0F) == DATA_1BYTE) {
+				sh532u_hvca_wr1(ep_type, ep_data1, ep_addr);
+			} else {
+				sh532u_hvca_wr2(ep_type,
+						ep_data1,
+						ep_data2,
+						ep_addr);
+			}
+		}
+	}
+	msleep(300);
+
+	init_hvca_pos();
+}
+
+
+static int sh532u_set_position(struct sh532u_info *info, s16 position)
+{
+	if (position > info->config.limit_high)
+		return -1;
+	/* Caller's responsibility to check motor status. */
+	move_driver(position);
+	return 0;
+}
+
+static int sh532u_get_move_status(unsigned long arg)
+{
+	enum sh532u_move_status status = SH532U_Forced32;
+	u8 ucTmp;
+	u16 usSmvFin;
+	int err = sh532u_read_u8(0, STMVEN_211, &ucTmp) |
+		sh532u_read_u16(RZ_211H, &usSmvFin);
+	if (err)
+		return err;
+
+	/* StepMove Error Handling, Unexpected Position */
+	if ((usSmvFin == 0x7FFF) || (usSmvFin == 0x8001)) {
+		/* Stop StepMove Operation */
+		err = sh532u_write_u8(STMVEN_211, ucTmp & 0xFE);
+		if (err)
+			return err;
+	}
+
+	if (ucTmp & STMVEN_ON) {
+		err = sh532u_read_u8(0, MSSET_211, &ucTmp);
+		if (err)
+			return err;
+		if  (ucTmp & CHTGST_ON)
+			status = SH532U_WAIT_FOR_SETTLE;
+		else
+			status = SH532U_LENS_SETTLED;
+	} else
+		status = SH532U_WAIT_FOR_MOVE_END;
+
+	if (copy_to_user((void __user *) arg, &status,
+			 sizeof(enum sh532u_move_status))) {
+		pr_info("Error in copying move status: %s: %d\n", __func__, __LINE__);
+		return -EFAULT;
+	}
+	return 0;
+}
+
+static long sh532u_ioctl(
+	struct file *file,
+	unsigned int cmd,
+	unsigned long arg)
+{
+	struct sh532u_info *info = file->private_data;
+
+	switch (cmd) {
+	case SH532U_IOCTL_GET_CONFIG:
+		if (copy_to_user((void __user *) arg, &info->config,
+				 sizeof(info->config))) {
+			pr_err("Error in copying config: %s: %d\n", __func__, __LINE__);
+			return -EFAULT;
+		}
+		return 0;
+
+	case SH532U_IOCTL_SET_POSITION:
+		return sh532u_set_position(info, (s16)(arg & 0xffff));
+
+	case SH532U_IOCTL_GET_MOVE_STATUS:
+		return sh532u_get_move_status(arg);
+
+	default:
+		return -EINVAL;
+	}
+}
+
+
+static int sh532u_open(struct inode *inode, struct file *file)
+{
+	pr_info("sh532 open\n");
+	file->private_data = info;
+	if (info->sh532u_pdata.board_init)
+		info->sh532u_pdata.board_init(
+			info->sh532u_pdata.context_data);
+	init_driver();
+	return 0;
+}
+
+int sh532u_release(struct inode *inode, struct file *file)
+{
+	pr_info("sh532 release\n");
+	if (info->sh532u_pdata.board_deinit)
+		info->sh532u_pdata.board_deinit(
+			info->sh532u_pdata.context_data);
+	file->private_data = NULL;
+	return 0;
+}
+
+
+static const struct file_operations sh532u_fileops = {
+	.owner = THIS_MODULE,
+	.open = sh532u_open,
+	.unlocked_ioctl = sh532u_ioctl,
+	.release = sh532u_release,
+};
+
+static struct miscdevice sh532u_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "sh532u",
+	.fops = &sh532u_fileops,
+};
+
+static int sh532u_probe(
+	struct i2c_client *client,
+	const struct i2c_device_id *id)
+{
+	int err;
+	struct sh532u_platform_data *sh532u_pdata = client->dev.platform_data;
+
+	pr_info("sh532u: probing sensor.\n");
+	info = kzalloc(sizeof(struct sh532u_info), GFP_KERNEL);
+	if (!info) {
+		pr_err("sh532u: Unable to allocate memory!\n");
+		return -ENOMEM;
+	}
+	err = misc_register(&sh532u_device);
+	if (err) {
+		pr_err("sh532u: Unable to register misc device!\n");
+		kfree(info);
+		return err;
+	}
+	info->i2c_client = client;
+	info->config.settle_time = SETTLETIME_MS;
+	info->config.focal_length = FOCAL_LENGTH;
+	info->config.fnumber = FNUMBER;
+	info->config.pos_low = POS_LOW;
+	info->config.pos_high = POS_HIGH;
+	i2c_set_clientdata(client, info);
+
+	if (sh532u_pdata) {
+		info->sh532u_pdata.context_data = sh532u_pdata->context_data;
+		info->sh532u_pdata.board_init = sh532u_pdata->board_init;
+		info->sh532u_pdata.board_deinit = sh532u_pdata->board_deinit;
+	}
+	return 0;
+}
+
+static int sh532u_remove(struct i2c_client *client)
+{
+	struct sh532u_info *info;
+	info = i2c_get_clientdata(client);
+	misc_deregister(&sh532u_device);
+	kfree(info);
+	return 0;
+}
+
+static const struct i2c_device_id sh532u_id[] = {
+	{ "sh532u", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, sh532u_id);
+
+static struct i2c_driver sh532u_i2c_driver = {
+	.driver = {
+		.name = "sh532u",
+		.owner = THIS_MODULE,
+	},
+	.probe = sh532u_probe,
+	.remove = sh532u_remove,
+	.id_table = sh532u_id,
+};
+
+static int __init sh532u_init(void)
+{
+	return i2c_add_driver(&sh532u_i2c_driver);
+}
+
+static void __exit sh532u_exit(void)
+{
+	i2c_del_driver(&sh532u_i2c_driver);
+}
+
+module_init(sh532u_init);
+module_exit(sh532u_exit);
+
diff --git a/drivers/media/video/tegra/soc380.c b/drivers/media/video/tegra/soc380.c
new file mode 100644
index 000000000000..1304cd58b53d
--- /dev/null
+++ b/drivers/media/video/tegra/soc380.c
@@ -0,0 +1,496 @@
+/*
+ * soc380.c - soc380 sensor driver
+ *
+ * Copyright (c) 2011, NVIDIA, All Rights Reserved.
+ *
+ * Contributors:
+ *      Abhinav Sinha <absinha@nvidia.com>
+ *
+ * Leverage OV2710.c
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ */
+
+/**
+ * SetMode Sequence for 640x480. Phase 0. Sensor Dependent.
+ * This sequence should put sensor in streaming mode for 640x480
+ * This is usually given by the FAE or the sensor vendor.
+ */
+
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <media/soc380.h>
+
+struct soc380_reg {
+	u16 addr;
+	u16 val;
+};
+
+struct soc380_info {
+	int mode;
+	struct i2c_client *i2c_client;
+	struct soc380_platform_data *pdata;
+};
+
+#define SOC380_TABLE_WAIT_MS 0
+#define SOC380_TABLE_END 1
+#define SOC380_MAX_RETRIES 3
+
+static struct soc380_reg mode_640x480[] = {
+	{0x001A, 0x0011},
+
+	{SOC380_TABLE_WAIT_MS, 1},
+
+	{0x001A, 0x0010},
+
+	{SOC380_TABLE_WAIT_MS, 1},
+
+	{0x0018, 0x4028},
+	{0x001A, 0x0210},
+	{0x001E, 0x0777},
+	{0x0016, 0x42DF},
+	{0x0010, 0x0217},
+	{0x0012, 0x0000},
+	{0x0014, 0x2147},
+
+	{SOC380_TABLE_WAIT_MS, 50},
+
+	{0x0014, 0x2047},
+
+	{SOC380_TABLE_WAIT_MS, 10},
+
+	{0x0014, 0xA046},
+
+	{SOC380_TABLE_WAIT_MS, 10},
+
+	{0x3040, 0x0027},
+	{0x301A, 0x1218},
+
+	{SOC380_TABLE_WAIT_MS, 10},
+
+	{0x301A, 0x121C},
+	{0x098C, 0x2703},
+	{0x0990, 0x0280},
+	{0x098C, 0x2705},
+	{0x0990, 0x01E0},
+	{0x098C, 0x2707},
+	{0x0990, 0x0280},
+	{0x098C, 0x2709},
+	{0x0990, 0x01E0},
+	{0x098C, 0x270D},
+	{0x0990, 0x0004},
+	{0x098C, 0x270F},
+	{0x0990, 0x0004},
+	{0x098C, 0x2711},
+	{0x0990, 0x01EB},
+	{0x098C, 0x2713},
+	{0x0990, 0x028B},
+	{0x098C, 0x2715},
+	{0x0990, 0x0001},
+	{0x098C, 0x2717},
+	{0x0990, 0x0026},
+	{0x098C, 0x2719},
+	{0x0990, 0x001A},
+	{0x098C, 0x271B},
+	{0x0990, 0x006B},
+	{0x098C, 0x271D},
+	{0x0990, 0x006B},
+	{0x098C, 0x271F},
+	{0x0990, 0x046F},
+	{0x098C, 0x2721},
+	{0x0990, 0x034A},
+	{0x098C, 0x2723},
+	{0x0990, 0x0004},
+	{0x098C, 0x2725},
+	{0x0990, 0x0004},
+	{0x098C, 0x2727},
+	{0x0990, 0x01EB},
+	{0x098C, 0x2729},
+	{0x0990, 0x028B},
+	{0x098C, 0x272B},
+	{0x0990, 0x0001},
+	{0x098C, 0x272D},
+	{0x0990, 0x0026},
+	{0x098C, 0x272F},
+	{0x0990, 0x001A},
+	{0x098C, 0x2731},
+	{0x0990, 0x006B},
+	{0x098C, 0x2733},
+	{0x0990, 0x006B},
+	{0x098C, 0x2735},
+	{0x0990, 0x046F},
+	{0x098C, 0x2737},
+	{0x0990, 0x034A},
+	{0x098C, 0x2739},
+	{0x0990, 0x0000},
+	{0x098C, 0x273B},
+	{0x0990, 0x027F},
+	{0x098C, 0x273D},
+	{0x0990, 0x0000},
+	{0x098C, 0x273F},
+	{0x0990, 0x01DF},
+	{0x098C, 0x2747},
+	{0x0990, 0x0000},
+	{0x098C, 0x2749},
+	{0x0990, 0x027F},
+	{0x098C, 0x274B},
+	{0x0990, 0x0000},
+	{0x098C, 0x274D},
+	{0x0990, 0x01DF},
+	{0x098C, 0x222D},
+	{0x0990, 0x008B},
+	{0x098C, 0xA408},
+	{0x0990, 0x001F},
+	{0x098C, 0xA409},
+	{0x0990, 0x0022},
+	{0x098C, 0xA40A},
+	{0x0990, 0x0019},
+	{0x098C, 0xA40B},
+	{0x0990, 0x001C},
+	{0x098C, 0x2411},
+	{0x0990, 0x008B},
+	{0x098C, 0x2413},
+	{0x0990, 0x00A6},
+	{0x098C, 0x2415},
+	{0x0990, 0x008B},
+	{0x098C, 0x2417},
+	{0x0990, 0x00A6},
+	{0x098C, 0xA40D},
+	{0x0990, 0x0002},
+	{0x098C, 0xA410},
+	{0x0990, 0x0001},
+	{0x098C, 0xA103},
+	{0x0990, 0x0006},
+
+	{SOC380_TABLE_WAIT_MS, 50},
+
+	{0x098C, 0xA103},
+	{0x0990, 0x0005},
+
+	{0x3012, 0x0384},
+	{0x098C, 0x2115},
+	{0x0990, 0x0002},
+	{0x321C, 0x0003},
+	{0x3330, 0x0000},
+	{0x098C, 0x2103},
+	{0x0990, 0x0002},
+	{0x321C, 0x0003},
+	{0x3330, 0x0000},
+	{0x3330, 0x0000},
+	{0x321C, 0x0003},
+	{0x098C, 0xA103},
+	{0x0990, 0x0000},
+	{0x098C, 0xA104},
+	{0x0990, 0x0007},
+
+	{0x098C, 0xA115},
+	{0x0990, 0x0002},
+	{0x098C, 0xA103},
+	{0x0990, 0x0002},
+
+	{SOC380_TABLE_WAIT_MS, 500},
+	{SOC380_TABLE_END, 0x0000}
+};
+
+enum {
+	SOC380_MODE_680x480,
+};
+
+static struct soc380_reg *mode_table[] = {
+	[SOC380_MODE_680x480] = mode_640x480,
+};
+
+static int soc380_read_reg(struct i2c_client *client, u16 addr, u16 *val)
+{
+	int err;
+	struct i2c_msg msg[2];
+	unsigned char data[4];
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 2;
+	msg[0].buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8) (addr >> 8);
+	data[1] = (u8) (addr & 0xff);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 2;
+	msg[1].buf = data + 2;
+
+	err = i2c_transfer(client->adapter, msg, 2);
+
+	if (err != 2)
+		return -EINVAL;
+
+	*val = data[2] << 8 | data[3];
+
+	return 0;
+}
+
+static int soc380_write_reg(struct i2c_client *client, u16 addr, u16 val)
+{
+	int err;
+	struct i2c_msg msg;
+	unsigned char data[4];
+	int retry = 0;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data[0] = (u8) (addr >> 8);
+	data[1] = (u8) (addr & 0xff);
+	data[2] = (u8) (val >> 8);
+	data[3] = (u8) (val & 0xff);
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 4;
+	msg.buf = data;
+
+	do {
+		err = i2c_transfer(client->adapter, &msg, 1);
+		if (err == 1)
+			return 0;
+		retry++;
+		pr_err("soc380: i2c transfer failed, retrying %x %x\n",
+		       addr, val);
+		msleep(3);
+	} while (retry <= SOC380_MAX_RETRIES);
+
+	return err;
+}
+
+static int soc380_write_table(struct i2c_client *client,
+			      const struct soc380_reg table[],
+			      const struct soc380_reg override_list[],
+			      int num_override_regs)
+{
+	int err;
+	const struct soc380_reg *next;
+	int i;
+	u16 val;
+
+	for (next = table; next->addr != SOC380_TABLE_END; next++) {
+		if (next->addr == SOC380_TABLE_WAIT_MS) {
+			msleep(next->val);
+			continue;
+		}
+
+		val = next->val;
+
+		/* When an override list is passed in, replace the reg */
+		/* value to write if the reg is in the list            */
+		if (override_list) {
+			for (i = 0; i < num_override_regs; i++) {
+				if (next->addr == override_list[i].addr) {
+					val = override_list[i].val;
+					break;
+				}
+			}
+		}
+
+		err = soc380_write_reg(client, next->addr, val);
+		if (err)
+			return err;
+	}
+	return 0;
+}
+
+static int soc380_set_mode(struct soc380_info *info, struct soc380_mode *mode)
+{
+	int sensor_mode;
+	int err;
+
+	pr_info("%s: xres %u yres %u\n", __func__, mode->xres, mode->yres);
+	if (mode->xres == 640 && mode->yres == 480)
+		sensor_mode = SOC380_MODE_680x480;
+	else {
+		pr_err("%s: invalid resolution supplied to set mode %d %d\n",
+		       __func__, mode->xres, mode->yres);
+		return -EINVAL;
+	}
+
+	err = soc380_write_table(info->i2c_client, mode_table[sensor_mode],
+		NULL, 0);
+	if (err)
+		return err;
+
+	info->mode = sensor_mode;
+	return 0;
+}
+
+static int soc380_get_status(struct soc380_info *info,
+		struct soc380_status *dev_status)
+{
+	int err;
+
+	err = soc380_write_reg(info->i2c_client, 0x98C, dev_status->data);
+	if (err)
+		return err;
+
+	err = soc380_read_reg(info->i2c_client, 0x0990,
+		(u16 *) &dev_status->status);
+	if (err)
+		return err;
+
+	return err;
+}
+
+static long soc380_ioctl(struct file *file,
+			 unsigned int cmd, unsigned long arg)
+{
+	int err;
+	struct soc380_info *info = file->private_data;
+
+	switch (cmd) {
+	case SOC380_IOCTL_SET_MODE:
+	{
+		struct soc380_mode mode;
+		if (copy_from_user(&mode,
+				   (const void __user *)arg,
+				   sizeof(struct soc380_mode))) {
+			return -EFAULT;
+		}
+
+		return soc380_set_mode(info, &mode);
+	}
+	case SOC380_IOCTL_GET_STATUS:
+	{
+		struct soc380_status dev_status;
+		if (copy_from_user(&dev_status,
+				   (const void __user *)arg,
+				   sizeof(struct soc380_status))) {
+			return -EFAULT;
+		}
+
+		err = soc380_get_status(info, &dev_status);
+		if (err)
+			return err;
+		if (copy_to_user((void __user *)arg, &dev_status,
+				 sizeof(struct soc380_status))) {
+			return -EFAULT;
+		}
+		return 0;
+	}
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static struct soc380_info *info;
+
+static int soc380_open(struct inode *inode, struct file *file)
+{
+	struct soc380_status dev_status;
+	int err;
+
+	file->private_data = info;
+	if (info->pdata && info->pdata->power_on)
+		info->pdata->power_on();
+
+	dev_status.data = 0;
+	dev_status.status = 0;
+	err = soc380_get_status(info, &dev_status);
+	return err;
+}
+
+int soc380_release(struct inode *inode, struct file *file)
+{
+	if (info->pdata && info->pdata->power_off)
+		info->pdata->power_off();
+	file->private_data = NULL;
+	return 0;
+}
+
+static const struct file_operations soc380_fileops = {
+	.owner = THIS_MODULE,
+	.open = soc380_open,
+	.unlocked_ioctl = soc380_ioctl,
+	.release = soc380_release,
+};
+
+static struct miscdevice soc380_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "soc380",
+	.fops = &soc380_fileops,
+};
+
+static int soc380_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int err;
+
+	pr_info("soc380: probing sensor.\n");
+
+	info = kzalloc(sizeof(struct soc380_info), GFP_KERNEL);
+	if (!info) {
+		pr_err("soc380: Unable to allocate memory!\n");
+		return -ENOMEM;
+	}
+
+	err = misc_register(&soc380_device);
+	if (err) {
+		pr_err("soc380: Unable to register misc device!\n");
+		kfree(info);
+		return err;
+	}
+
+	info->pdata = client->dev.platform_data;
+	info->i2c_client = client;
+
+	i2c_set_clientdata(client, info);
+	return 0;
+}
+
+static int soc380_remove(struct i2c_client *client)
+{
+	struct soc380_info *info;
+	info = i2c_get_clientdata(client);
+	misc_deregister(&soc380_device);
+	kfree(info);
+	return 0;
+}
+
+static const struct i2c_device_id soc380_id[] = {
+	{ "soc380", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, soc380_id);
+
+static struct i2c_driver soc380_i2c_driver = {
+	.driver = {
+		.name = "soc380",
+		.owner = THIS_MODULE,
+	},
+	.probe = soc380_probe,
+	.remove = soc380_remove,
+	.id_table = soc380_id,
+};
+
+static int __init soc380_init(void)
+{
+	pr_info("soc380 sensor driver loading\n");
+	return i2c_add_driver(&soc380_i2c_driver);
+}
+
+static void __exit soc380_exit(void)
+{
+	i2c_del_driver(&soc380_i2c_driver);
+}
+
+module_init(soc380_init);
+module_exit(soc380_exit);
diff --git a/drivers/media/video/tegra/ssl3250a.c b/drivers/media/video/tegra/ssl3250a.c
new file mode 100644
index 000000000000..f832ffee713c
--- /dev/null
+++ b/drivers/media/video/tegra/ssl3250a.c
@@ -0,0 +1,333 @@
+/*
+ * ssl3250a.c - ssl3250a flash/torch kernel driver
+ *
+ * Copyright (C) 2011 NVIDIA Corp.
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ */
+
+
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <media/ssl3250a.h>
+
+#define SSL3250A_I2C_REG_AMP		0x00
+#define SSL3250A_I2C_REG_TMR		0x01
+#define SSL3250A_I2C_REG_STRB		0x02
+#define SSL3250A_I2C_REG_STS		0x03
+
+
+enum {
+	SSL3250A_GPIO_ACT,
+	SSL3250A_GPIO_EN1,
+	SSL3250A_GPIO_EN2,
+	SSL3250A_GPIO_STRB,
+};
+
+struct ssl3250a_info {
+	struct i2c_client *i2c_client;
+	struct ssl3250a_platform_data *pdata;
+};
+
+static struct ssl3250a_info *info;
+
+static int ssl3250a_gpio(u8 gpio, u8 val)
+{
+	int prev_val;
+
+	switch (gpio) {
+	case SSL3250A_GPIO_ACT:
+		if (info->pdata && info->pdata->gpio_act) {
+			prev_val = info->pdata->gpio_act(val);
+			if (val && (prev_val ^ val))
+				mdelay(1); /*delay for device ready*/
+			return 0;
+		}
+		return -1;
+
+	case SSL3250A_GPIO_EN1:
+		if (info->pdata && info->pdata->gpio_en1) {
+			info->pdata->gpio_en1(val);
+			return 0;
+		}
+		return -1;
+
+	case SSL3250A_GPIO_EN2:
+		if (info->pdata && info->pdata->gpio_en2) {
+			info->pdata->gpio_en2(val);
+			return 0;
+		}
+		return -1;
+
+	case SSL3250A_GPIO_STRB:
+		if (info->pdata && info->pdata->gpio_strb) {
+			info->pdata->gpio_strb(val);
+			return 0;
+		}
+
+	default:
+		return -1;
+	}
+}
+
+static int ssl3250a_get_reg(u8 addr, u8 *val)
+{
+	struct i2c_client *client = info->i2c_client;
+	struct i2c_msg msg[2];
+	unsigned char data[2];
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = data;
+
+	data[0] = (u8) (addr);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 1;
+	msg[1].buf = data + 1;
+
+	*val = 0;
+
+	if (i2c_transfer(client->adapter, msg, 2) == 2) {
+		*val = data[1];
+		return 0;
+	} else {
+		return -1;
+	}
+}
+
+static int ssl3250a_set_reg(u8 addr, u8 val)
+{
+	struct i2c_client *client = info->i2c_client;
+	struct i2c_msg msg;
+	unsigned char data[2];
+
+	data[0] = (u8) (addr);
+	data[1] = (u8) (val);
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 2;
+	msg.buf = data;
+
+	if (i2c_transfer(client->adapter, &msg, 1) == 1)
+		return 0;
+	else
+		return -1;
+}
+
+static long ssl3250a_ioctl(
+	struct file *file,
+	unsigned int cmd,
+	unsigned long arg)
+{
+	u8 val = (u8)arg;
+	u8 reg;
+
+	switch (cmd) {
+	case SSL3250A_IOCTL_MODE_SHUTDOWN:
+		ssl3250a_gpio(SSL3250A_GPIO_ACT, 0);
+		return 0;
+
+	case SSL3250A_IOCTL_MODE_STANDBY:
+		ssl3250a_gpio(SSL3250A_GPIO_ACT, 1);
+		if (info->pdata->config & 0x01) {	/*0:0 0=I2C, 1=GPIO*/
+			ssl3250a_gpio(SSL3250A_GPIO_EN1, 0);
+			ssl3250a_gpio(SSL3250A_GPIO_EN2, 0);
+			return 0;
+		} else {
+			return ssl3250a_set_reg(SSL3250A_I2C_REG_AMP, 0x00);
+		}
+
+/* Amp limit for torch, flash, and LED is controlled by external circuitry in
+ * GPIO mode.  In I2C mode amp limit is controlled by chip registers and the
+ * limit values are in the board-sensors file.
+ */
+	case SSL3250A_IOTCL_MODE_TORCH:
+		ssl3250a_gpio(SSL3250A_GPIO_ACT, 1);
+		if (info->pdata->config & 0x01) {	/*0:0 0=I2C, 1=GPIO*/
+			ssl3250a_gpio(SSL3250A_GPIO_EN1, 0);
+			ssl3250a_gpio(SSL3250A_GPIO_EN2, 1);
+			return 0;
+		} else {
+			if (val > info->pdata->max_amp_torch)
+				val = info->pdata->max_amp_torch;
+			val = ((val << 3) & 0xF8);	/*7:3=torch amps*/
+			if (!ssl3250a_get_reg(SSL3250A_I2C_REG_AMP, &reg)) {
+				val = val | (reg & 0x07); /*shared w/ LED 2:0*/
+				return ssl3250a_set_reg(SSL3250A_I2C_REG_AMP,
+							val);
+			} else {
+				return -1;
+			}
+		}
+
+	case SSL3250A_IOCTL_MODE_FLASH:
+		ssl3250a_gpio(SSL3250A_GPIO_ACT, 1);
+		if (info->pdata->config & 0x01) {	/*0:0 0=I2C, 1=GPIO*/
+			ssl3250a_gpio(SSL3250A_GPIO_EN1, 1);
+			ssl3250a_gpio(SSL3250A_GPIO_EN2, 1);
+			return 0;
+		} else {
+			if (val != 0)			/*if 0 then flash=off*/
+				val = val + 11;		/*flash starts at 12*/
+			if (val > info->pdata->max_amp_flash)
+				val = info->pdata->max_amp_flash;
+			val = ((val << 3) & 0xF8);	/*7:3=flash amps*/
+			if (!ssl3250a_get_reg(SSL3250A_I2C_REG_AMP, &reg)) {
+				val = val | (reg & 0x07); /*shared w/ LED 2:0*/
+				return ssl3250a_set_reg(SSL3250A_I2C_REG_AMP,
+							val);
+			} else {
+				return -1;
+			}
+		}
+
+	case SSL3250A_IOCTL_MODE_LED:
+		ssl3250a_gpio(SSL3250A_GPIO_ACT, 1);
+		if (info->pdata->config & 0x01) {	/*0:0 0=I2C, 1=GPIO*/
+			ssl3250a_gpio(SSL3250A_GPIO_EN1, 1);
+			ssl3250a_gpio(SSL3250A_GPIO_EN2, 0);
+			return 0;
+		} else {
+			if (val > info->pdata->max_amp_indic)
+				val = info->pdata->max_amp_indic;
+			val = (val & 0x07);		/*2:0=LED amps*/
+			if (!ssl3250a_get_reg(SSL3250A_I2C_REG_AMP, &reg)) {
+				val = val | (reg & 0xF8); /*shared w/ 7:3*/
+				return ssl3250a_set_reg(SSL3250A_I2C_REG_AMP,
+							val);
+			} else {
+				return -1;
+			}
+		}
+
+	case SSL3250A_IOCTL_STRB:
+		if (val)
+			val = 0x01;			/*bit 0=I2C, >0=GPIO*/
+		/* if STRB GPIO use that regardless of operation mode */
+		if (!ssl3250a_gpio(SSL3250A_GPIO_STRB, val))
+			return 0;
+		if (!info->pdata->config & 0x01)	/*0:0 0=I2C, 1=GPIO*/
+			return ssl3250a_set_reg(SSL3250A_I2C_REG_STRB, val);
+		else
+			return -1;
+
+	case SSL3250A_IOCTL_TIMER:
+		if (!info->pdata->config & 0x01)	/*if I2C mode*/
+			return ssl3250a_set_reg(SSL3250A_I2C_REG_TMR, val);
+
+	default:
+		return -1;
+	}
+}
+
+
+static int ssl3250a_open(struct inode *inode, struct file *file)
+{
+	int err;
+	u8 reg;
+	file->private_data = info;
+
+	pr_info("%s\n", __func__);
+	if (info->pdata && info->pdata->init) {
+		err = info->pdata->init();
+		if (err)
+			pr_err("ssl3250a_open: Board init failed\n");
+	}
+	ssl3250a_gpio(SSL3250A_GPIO_ACT, 1);
+	err = ssl3250a_get_reg(SSL3250A_I2C_REG_STS, &reg);
+	ssl3250a_gpio(SSL3250A_GPIO_ACT, 0);
+	if (err)
+		pr_err("ssl3250a_open: Device init failed\n");
+	return 0;
+}
+
+int ssl3250a_release(struct inode *inode, struct file *file)
+{
+	if (info->pdata && info->pdata->exit)
+		info->pdata->exit();
+	file->private_data = NULL;
+	return 0;
+}
+
+
+static const struct file_operations ssl3250a_fileops = {
+	.owner = THIS_MODULE,
+	.open = ssl3250a_open,
+	.unlocked_ioctl = ssl3250a_ioctl,
+	.release = ssl3250a_release,
+};
+
+static struct miscdevice ssl3250a_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "ssl3250a",
+	.fops = &ssl3250a_fileops,
+};
+
+static int ssl3250a_probe(
+	struct i2c_client *client,
+	const struct i2c_device_id *id)
+{
+	int err;
+	info = kzalloc(sizeof(struct ssl3250a_info), GFP_KERNEL);
+	if (!info) {
+		pr_err("ssl3250a: Unable to allocate memory!\n");
+		return -ENOMEM;
+	}
+	err = misc_register(&ssl3250a_device);
+	if (err) {
+		pr_err("ssl3250a: Unable to register misc device!\n");
+		kfree(info);
+		return err;
+	}
+	info->pdata = client->dev.platform_data;
+	info->i2c_client = client;
+	i2c_set_clientdata(client, info);
+	return 0;
+}
+
+static int ssl3250a_remove(struct i2c_client *client)
+{
+	info = i2c_get_clientdata(client);
+	misc_deregister(&ssl3250a_device);
+	kfree(info);
+	return 0;
+}
+
+static const struct i2c_device_id ssl3250a_id[] = {
+	{ "ssl3250a", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, ssl3250a_id);
+
+static struct i2c_driver ssl3250a_i2c_driver = {
+	.driver = {
+		.name = "ssl3250a",
+		.owner = THIS_MODULE,
+	},
+	.probe = ssl3250a_probe,
+	.remove = ssl3250a_remove,
+	.id_table = ssl3250a_id,
+};
+
+static int __init ssl3250a_init(void)
+{
+	return i2c_add_driver(&ssl3250a_i2c_driver);
+}
+
+static void __exit ssl3250a_exit(void)
+{
+	i2c_del_driver(&ssl3250a_i2c_driver);
+}
+
+module_init(ssl3250a_init);
+module_exit(ssl3250a_exit);
+
diff --git a/drivers/media/video/tegra/tegra_camera.c b/drivers/media/video/tegra/tegra_camera.c
index f310d0f5619f..2d4baa54c911 100644
--- a/drivers/media/video/tegra/tegra_camera.c
+++ b/drivers/media/video/tegra/tegra_camera.c
@@ -290,9 +290,13 @@ static int tegra_camera_probe(struct platform_device *pdev)
 	int err;
 
 	pr_info("%s: probe\n", TEGRA_CAMERA_NAME);
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
 	tegra_camera_regulator_csi = regulator_get(&pdev->dev, "vcsi");
+#else
+	tegra_camera_regulator_csi = regulator_get(&pdev->dev, "avdd_dsi_csi");
+#endif
 	if (IS_ERR_OR_NULL(tegra_camera_regulator_csi)) {
-		pr_err("%s: Couldn't get regulator vcsi\n", TEGRA_CAMERA_NAME);
+		pr_err("%s: Couldn't get regulator\n", TEGRA_CAMERA_NAME);
 		return PTR_ERR(tegra_camera_regulator_csi);
 	}