diff --git a/drivers/media/i2c/lt6911uxc.c b/drivers/media/i2c/lt6911uxc.c
index f9d07c51..c2b2ec53 100644
--- a/drivers/media/i2c/lt6911uxc.c
+++ b/drivers/media/i2c/lt6911uxc.c
@@ -19,6 +19,134 @@
 
 #include "../platform/tegra/camera/camera_gpio.h"
 
+#define lt6911uxc_reg struct reg_8
+#define LT6911_TABLE_WAIT_MS	0
+#define LT6911_TABLE_END        1
+
+static lt6911uxc_reg lt_enable_i2c[] = {
+	{0xff, 0x80},
+	{0xee, 0x01},
+	{0x10, 0x00},
+	{LT6911_TABLE_END, 0x00}
+};
+
+static lt6911uxc_reg lt_configure_param[] = {
+	{0x5e, 0xdf},
+	{0x58, 0x00},
+	{0x59, 0x50},
+	{0x5a, 0x10},
+	{LT6911_TABLE_WAIT_MS, 1},
+	{0x5a, 0x00},
+	{0x58, 0x21},
+	{LT6911_TABLE_END, 0x00}
+};
+
+static lt6911uxc_reg lt_block_erase[] = {
+	{0x5a, 0x04},
+	{LT6911_TABLE_WAIT_MS, 1},
+	{0x5a, 0x00},
+	{0x5b, 0x01},
+	{0x5c, 0x80},
+	{0x5d, 0x00},
+	{0x5a, 0x01},
+	{LT6911_TABLE_WAIT_MS, 1},
+	{0x5a, 0x00},
+	{LT6911_TABLE_END, 0x00}
+};
+
+static lt6911uxc_reg lt_fifo_rst[] = {
+	{0xff, 0x81},
+	{0x08, 0xbf},
+	{LT6911_TABLE_WAIT_MS, 1},
+	{0x08, 0xff},
+	{LT6911_TABLE_END, 0x00}
+};
+
+static lt6911uxc_reg lt_write_enable[] = {
+	{0xff, 0x80},
+	{0x5a, 0x04},
+	{LT6911_TABLE_WAIT_MS, 1},
+	{0x5a, 0x00},
+	{LT6911_TABLE_END, 0x00}
+};
+
+static lt6911uxc_reg lt_data_to_fifo[] = {
+	{0x5e, 0xdf},
+	{0x5a, 0x20},
+	{LT6911_TABLE_WAIT_MS, 1},
+	{0x5a, 0x00},
+	{LT6911_TABLE_END, 0x00}
+};
+
+static lt6911uxc_reg lt_write_disable[] = {
+	{0xff, 0x80},
+	{0x5a, 0x08},
+	{LT6911_TABLE_WAIT_MS, 1},
+	{0x5a, 0x00},
+	{0x58, 0x00},
+	{LT6911_TABLE_END, 0x00}
+};
+
+static u8 LT6911_EDIDs[][256] = {
+	// EDID with default 1920x1080 60fps
+	{
+		0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x3A,0xC4,0x04,0xED,0x01,0x01,0x01,0x01,
+		0x1E,0x21,0x01,0x03,0x80,0x6F,0x3E,0x78,0x0A,0xEE,0x91,0xA3,0x54,0x4C,0x99,0x26,
+		0x0F,0x50,0x54,0x00,0x00,0x00,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,
+		0x01,0x01,0x01,0x01,0x01,0x01,0x02,0x3A,0x80,0x18,0x71,0x38,0x2D,0x40,0x58,0x2C,
+		0x45,0x00,0xBA,0x88,0x21,0x00,0x00,0x1E,0x02,0x3A,0x80,0x18,0x71,0x38,0x2D,0x40,
+		0x58,0x2C,0x45,0x00,0xBA,0x88,0x21,0x00,0x00,0x1E,0x00,0x00,0x00,0xFD,0x00,0x18,
+		0x4B,0x0F,0x87,0x22,0x00,0x0A,0x20,0x20,0x20,0x20,0x20,0x20,0x00,0x00,0x00,0xFC,
+		0x00,0x44,0x75,0x61,0x6C,0x20,0x50,0x6F,0x72,0x74,0x20,0x52,0x47,0x42,0x01,0x50,
+		0x02,0x03,0x18,0x71,0x43,0x04,0x01,0x03,0x23,0x09,0x07,0x01,0x83,0x01,0x00,0x00,
+		0x67,0x03,0x0C,0x00,0x10,0x00,0x00,0x3C,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+		0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+		0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+		0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+		0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+		0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+		0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xAD
+	},
+	// EDID with default 3840x2160 60fps
+	{
+		0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x3A,0xC4,0x01,0xED,0x01,0x01,0x01,0x01,
+		0x1E,0x21,0x01,0x03,0x80,0x6F,0x3E,0x78,0x0A,0xEE,0x91,0xA3,0x54,0x4C,0x99,0x26,
+		0x0F,0x50,0x54,0x00,0x00,0x00,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,
+		0x01,0x01,0x01,0x01,0x01,0x01,0x08,0xE8,0x00,0x30,0xF2,0x70,0x5A,0x80,0xB0,0x58,
+		0x8A,0x00,0x50,0x1D,0x74,0x00,0x00,0x1E,0x08,0xE8,0x00,0x30,0xF2,0x70,0x5A,0x80,
+		0xB0,0x58,0x8A,0x00,0x50,0x1D,0x74,0x00,0x00,0x1E,0x00,0x00,0x00,0xFD,0x00,0x18,
+		0x4B,0x0F,0x87,0x3C,0x00,0x0A,0x20,0x20,0x20,0x20,0x20,0x20,0x00,0x00,0x00,0xFC,
+		0x00,0x44,0x75,0x61,0x6C,0x20,0x50,0x6F,0x72,0x74,0x20,0x52,0x47,0x42,0x01,0xC7,
+		0x02,0x03,0x20,0x71,0x43,0x61,0x01,0x03,0x23,0x09,0x07,0x01,0x83,0x01,0x00,0x00,
+		0x67,0x03,0x0C,0x00,0x10,0x00,0x00,0x3C,0x67,0xD8,0x5D,0xC4,0x01,0x78,0x80,0x00,
+		0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+		0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+		0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+		0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+		0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+		0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xEF
+	},
+	// EDID with default 1280x720 60fps
+	{
+		0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x3A,0xC4,0x05,0xED,0x01,0x01,0x01,0x01,
+		0x1E,0x21,0x01,0x03,0x80,0x6F,0x3E,0x78,0x0A,0xEE,0x91,0xA3,0x54,0x4C,0x99,0x26,
+		0x0F,0x50,0x54,0x00,0x00,0x00,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,
+		0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x1D,0x00,0x72,0x51,0xD0,0x1E,0x20,0x6E,0x28,
+		0x55,0x00,0xC4,0x8E,0x21,0x00,0x00,0x1E,0x01,0x1D,0x00,0x72,0x51,0xD0,0x1E,0x20,
+		0x6E,0x28,0x55,0x00,0xC4,0x8E,0x21,0x00,0x00,0x1E,0x00,0x00,0x00,0xFD,0x00,0x18,
+		0x4B,0x0F,0x87,0x22,0x00,0x0A,0x20,0x20,0x20,0x20,0x20,0x20,0x00,0x00,0x00,0xFC,
+		0x00,0x44,0x75,0x61,0x6C,0x20,0x50,0x6F,0x72,0x74,0x20,0x52,0x47,0x42,0x01,0xE1,
+		0x02,0x03,0x18,0x71,0x43,0x04,0x01,0x03,0x23,0x09,0x07,0x01,0x83,0x01,0x00,0x00,
+		0x67,0x03,0x0C,0x00,0x10,0x00,0x00,0x3C,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+		0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+		0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+		0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+		0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+		0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+		0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xAD
+	}
+};
+
 static const struct of_device_id lt6911uxc_of_match[] = {
 	{ .compatible = "nvidia,lt6911uxc", },
 	{ },
@@ -37,13 +165,18 @@ static const int lt6911uxc_30fps[] = {
 	30,
 };
 
+static const u8 lt6911uxc_HDMI_Signal = 0xa3;
+
+static const u8 lt6911uxc_FW_version[] = { 0xa7, 0xa8, 0xa9, 0xaa };
+
 struct lt6911uxc {
 	struct i2c_client		*i2c_client;
 	struct v4l2_subdev		*subdev;
-	u16	fine_integ_time;
-	u32	frame_length;
+	u16				fine_integ_time;
+	u32				frame_length;
 	struct camera_common_data	*s_data;
 	struct tegracam_device		*tc_dev;
+	struct regmap 			*regmap;
 };
 
 static const struct camera_common_frmfmt lt6911uxc_frmfmt[] = {
@@ -53,11 +186,50 @@ static const struct camera_common_frmfmt lt6911uxc_frmfmt[] = {
 };
 
 static const struct regmap_config sensor_regmap_config = {
-	.reg_bits = 16,
+	.reg_bits = 8,
 	.val_bits = 8,
 	.cache_type = REGCACHE_RBTREE,
 };
 
+static inline int lt6911uxc_read_reg(struct camera_common_data *s_data,
+	u16 addr, u8 *val)
+{
+	int err = 0;
+	u32 reg_val = 0;
+
+	err = regmap_read(s_data->regmap, addr, &reg_val);
+	if (err) {
+		dev_err(s_data->dev, "%s: i2c read , 0x%x = %x",
+			__func__, addr, reg_val);
+		return err;
+	}
+
+	*val = reg_val & 0xff;
+
+	return err;
+}
+
+static inline int lt6911uxc_write_reg(struct camera_common_data *s_data,
+					u16 addr, u8 val)
+{
+	int err = 0;
+	err = regmap_write(s_data->regmap, addr, val);
+	if (err)
+		dev_err(s_data->dev, "%s: i2c write failed, 0x%x = %x",
+			__func__, addr, val);
+	return err;
+}
+
+static int lt6911uxc_write_table(struct lt6911uxc *priv,
+				const lt6911uxc_reg table[])
+{
+	return regmap_util_write_table_8(priv->s_data->regmap,
+					 table,
+					 NULL, 0,
+					 LT6911_TABLE_WAIT_MS,
+					 LT6911_TABLE_END);
+}
+
 static int lt6911uxc_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
@@ -84,6 +256,8 @@ static int lt6911uxc_start_streaming(struct tegracam_device *tc_dev)
 		else
 			gpio_set_value(pw->reset_gpio, 1);
 	}
+	msleep(300);
+
 	return 0;
 }
 
@@ -101,29 +275,7 @@ static int lt6911uxc_stop_streaming(struct tegracam_device *tc_dev)
 	return 0;
 }
 
-static int lt6911uxc_set_mode(struct tegracam_device *tc_dev)
-{
-	/* Width and Height taken care of by the firmware */
-	return 0;
-}
 
-static inline int lt6911uxc_read_reg(struct camera_common_data *s_data,
-	u16 addr, u8 *val)
-{
-	int err = 0;
-	u32 reg_val = 0;
-
-	err = regmap_read(s_data->regmap, addr, &reg_val);
-	*val = reg_val & 0xff;
-
-	return err;
-}
-
-static inline int lt6911uxc_write_reg(struct camera_common_data *s_data,
-					u16 addr, u8 val)
-{
-	return 0;
-}
 
 static int lt6911uxc_power_get(struct tegracam_device *tc_dev)
 {
@@ -309,9 +461,9 @@ static int lt6911uxc_power_on(struct camera_common_data *s_data)
 
 	if (pw->reset_gpio) {
 		if (gpiod_cansleep(gpio_to_desc(pw->reset_gpio)))
-			gpio_set_value_cansleep(pw->reset_gpio, 0);
+			gpio_set_value_cansleep(pw->reset_gpio, 1);
 		else
-			gpio_set_value(pw->reset_gpio, 0);
+			gpio_set_value(pw->reset_gpio, 1);
 	}
 
 	usleep_range(10, 20);
@@ -335,7 +487,7 @@ static int lt6911uxc_power_on(struct camera_common_data *s_data)
 			goto lt6911uxc_dvdd_fail;
 	}
 
-	usleep_range(10, 20);
+	msleep(300);
 
 	pw->state = SWITCH_ON;
 
@@ -389,11 +541,64 @@ static int lt6911uxc_power_off(struct camera_common_data *s_data)
 	return 0;
 }
 
+static int lt6911uxc_set_mode(struct tegracam_device *tc_dev)
+{
+	int err = 0;
+	int i, j;
+
+	struct camera_common_data *s_data = tc_dev->s_data;
+	struct lt6911uxc *priv = (struct lt6911uxc *)s_data->priv;
+	struct camera_common_power_rail *pw = s_data->power;
+
+	// Write the corresponding Shadow EDID
+	err |= lt6911uxc_write_table(priv, lt_enable_i2c);
+	err |= lt6911uxc_write_table(priv, lt_configure_param);
+	err |= lt6911uxc_write_table(priv, lt_block_erase);
+	msleep(600);
+
+	for (i = 0; i < 8; i++)
+	{
+		err |= lt6911uxc_write_table(priv, lt_fifo_rst);
+		err |= lt6911uxc_write_table(priv, lt_write_enable);
+		err |= lt6911uxc_write_table(priv, lt_data_to_fifo);
+
+		for(j = 0; j < 32; j++)
+		{
+			lt6911uxc_write_reg(priv->s_data, 0x59,
+				LT6911_EDIDs[s_data->mode][(i * 32) + j]);
+		}
+		err |= lt6911uxc_write_reg(priv->s_data, 0x5b, 0x01);
+		err |= lt6911uxc_write_reg(priv->s_data, 0x5c, 0x80);
+		err |= lt6911uxc_write_reg(priv->s_data, 0x5d, (i) * 32);
+		err |= lt6911uxc_write_reg(priv->s_data, 0x5a, 0x10);
+		msleep(1);
+		err |= lt6911uxc_write_reg(priv->s_data, 0x5a, 0x00);
+		msleep(1);
+	}
+	err |= lt6911uxc_write_table(priv, lt_write_disable);
+	if (err)
+		dev_err(tc_dev->dev, "%s Error in writing shadow EDID \n", __func__);
+
+	// Toggle the reset pin to low, stream on will again set it to high
+	if (pw->reset_gpio) {
+		if (gpiod_cansleep(gpio_to_desc(pw->reset_gpio)))
+			gpio_set_value_cansleep(pw->reset_gpio, 0);
+		else
+			gpio_set_value(pw->reset_gpio, 0);
+	}
+	// Keep reset pin low for the changes to take effect
+	msleep(800);
+
+	return err;
+}
+
 static int lt6911uxc_board_setup(struct lt6911uxc *priv)
 {
 	struct camera_common_data *s_data = priv->s_data;
 	struct camera_common_pdata *pdata = s_data->pdata;
 	struct device *dev = s_data->dev;
+	u8 version;
+	int i = 0, fw_ver;
 	int err = 0;
 
 	if (pdata->mclk_name) {
@@ -409,11 +614,31 @@ static int lt6911uxc_board_setup(struct lt6911uxc *priv)
 		dev_err(dev, "error during power on sensor (%d)\n", err);
 		goto err_power_on;
 	}
+	msleep(300);
+
+	err |= lt6911uxc_write_table(priv, lt_enable_i2c);
+	err |= lt6911uxc_write_reg(s_data, 0xff, 0x86);
+
+	for (i = 0; i < sizeof(lt6911uxc_FW_version); i++) {
+		fw_ver = 0;
+		err = lt6911uxc_read_reg(s_data, lt6911uxc_FW_version[i], &version);
+		if (err)
+			dev_err(s_data->dev, "%s: Error in reading FW version \n",
+			__func__);
+		fw_ver = (fw_ver << 1) | version;
+	}
+	dev_info(s_data->dev, "Lontium FW version %d \n",fw_ver);
 
 err_power_on:
 	if (pdata->mclk_name)
 		camera_common_mclk_disable(s_data);
 
+	err = lt6911uxc_power_off(s_data);
+	if (err) {
+		dev_err(dev, "error during power off sensor (%d)\n", err);
+		goto err_power_on;
+	}
+
 done:
 	return err;
 }