fixup: moduleparam.h/export.h (dsi.c)
[linux-2.6.git] / drivers / video / tegra / dc / dsi.c
index 62aa1ab..d126ecf 100644 (file)
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/workqueue.h>
+#include <linux/moduleparam.h>
+#include <linux/export.h>
 
 #include <mach/clk.h>
 #include <mach/dc.h>
 #include <mach/fb.h>
-#include <mach/nvhost.h>
-#include <../gpio-names.h>
+#include <mach/csi.h>
+#include <linux/nvhost.h>
 
 #include "dc_reg.h"
 #include "dc_priv.h"
 #include "dsi_regs.h"
 #include "dsi.h"
 
-#define DSI_USE_SYNC_POINTS 1
-
-#define DSI_STOP_DC_DURATION_MSEC 1000
+#define DSI_USE_SYNC_POINTS            1
+#define S_TO_MS(x)                     (1000 * (x))
 
 #define DSI_MODULE_NOT_INIT            0x0
 #define DSI_MODULE_INIT                        0x1
 #define DSI_DC_STREAM_DISABLE          0x0
 #define DSI_DC_STREAM_ENABLE           0x1
 
+#define DSI_LP_OP_NOT_INIT             0x0
+#define DSI_LP_OP_WRITE                        0x1
+#define DSI_LP_OP_READ                 0x2
+
+static bool enable_read_debug;
+module_param(enable_read_debug, bool, 0644);
+MODULE_PARM_DESC(enable_read_debug,
+               "Enable to print read fifo and return packet type");
+
 struct dsi_status {
-       unsigned        init:2;
+       unsigned init:2;
 
-       unsigned        lphs:2;
+       unsigned lphs:2;
 
-       unsigned        vtype:2;
-       unsigned        driven:2;
+       unsigned vtype:2;
+       unsigned driven:2;
 
-       unsigned        clk_out:2;
-       unsigned        clk_mode:2;
-       unsigned        clk_burst:2;
+       unsigned clk_out:2;
+       unsigned clk_mode:2;
+       unsigned clk_burst:2;
 
-       unsigned        dc_stream:1;
+       unsigned lp_op:2;
+
+       unsigned dc_stream:1;
 };
 
 /* source of video data */
-enum{
+enum {
        TEGRA_DSI_DRIVEN_BY_DC,
        TEGRA_DSI_DRIVEN_BY_HOST,
 };
 
 struct tegra_dc_dsi_data {
-       struct tegra_dc *dc;
-       void __iomem    *base;
-       struct resource *base_res;
+       struct tegra_dc *dc;
+       void __iomem *base;
+       struct resource *base_res;
 
-       struct clk              *dc_clk;
-       struct clk              *dsi_clk;
+       struct clk *dc_clk;
+       struct clk *dsi_clk;
+       bool clk_ref;
 
-       struct mutex    lock;
+       struct mutex lock;
 
        /* data from board info */
        struct tegra_dsi_out info;
 
-       struct dsi_status       status;
+       struct dsi_status status;
 
-       u8              driven_mode;
-       u8              controller_index;
+       struct dsi_phy_timing_inclk phy_timing;
 
-       u8              pixel_scaler_mul;
-       u8              pixel_scaler_div;
+       u8 driven_mode;
+       u8 controller_index;
 
-       u32             default_shift_clk_div;
-       u32             default_pixel_clk_khz;
-       u32             default_hs_clk_khz;
+       u8 pixel_scaler_mul;
+       u8 pixel_scaler_div;
 
-       u32             shift_clk_div;
-       u32             target_hs_clk_khz;
-       u32             target_lp_clk_khz;
+       u32 default_shift_clk_div;
+       u32 default_pixel_clk_khz;
+       u32 default_hs_clk_khz;
 
-       u32             syncpt_id;
-       u32             syncpt_val;
+       u32 shift_clk_div;
+       u32 target_hs_clk_khz;
+       u32 target_lp_clk_khz;
+
+       u32 syncpt_id;
+       u32 syncpt_val;
 
-       u16             current_bit_clk_ns;
-       u32             current_dsi_clk_khz;
+       u16 current_bit_clk_ns;
+       u32 current_dsi_clk_khz;
 
-       u32             dsi_control_val;
+       u32 dsi_control_val;
 
-       bool            ulpm;
-       bool            enabled;
+       bool ulpm;
+       bool enabled;
 };
 
 const u32 dsi_pkt_seq_reg[NUMOF_PKT_SEQ] = {
@@ -154,15 +169,15 @@ const u32 dsi_pkt_seq_video_non_burst_syne[NUMOF_PKT_SEQ] = {
        PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(0) | PKT_LP,
        0,
        PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_BLNK) | PKT_LEN1(1) |
-               PKT_ID2(CMD_HE) | PKT_LEN2(0),
+       PKT_ID2(CMD_HE) | PKT_LEN2(0),
        PKT_ID3(CMD_BLNK) | PKT_LEN3(2) | PKT_ID4(CMD_RGB) | PKT_LEN4(3) |
-               PKT_ID5(CMD_BLNK) | PKT_LEN5(4),
+       PKT_ID5(CMD_BLNK) | PKT_LEN5(4),
        PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(0) | PKT_LP,
        0,
        PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_BLNK) | PKT_LEN1(1) |
-               PKT_ID2(CMD_HE) | PKT_LEN2(0),
+       PKT_ID2(CMD_HE) | PKT_LEN2(0),
        PKT_ID3(CMD_BLNK) | PKT_LEN3(2) | PKT_ID4(CMD_RGB) | PKT_LEN4(3) |
-               PKT_ID5(CMD_BLNK) | PKT_LEN5(4),
+       PKT_ID5(CMD_BLNK) | PKT_LEN5(4),
 };
 
 const u32 dsi_pkt_seq_video_non_burst[NUMOF_PKT_SEQ] = {
@@ -173,12 +188,12 @@ const u32 dsi_pkt_seq_video_non_burst[NUMOF_PKT_SEQ] = {
        PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(0) | PKT_LP,
        0,
        PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_BLNK) | PKT_LEN1(2) |
-               PKT_ID2(CMD_RGB) | PKT_LEN2(3),
+       PKT_ID2(CMD_RGB) | PKT_LEN2(3),
        PKT_ID3(CMD_BLNK) | PKT_LEN3(4),
        PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(0) | PKT_LP,
        0,
        PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_BLNK) | PKT_LEN1(2) |
-               PKT_ID2(CMD_RGB) | PKT_LEN2(3),
+       PKT_ID2(CMD_RGB) | PKT_LEN2(3),
        PKT_ID3(CMD_BLNK) | PKT_LEN3(4),
 };
 
@@ -217,7 +232,7 @@ static const u32 dsi_pkt_seq_video_burst_no_eot[NUMOF_PKT_SEQ] = {
 };
 
 /* TODO: verify with hw about this format */
-const u32 dsi_pkt_seq_cmd_mode [NUMOF_PKT_SEQ] = {
+const u32 dsi_pkt_seq_cmd_mode[NUMOF_PKT_SEQ] = {
        0,
        0,
        0,
@@ -233,7 +248,6 @@ const u32 dsi_pkt_seq_cmd_mode [NUMOF_PKT_SEQ] = {
 };
 
 const u32 init_reg[] = {
-       DSI_WR_DATA,
        DSI_INT_ENABLE,
        DSI_INT_STATUS,
        DSI_INT_MASK,
@@ -241,6 +255,10 @@ const u32 init_reg[] = {
        DSI_INIT_SEQ_DATA_1,
        DSI_INIT_SEQ_DATA_2,
        DSI_INIT_SEQ_DATA_3,
+       DSI_INIT_SEQ_DATA_4,
+       DSI_INIT_SEQ_DATA_5,
+       DSI_INIT_SEQ_DATA_6,
+       DSI_INIT_SEQ_DATA_7,
        DSI_DCS_CMDS,
        DSI_PKT_SEQ_0_LO,
        DSI_PKT_SEQ_1_LO,
@@ -269,17 +287,19 @@ const u32 init_reg[] = {
        DSI_PKT_LEN_6_7,
 };
 
-static inline unsigned long tegra_dsi_readl(struct tegra_dc_dsi_data *dsi,
-                                                                       u32 reg)
+inline unsigned long tegra_dsi_readl(struct tegra_dc_dsi_data *dsi, u32 reg)
 {
+       BUG_ON(!nvhost_module_powered(nvhost_get_host(dsi->dc->ndev)->dev));
        return readl(dsi->base + reg * 4);
 }
+EXPORT_SYMBOL(tegra_dsi_readl);
 
-static inline void tegra_dsi_writel(struct tegra_dc_dsi_data *dsi,u32 val,
-                                                                       u32 reg)
+inline void tegra_dsi_writel(struct tegra_dc_dsi_data *dsi, u32 val, u32 reg)
 {
+       BUG_ON(!nvhost_module_powered(nvhost_get_host(dsi->dc->ndev)->dev));
        writel(val, dsi->base + reg * 4);
 }
+EXPORT_SYMBOL(tegra_dsi_writel);
 
 static int tegra_dsi_syncpt(struct tegra_dc_dsi_data *dsi)
 {
@@ -289,17 +309,18 @@ static int tegra_dsi_syncpt(struct tegra_dc_dsi_data *dsi)
        ret = 0;
 
        dsi->syncpt_val = nvhost_syncpt_read(
-                       &dsi->dc->ndev->host->syncpt, dsi->syncpt_id);
+                       &nvhost_get_host(dsi->dc->ndev)->syncpt,
+                       dsi->syncpt_id);
 
        val = DSI_INCR_SYNCPT_COND(OP_DONE) |
                DSI_INCR_SYNCPT_INDX(dsi->syncpt_id);
        tegra_dsi_writel(dsi, val, DSI_INCR_SYNCPT);
 
        /* TODO: Use interrupt rather than polling */
-       ret = nvhost_syncpt_wait(&dsi->dc->ndev->host->syncpt,
+       ret = nvhost_syncpt_wait(&nvhost_get_host(dsi->dc->ndev)->syncpt,
                dsi->syncpt_id, dsi->syncpt_val + 1);
        if (ret < 0) {
-               printk(KERN_ERR "DSI sync point failure\n");
+               dev_err(&dsi->dc->ndev->dev, "DSI sync point failure\n");
                goto fail;
        }
 
@@ -334,7 +355,7 @@ static u32 tegra_dsi_get_hs_clk_rate(struct tegra_dc_dsi_data *dsi)
        return dsi_clock_rate_khz;
 }
 
-static u32 tegra_dsi_get_lp_clk_rate(struct tegra_dc_dsi_data *dsi)
+static u32 tegra_dsi_get_lp_clk_rate(struct tegra_dc_dsi_data *dsi, u8 lp_op)
 {
        u32 dsi_clock_rate_khz;
 
@@ -345,7 +366,12 @@ static u32 tegra_dsi_get_lp_clk_rate(struct tegra_dc_dsi_data *dsi)
                else
                        dsi_clock_rate_khz = tegra_dsi_get_hs_clk_rate(dsi);
        else
-               dsi_clock_rate_khz = dsi->info.lp_cmd_mode_freq_khz;
+               if (lp_op == DSI_LP_OP_READ)
+                       dsi_clock_rate_khz =
+                               dsi->info.lp_read_cmd_mode_freq_khz;
+               else
+                       dsi_clock_rate_khz =
+                               dsi->info.lp_cmd_mode_freq_khz;
 
        return dsi_clock_rate_khz;
 }
@@ -358,14 +384,16 @@ static u32 tegra_dsi_get_shift_clk_div(struct tegra_dc_dsi_data *dsi)
        u32 burst_width_max;
 
        /* Get the real value of default shift_clk_div. default_shift_clk_div
-        * holds the real value of shift_clk_div. */
+        * holds the real value of shift_clk_div.
+        */
        shift_clk_div = dsi->default_shift_clk_div;
 
-       /* Calculate shift_clk_div which can matche the video_burst_mode.*/
+       /* Calculate shift_clk_div which can matche the video_burst_mode. */
        if (dsi->info.video_burst_mode >=
                        TEGRA_DSI_VIDEO_BURST_MODE_LOWEST_SPEED) {
                /* The max_shift_clk_div is multiplied by 10 to save the
-                * fraction */
+                * fraction
+                */
                if (dsi->info.max_panel_freq_khz >= dsi->default_hs_clk_khz)
                        max_shift_clk_div = dsi->info.max_panel_freq_khz
                                * shift_clk_div * 10 / dsi->default_hs_clk_khz;
@@ -385,7 +413,7 @@ static u32 tegra_dsi_get_shift_clk_div(struct tegra_dc_dsi_data *dsi)
 }
 
 static void tegra_dsi_init_sw(struct tegra_dc *dc,
-                                               struct tegra_dc_dsi_data *dsi)
+                       struct tegra_dc_dsi_data *dsi)
 {
        u32 h_width_pixels;
        u32 v_width_lines;
@@ -394,29 +422,30 @@ static void tegra_dsi_init_sw(struct tegra_dc *dc,
        u32 plld_clk_mhz;
 
        switch (dsi->info.pixel_format) {
-               case TEGRA_DSI_PIXEL_FORMAT_16BIT_P:
-                       /* 2 bytes per pixel */
-                       dsi->pixel_scaler_mul = 2;
-                       dsi->pixel_scaler_div = 1;
-                       break;
-               case TEGRA_DSI_PIXEL_FORMAT_18BIT_P:
-                       /* 2.25 bytes per pixel */
-                       dsi->pixel_scaler_mul = 9;
-                       dsi->pixel_scaler_div = 4;
-                       break;
-               case TEGRA_DSI_PIXEL_FORMAT_18BIT_NP:
-               case TEGRA_DSI_PIXEL_FORMAT_24BIT_P:
-                       /* 3 bytes per pixel */
-                       dsi->pixel_scaler_mul = 3;
-                       dsi->pixel_scaler_div = 1;
-                       break;
-               default:
-                       break;
+       case TEGRA_DSI_PIXEL_FORMAT_16BIT_P:
+               /* 2 bytes per pixel */
+               dsi->pixel_scaler_mul = 2;
+               dsi->pixel_scaler_div = 1;
+               break;
+       case TEGRA_DSI_PIXEL_FORMAT_18BIT_P:
+               /* 2.25 bytes per pixel */
+               dsi->pixel_scaler_mul = 9;
+               dsi->pixel_scaler_div = 4;
+               break;
+       case TEGRA_DSI_PIXEL_FORMAT_18BIT_NP:
+       case TEGRA_DSI_PIXEL_FORMAT_24BIT_P:
+               /* 3 bytes per pixel */
+               dsi->pixel_scaler_mul = 3;
+               dsi->pixel_scaler_div = 1;
+               break;
+       default:
+               break;
        }
 
        dsi->controller_index = dc->ndev->id;
        dsi->ulpm = false;
        dsi->enabled = false;
+       dsi->clk_ref = false;
 
        dsi->dsi_control_val =
                        DSI_CONTROL_VIRTUAL_CHANNEL(dsi->info.virtual_channel) |
@@ -425,7 +454,8 @@ static void tegra_dsi_init_sw(struct tegra_dc *dc,
                        DSI_CONTROL_DATA_FORMAT(dsi->info.pixel_format);
 
        /* Below we are going to calculate dsi and dc clock rate.
-        * Calcuate the horizontal and vertical width. */
+        * Calcuate the horizontal and vertical width.
+        */
        h_width_pixels = dc->mode.h_back_porch + dc->mode.h_front_porch +
                        dc->mode.h_sync_width + dc->mode.h_active;
        v_width_lines = dc->mode.v_back_porch + dc->mode.v_front_porch +
@@ -433,8 +463,13 @@ static void tegra_dsi_init_sw(struct tegra_dc *dc,
 
        /* Calculate minimum required pixel rate. */
        pixel_clk_hz = h_width_pixels * v_width_lines * dsi->info.refresh_rate;
-
-       dc->pixel_clk = pixel_clk_hz;
+       if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE) {
+               if (dsi->info.rated_refresh_rate >= dsi->info.refresh_rate)
+                       dev_info(&dc->ndev->dev, "DSI: measured refresh rate "
+                               "should be larger than rated refresh rate.\n");
+               dc->mode.rated_pclk = h_width_pixels * v_width_lines *
+                                               dsi->info.rated_refresh_rate;
+       }
 
        /* Calculate minimum byte rate on DSI interface. */
        byte_clk_hz = (pixel_clk_hz * dsi->pixel_scaler_mul) /
@@ -451,21 +486,21 @@ static void tegra_dsi_init_sw(struct tegra_dc *dc,
        /* Calculate default DSI hs clock. DSI interface is double data rate.
         * Data is transferred on both rising and falling edge of clk, div by 2
         * to get the actual clock rate.
-        * */
+        */
        dsi->default_hs_clk_khz = plld_clk_mhz * 1000 / 2;
        dsi->default_pixel_clk_khz = plld_clk_mhz * 1000 / 2
                                                / dsi->default_shift_clk_div;
 
        /* Get the actual shift_clk_div and clock rates. */
        dsi->shift_clk_div = tegra_dsi_get_shift_clk_div(dsi);
-       dsi->target_lp_clk_khz = tegra_dsi_get_lp_clk_rate(dsi);
+       dsi->target_lp_clk_khz =
+                       tegra_dsi_get_lp_clk_rate(dsi, DSI_LP_OP_WRITE);
        dsi->target_hs_clk_khz = tegra_dsi_get_hs_clk_rate(dsi);
 
        dev_info(&dc->ndev->dev, "DSI: HS clock rate is %d\n",
                                                        dsi->target_hs_clk_khz);
 
        dsi->controller_index = dc->ndev->id;
-       dsi->ulpm = false;
 
 #if DSI_USE_SYNC_POINTS
        dsi->syncpt_id = NVSYNCPT_DSI;
@@ -476,71 +511,410 @@ static void tegra_dsi_init_sw(struct tegra_dc *dc,
         * enable_hs_clock_on_lp_cmd_mode is set
         */
        if (dsi->info.enable_hs_clock_on_lp_cmd_mode) {
-               if (dsi->info.video_clock_mode != TEGRA_DSI_VIDEO_CLOCK_CONTINUOUS)
-                       printk("Force to clock continuous mode\n");
+               if (dsi->info.video_clock_mode !=
+                                       TEGRA_DSI_VIDEO_CLOCK_CONTINUOUS)
+                       dev_warn(&dc->ndev->dev,
+                               "Force clock continuous mode\n");
 
                dsi->info.video_clock_mode = TEGRA_DSI_VIDEO_CLOCK_CONTINUOUS;
        }
 
 }
 
+#define SELECT_T_PHY(platform_t_phy_ns, default_phy, clk_ns, hw_inc) ( \
+(platform_t_phy_ns) ? ( \
+((DSI_CONVERT_T_PHY_NS_TO_T_PHY(platform_t_phy_ns, clk_ns, hw_inc)) < 0 ? 0 : \
+(DSI_CONVERT_T_PHY_NS_TO_T_PHY(platform_t_phy_ns, clk_ns, hw_inc)))) : \
+((default_phy) < 0 ? 0 : (default_phy)))
+
+static void tegra_dsi_get_clk_phy_timing(struct tegra_dc_dsi_data *dsi,
+               struct dsi_phy_timing_inclk *phy_timing_clk, u32 clk_ns)
+{
+       phy_timing_clk->t_tlpx = SELECT_T_PHY(
+               dsi->info.phy_timing.t_tlpx_ns,
+               T_TLPX_DEFAULT(clk_ns), clk_ns, T_TLPX_HW_INC);
+
+       phy_timing_clk->t_clktrail = SELECT_T_PHY(
+               dsi->info.phy_timing.t_clktrail_ns,
+               T_CLKTRAIL_DEFAULT(clk_ns), clk_ns, T_CLKTRAIL_HW_INC);
+
+       phy_timing_clk->t_clkpost = SELECT_T_PHY(
+               dsi->info.phy_timing.t_clkpost_ns,
+               T_CLKPOST_DEFAULT(clk_ns), clk_ns, T_CLKPOST_HW_INC);
+
+       phy_timing_clk->t_clkzero = SELECT_T_PHY(
+               dsi->info.phy_timing.t_clkzero_ns,
+               T_CLKZERO_DEFAULT(clk_ns), clk_ns, T_CLKZERO_HW_INC);
+
+       phy_timing_clk->t_clkprepare = SELECT_T_PHY(
+               dsi->info.phy_timing.t_clkprepare_ns,
+               T_CLKPREPARE_DEFAULT(clk_ns), clk_ns, T_CLKPREPARE_HW_INC);
+
+       phy_timing_clk->t_clkpre = SELECT_T_PHY(
+               dsi->info.phy_timing.t_clkpre_ns,
+               T_CLKPRE_DEFAULT, clk_ns, T_CLKPRE_HW_INC);
+}
+
+static void tegra_dsi_get_hs_phy_timing(struct tegra_dc_dsi_data *dsi,
+               struct dsi_phy_timing_inclk *phy_timing_clk, u32 clk_ns)
+{
+       phy_timing_clk->t_tlpx = SELECT_T_PHY(
+               dsi->info.phy_timing.t_tlpx_ns,
+               T_TLPX_DEFAULT(clk_ns), clk_ns, T_TLPX_HW_INC);
+
+       phy_timing_clk->t_hsdexit = SELECT_T_PHY(
+               dsi->info.phy_timing.t_hsdexit_ns,
+               T_HSEXIT_DEFAULT(clk_ns), clk_ns, T_HSEXIT_HW_INC);
+
+       phy_timing_clk->t_hstrail = SELECT_T_PHY(
+               dsi->info.phy_timing.t_hstrail_ns,
+               T_HSTRAIL_DEFAULT(clk_ns), clk_ns, T_HSTRAIL_HW_INC);
+
+       phy_timing_clk->t_datzero = SELECT_T_PHY(
+               dsi->info.phy_timing.t_datzero_ns,
+               T_DATZERO_DEFAULT(clk_ns), clk_ns, T_DATZERO_HW_INC);
+
+       phy_timing_clk->t_hsprepare = SELECT_T_PHY(
+               dsi->info.phy_timing.t_hsprepare_ns,
+               T_HSPREPARE_DEFAULT(clk_ns), clk_ns, T_HSPREPARE_HW_INC);
+}
+
+static void tegra_dsi_get_escape_phy_timing(struct tegra_dc_dsi_data *dsi,
+               struct dsi_phy_timing_inclk *phy_timing_clk, u32 clk_ns)
+{
+       phy_timing_clk->t_tlpx = SELECT_T_PHY(
+               dsi->info.phy_timing.t_tlpx_ns,
+               T_TLPX_DEFAULT(clk_ns), clk_ns, T_TLPX_HW_INC);
+}
+
+static void tegra_dsi_get_bta_phy_timing(struct tegra_dc_dsi_data *dsi,
+               struct dsi_phy_timing_inclk *phy_timing_clk, u32 clk_ns)
+{
+       phy_timing_clk->t_tlpx = SELECT_T_PHY(
+               dsi->info.phy_timing.t_tlpx_ns,
+               T_TLPX_DEFAULT(clk_ns), clk_ns, T_TLPX_HW_INC);
+
+       phy_timing_clk->t_taget = SELECT_T_PHY(
+               dsi->info.phy_timing.t_taget_ns,
+               T_TAGET_DEFAULT(clk_ns), clk_ns, T_TAGET_HW_INC);
+
+       phy_timing_clk->t_tasure = SELECT_T_PHY(
+               dsi->info.phy_timing.t_tasure_ns,
+               T_TASURE_DEFAULT(clk_ns), clk_ns, T_TASURE_HW_INC);
+
+       phy_timing_clk->t_tago = SELECT_T_PHY(
+               dsi->info.phy_timing.t_tago_ns,
+               T_TAGO_DEFAULT(clk_ns), clk_ns, T_TAGO_HW_INC);
+}
+
+static void tegra_dsi_get_ulps_phy_timing(struct tegra_dc_dsi_data *dsi,
+               struct dsi_phy_timing_inclk *phy_timing_clk, u32 clk_ns)
+{
+       phy_timing_clk->t_tlpx = SELECT_T_PHY(
+               dsi->info.phy_timing.t_tlpx_ns,
+               T_TLPX_DEFAULT(clk_ns), clk_ns, T_TLPX_HW_INC);
+
+       phy_timing_clk->t_wakeup = SELECT_T_PHY(
+               dsi->info.phy_timing.t_wakeup_ns,
+               T_WAKEUP_DEFAULT, clk_ns, T_WAKEUP_HW_INC);
+}
+
+#undef SELECT_T_PHY
+
 static void tegra_dsi_get_phy_timing(struct tegra_dc_dsi_data *dsi,
                                struct dsi_phy_timing_inclk *phy_timing_clk,
-                               u32 clk_ns)
+                               u32 clk_ns, u8 lphs)
 {
+       if (lphs == DSI_LPHS_IN_HS_MODE) {
+               tegra_dsi_get_clk_phy_timing(dsi, phy_timing_clk, clk_ns);
+               tegra_dsi_get_hs_phy_timing(dsi, phy_timing_clk, clk_ns);
+       } else {
+               /* default is LP mode */
+               tegra_dsi_get_escape_phy_timing(dsi, phy_timing_clk, clk_ns);
+               tegra_dsi_get_bta_phy_timing(dsi, phy_timing_clk, clk_ns);
+               tegra_dsi_get_ulps_phy_timing(dsi, phy_timing_clk, clk_ns);
+               if (dsi->info.enable_hs_clock_on_lp_cmd_mode)
+                       tegra_dsi_get_clk_phy_timing
+                               (dsi, phy_timing_clk, clk_ns);
+       }
+}
+
+static int tegra_dsi_mipi_phy_timing_range(struct tegra_dc_dsi_data *dsi,
+                               struct dsi_phy_timing_inclk *phy_timing,
+                               u32 clk_ns, u8 lphs)
+{
+#define CHECK_RANGE(val, min, max) ( \
+               ((min) == NOT_DEFINED ? 0 : (val) < (min)) || \
+               ((max) == NOT_DEFINED ? 0 : (val) > (max)) ? -EINVAL : 0)
+
+       int err = 0;
+
+       err = CHECK_RANGE(
+       DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+                       phy_timing->t_tlpx, clk_ns, T_TLPX_HW_INC),
+                       MIPI_T_TLPX_NS_MIN, MIPI_T_TLPX_NS_MAX);
+       if (err < 0) {
+               dev_warn(&dsi->dc->ndev->dev,
+                       "dsi: Tlpx mipi range violated\n");
+               goto fail;
+       }
+
+       if (lphs == DSI_LPHS_IN_HS_MODE) {
+               err = CHECK_RANGE(
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+                       phy_timing->t_hsdexit, clk_ns, T_HSEXIT_HW_INC),
+                       MIPI_T_HSEXIT_NS_MIN, MIPI_T_HSEXIT_NS_MAX);
+               if (err < 0) {
+                       dev_warn(&dsi->dc->ndev->dev,
+                               "dsi: HsExit mipi range violated\n");
+                       goto fail;
+               }
+
+               err = CHECK_RANGE(
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+                       phy_timing->t_hstrail, clk_ns, T_HSTRAIL_HW_INC),
+                       MIPI_T_HSTRAIL_NS_MIN(clk_ns), MIPI_T_HSTRAIL_NS_MAX);
+               if (err < 0) {
+                       dev_warn(&dsi->dc->ndev->dev,
+                               "dsi: HsTrail mipi range violated\n");
+                       goto fail;
+               }
+
+               err = CHECK_RANGE(
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+                       phy_timing->t_datzero, clk_ns, T_DATZERO_HW_INC),
+                       MIPI_T_HSZERO_NS_MIN, MIPI_T_HSZERO_NS_MAX);
+               if (err < 0) {
+                       dev_warn(&dsi->dc->ndev->dev,
+                               "dsi: HsZero mipi range violated\n");
+                       goto fail;
+               }
+
+               err = CHECK_RANGE(
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+                       phy_timing->t_hsprepare, clk_ns, T_HSPREPARE_HW_INC),
+                       MIPI_T_HSPREPARE_NS_MIN(clk_ns),
+                       MIPI_T_HSPREPARE_NS_MAX(clk_ns));
+               if (err < 0) {
+                       dev_warn(&dsi->dc->ndev->dev,
+                               "dsi: HsPrepare mipi range violated\n");
+                       goto fail;
+               }
+
+               err = CHECK_RANGE(
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+               phy_timing->t_hsprepare, clk_ns, T_HSPREPARE_HW_INC) +
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+                       phy_timing->t_datzero, clk_ns, T_DATZERO_HW_INC),
+                       MIPI_T_HSPREPARE_ADD_HSZERO_NS_MIN(clk_ns),
+                       MIPI_T_HSPREPARE_ADD_HSZERO_NS_MAX);
+               if (err < 0) {
+                       dev_warn(&dsi->dc->ndev->dev,
+                       "dsi: HsPrepare + HsZero mipi range violated\n");
+                       goto fail;
+               }
+       } else {
+               /* default is LP mode */
+               err = CHECK_RANGE(
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+                       phy_timing->t_wakeup, clk_ns, T_WAKEUP_HW_INC),
+                       MIPI_T_WAKEUP_NS_MIN, MIPI_T_WAKEUP_NS_MAX);
+               if (err < 0) {
+                       dev_warn(&dsi->dc->ndev->dev,
+                               "dsi: WakeUp mipi range violated\n");
+                       goto fail;
+               }
 
-       phy_timing_clk->t_hsdexit = dsi->info.phy_timing.t_hsdexit_ns ?
-                       (dsi->info.phy_timing.t_hsdexit_ns / clk_ns) :
-                       (T_HSEXIT_DEFAULT(clk_ns));
+               err = CHECK_RANGE(
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+                       phy_timing->t_tasure, clk_ns, T_TASURE_HW_INC),
+                       MIPI_T_TASURE_NS_MIN(DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+                       phy_timing->t_tlpx, clk_ns, T_TLPX_HW_INC)),
+                       MIPI_T_TASURE_NS_MAX(DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+                       phy_timing->t_tlpx, clk_ns, T_TLPX_HW_INC)));
+               if (err < 0) {
+                       dev_warn(&dsi->dc->ndev->dev,
+                               "dsi: TaSure mipi range violated\n");
+                       goto fail;
+               }
+       }
 
-       phy_timing_clk->t_hstrail = dsi->info.phy_timing.t_hstrail_ns ?
-                       (dsi->info.phy_timing.t_hstrail_ns / clk_ns) :
-                       (T_HSTRAIL_DEFAULT(clk_ns));
+       if (lphs == DSI_LPHS_IN_HS_MODE ||
+               dsi->info.enable_hs_clock_on_lp_cmd_mode) {
+               err = CHECK_RANGE(
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+                       phy_timing->t_clktrail, clk_ns, T_CLKTRAIL_HW_INC),
+                       MIPI_T_CLKTRAIL_NS_MIN, MIPI_T_CLKTRAIL_NS_MAX);
+               if (err < 0) {
+                       dev_warn(&dsi->dc->ndev->dev,
+                               "dsi: ClkTrail mipi range violated\n");
+                       goto fail;
+               }
 
-       phy_timing_clk->t_datzero = dsi->info.phy_timing.t_datzero_ns ?
-                       (dsi->info.phy_timing.t_datzero_ns / clk_ns) :
-                       (T_DATZERO_DEFAULT(clk_ns));
+               err = CHECK_RANGE(
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+                       phy_timing->t_clkpost, clk_ns, T_CLKPOST_HW_INC),
+                       MIPI_T_CLKPOST_NS_MIN(clk_ns), MIPI_T_CLKPOST_NS_MAX);
+               if (err < 0) {
+                       dev_warn(&dsi->dc->ndev->dev,
+                               "dsi: ClkPost mipi range violated\n");
+                       goto fail;
+               }
 
-       phy_timing_clk->t_hsprepr = dsi->info.phy_timing.t_hsprepr_ns ?
-                       (dsi->info.phy_timing.t_hsprepr_ns / clk_ns) :
-                       (T_HSPREPR_DEFAULT(clk_ns));
+               err = CHECK_RANGE(
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+                       phy_timing->t_clkzero, clk_ns, T_CLKZERO_HW_INC),
+                       MIPI_T_CLKZERO_NS_MIN, MIPI_T_CLKZERO_NS_MAX);
+               if (err < 0) {
+                       dev_warn(&dsi->dc->ndev->dev,
+                               "dsi: ClkZero mipi range violated\n");
+                       goto fail;
+               }
 
-       phy_timing_clk->t_clktrail = dsi->info.phy_timing.t_clktrail_ns ?
-                               (dsi->info.phy_timing.t_clktrail_ns / clk_ns) :
-                               (T_CLKTRAIL_DEFAULT(clk_ns));
+               err = CHECK_RANGE(
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+                       phy_timing->t_clkprepare, clk_ns, T_CLKPREPARE_HW_INC),
+                       MIPI_T_CLKPREPARE_NS_MIN, MIPI_T_CLKPREPARE_NS_MAX);
+               if (err < 0) {
+                       dev_warn(&dsi->dc->ndev->dev,
+                               "dsi: ClkPrepare mipi range violated\n");
+                       goto fail;
+               }
 
-       phy_timing_clk->t_clkpost = dsi->info.phy_timing.t_clkpost_ns ?
-                               (dsi->info.phy_timing.t_clkpost_ns / clk_ns) :
-                               (T_CLKPOST_DEFAULT(clk_ns));
+               err = CHECK_RANGE(
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+                       phy_timing->t_clkpre, clk_ns, T_CLKPRE_HW_INC),
+                       MIPI_T_CLKPRE_NS_MIN, MIPI_T_CLKPRE_NS_MAX);
+               if (err < 0) {
+                       dev_warn(&dsi->dc->ndev->dev,
+                               "dsi: ClkPre mipi range violated\n");
+                       goto fail;
+               }
 
-       phy_timing_clk->t_clkzero = dsi->info.phy_timing.t_clkzero_ns ?
-                               (dsi->info.phy_timing.t_clkzero_ns / clk_ns) :
-                               (T_CLKZERO_DEFAULT(clk_ns));
+               err = CHECK_RANGE(
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+               phy_timing->t_clkprepare, clk_ns, T_CLKPREPARE_HW_INC) +
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+                       phy_timing->t_clkzero, clk_ns, T_CLKZERO_HW_INC),
+                       MIPI_T_CLKPREPARE_ADD_CLKZERO_NS_MIN,
+                       MIPI_T_CLKPREPARE_ADD_CLKZERO_NS_MAX);
+               if (err < 0) {
+                       dev_warn(&dsi->dc->ndev->dev,
+                       "dsi: ClkPrepare + ClkZero mipi range violated\n");
+                       goto fail;
+               }
+       }
+fail:
+#undef CHECK_RANGE
+       return err;
+}
 
-       phy_timing_clk->t_tlpx = dsi->info.phy_timing.t_tlpx_ns ?
-                               (dsi->info.phy_timing.t_tlpx_ns / clk_ns) :
-                               (T_TLPX_DEFAULT(clk_ns));
+static int tegra_dsi_hs_phy_len(struct tegra_dc_dsi_data *dsi,
+                               struct dsi_phy_timing_inclk *phy_timing,
+                               u32 clk_ns, u8 lphs)
+{
+       u32 hs_t_phy_ns;
+       u32 clk_t_phy_ns;
+       u32 t_phy_ns;
+       u32 h_blank_ns;
+       struct tegra_dc_mode *modes;
+       u32 t_pix_ns;
+       int err = 0;
+
+       if (!(lphs == DSI_LPHS_IN_HS_MODE))
+               goto fail;
 
-       phy_timing_clk->t_clkpre = T_CLKPRE_DEFAULT(clk_ns);
-       phy_timing_clk->t_clkprepare = T_CLKPREPARE_DEFAULT(clk_ns);
-       phy_timing_clk->t_wakeup = T_WAKEUP_DEFAULT(clk_ns);
+       modes = dsi->dc->out->modes;
+       t_pix_ns = clk_ns * BITS_PER_BYTE *
+               dsi->pixel_scaler_mul / dsi->pixel_scaler_div;
+
+       hs_t_phy_ns =
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+               phy_timing->t_tlpx, clk_ns, T_TLPX_HW_INC) +
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+               phy_timing->t_hsprepare, clk_ns, T_HSPREPARE_HW_INC) +
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+               phy_timing->t_datzero, clk_ns, T_DATZERO_HW_INC) +
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+               phy_timing->t_hstrail, clk_ns, T_HSTRAIL_HW_INC) +
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+               phy_timing->t_hsdexit, clk_ns, T_HSEXIT_HW_INC);
+
+       clk_t_phy_ns =
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+               phy_timing->t_clkpost, clk_ns, T_CLKPOST_HW_INC) +
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+               phy_timing->t_clktrail, clk_ns, T_CLKTRAIL_HW_INC) +
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+               phy_timing->t_hsdexit, clk_ns, T_HSEXIT_HW_INC) +
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+               phy_timing->t_tlpx, clk_ns, T_TLPX_HW_INC) +
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+               phy_timing->t_clkprepare, clk_ns, T_CLKPREPARE_HW_INC) +
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+               phy_timing->t_clkzero, clk_ns, T_CLKZERO_HW_INC) +
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+               phy_timing->t_clkpre, clk_ns, T_CLKPRE_HW_INC);
+
+       h_blank_ns = t_pix_ns * (modes->h_sync_width + modes->h_back_porch +
+                                               modes->h_front_porch);
+
+       /* Extra tlpx and byte cycle required by dsi HW */
+       t_phy_ns = dsi->info.n_data_lanes * (hs_t_phy_ns + clk_t_phy_ns +
+               DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+               phy_timing->t_tlpx, clk_ns, T_TLPX_HW_INC) +
+               clk_ns * BITS_PER_BYTE);
+
+       if (h_blank_ns < t_phy_ns) {
+               err = -EINVAL;
+               dev_err(&dsi->dc->ndev->dev,
+                       "dsi: Hblank is smaller than HS trans phy timing\n");
+               goto fail;
+       }
 
-       phy_timing_clk->t_taget = 5 * phy_timing_clk->t_tlpx;
-       phy_timing_clk->t_tasure = 2 * phy_timing_clk->t_tlpx;
-       phy_timing_clk->t_tago = 4 * phy_timing_clk->t_tlpx;
+       return 0;
+fail:
+       return err;
 }
 
-static void tegra_dsi_set_phy_timing(struct tegra_dc_dsi_data *dsi)
+static int tegra_dsi_constraint_phy_timing(struct tegra_dc_dsi_data *dsi,
+                               struct dsi_phy_timing_inclk *phy_timing,
+                               u32 clk_ns, u8 lphs)
+{
+       int err = 0;
+
+       err = tegra_dsi_mipi_phy_timing_range(dsi, phy_timing, clk_ns, lphs);
+       if (err < 0) {
+               dev_warn(&dsi->dc->ndev->dev, "dsi: mipi range violated\n");
+               goto fail;
+       }
+
+       err = tegra_dsi_hs_phy_len(dsi, phy_timing, clk_ns, lphs);
+       if (err < 0) {
+               dev_err(&dsi->dc->ndev->dev, "dsi: Hblank too short\n");
+               goto fail;
+       }
+
+       /* TODO: add more contraints */
+fail:
+       return err;
+}
+
+static void tegra_dsi_set_phy_timing(struct tegra_dc_dsi_data *dsi, u8 lphs)
 {
        u32 val;
-       struct dsi_phy_timing_inclk     phy_timing;
+       struct dsi_phy_timing_inclk phy_timing = dsi->phy_timing;
+
+       tegra_dsi_get_phy_timing
+               (dsi, &phy_timing, dsi->current_bit_clk_ns, lphs);
 
-       tegra_dsi_get_phy_timing(dsi, &phy_timing, dsi->current_bit_clk_ns);
+       tegra_dsi_constraint_phy_timing(dsi, &phy_timing,
+                                       dsi->current_bit_clk_ns, lphs);
 
        val = DSI_PHY_TIMING_0_THSDEXIT(phy_timing.t_hsdexit) |
                        DSI_PHY_TIMING_0_THSTRAIL(phy_timing.t_hstrail) |
                        DSI_PHY_TIMING_0_TDATZERO(phy_timing.t_datzero) |
-                       DSI_PHY_TIMING_0_THSPREPR(phy_timing.t_hsprepr);
+                       DSI_PHY_TIMING_0_THSPREPR(phy_timing.t_hsprepare);
        tegra_dsi_writel(dsi, val, DSI_PHY_TIMING_0);
 
        val = DSI_PHY_TIMING_1_TCLKTRAIL(phy_timing.t_clktrail) |
@@ -550,7 +924,7 @@ static void tegra_dsi_set_phy_timing(struct tegra_dc_dsi_data *dsi)
        tegra_dsi_writel(dsi, val, DSI_PHY_TIMING_1);
 
        val = DSI_PHY_TIMING_2_TCLKPREPARE(phy_timing.t_clkprepare) |
-                DSI_PHY_TIMING_2_TCLKPRE(phy_timing.t_clkpre) |
+               DSI_PHY_TIMING_2_TCLKPRE(phy_timing.t_clkpre) |
                        DSI_PHY_TIMING_2_TWAKEUP(phy_timing.t_wakeup);
        tegra_dsi_writel(dsi, val, DSI_PHY_TIMING_2);
 
@@ -558,10 +932,12 @@ static void tegra_dsi_set_phy_timing(struct tegra_dc_dsi_data *dsi)
                        DSI_BTA_TIMING_TTASURE(phy_timing.t_tasure) |
                        DSI_BTA_TIMING_TTAGO(phy_timing.t_tago);
        tegra_dsi_writel(dsi, val, DSI_BTA_TIMING);
+
+       dsi->phy_timing = phy_timing;
 }
 
 static u32 tegra_dsi_sol_delay_burst(struct tegra_dc *dc,
-                                               struct tegra_dc_dsi_data *dsi)
+                               struct tegra_dc_dsi_data *dsi)
 {
        u32 dsi_to_pixel_clk_ratio;
        u32 temp;
@@ -570,7 +946,7 @@ static u32 tegra_dsi_sol_delay_burst(struct tegra_dc *dc,
        u32 sol_delay;
        struct tegra_dc_mode *dc_modes = &dc->mode;
 
-       /* Get Fdsi/Fpixel ration (note: Fdsi si in bit format) */
+       /* Get Fdsi/Fpixel ration (note: Fdsi is in bit format) */
        dsi_to_pixel_clk_ratio = (dsi->current_dsi_clk_khz * 2 +
                dsi->default_pixel_clk_khz - 1) / dsi->default_pixel_clk_khz;
 
@@ -610,15 +986,17 @@ static u32 tegra_dsi_sol_delay_burst(struct tegra_dc *dc,
 }
 
 static void tegra_dsi_set_sol_delay(struct tegra_dc *dc,
-                                               struct tegra_dc_dsi_data *dsi)
+                               struct tegra_dc_dsi_data *dsi)
 {
        u32 sol_delay;
 
        if (dsi->info.video_burst_mode == TEGRA_DSI_VIDEO_NONE_BURST_MODE ||
                dsi->info.video_burst_mode ==
                                TEGRA_DSI_VIDEO_NONE_BURST_MODE_WITH_SYNC_END) {
-               sol_delay = NUMOF_BIT_PER_BYTE * dsi->pixel_scaler_mul /
-                       (dsi->pixel_scaler_div * dsi->info.n_data_lanes);
+#define VIDEO_FIFO_LATENCY_PIXEL_CLK 8
+               sol_delay = VIDEO_FIFO_LATENCY_PIXEL_CLK *
+                       dsi->pixel_scaler_mul / dsi->pixel_scaler_div;
+#undef VIDEO_FIFO_LATENCY_PIXEL_CLK
                dsi->status.clk_burst = DSI_CLK_BURST_NONE_BURST;
        } else {
                sol_delay = tegra_dsi_sol_delay_burst(dc, dsi);
@@ -635,7 +1013,7 @@ static void tegra_dsi_set_timeout(struct tegra_dc_dsi_data *dsi)
        u32 bytes_per_frame;
        u32 timeout = 0;
 
-       /* TODO: verify the following eq */
+       /* TODO: verify the following equation */
        bytes_per_frame = dsi->current_dsi_clk_khz * 1000 * 2 /
                                                (dsi->info.refresh_rate * 8);
        timeout = bytes_per_frame / DSI_CYCLE_COUNTER_VALUE;
@@ -688,14 +1066,16 @@ static void tegra_dsi_setup_video_mode_pkt_length(struct tegra_dc *dc,
        hbp_pkt_len -= DSI_HBACK_PORCH_PKT_OVERHEAD;
        hfp_pkt_len -= DSI_HFRONT_PORCH_PKT_OVERHEAD;
 
-       val = DSI_PKT_LEN_0_1_LENGTH_0(0) | DSI_PKT_LEN_0_1_LENGTH_1(hsa_pkt_len);
+       val = DSI_PKT_LEN_0_1_LENGTH_0(0) |
+                       DSI_PKT_LEN_0_1_LENGTH_1(hsa_pkt_len);
        tegra_dsi_writel(dsi, val, DSI_PKT_LEN_0_1);
 
        val = DSI_PKT_LEN_2_3_LENGTH_2(hbp_pkt_len) |
                        DSI_PKT_LEN_2_3_LENGTH_3(hact_pkt_len);
        tegra_dsi_writel(dsi, val, DSI_PKT_LEN_2_3);
 
-       val = DSI_PKT_LEN_4_5_LENGTH_4(hfp_pkt_len) | DSI_PKT_LEN_4_5_LENGTH_5(0);
+       val = DSI_PKT_LEN_4_5_LENGTH_4(hfp_pkt_len) |
+                       DSI_PKT_LEN_4_5_LENGTH_5(0);
        tegra_dsi_writel(dsi, val, DSI_PKT_LEN_4_5);
 
        val = DSI_PKT_LEN_6_7_LENGTH_6(0) | DSI_PKT_LEN_6_7_LENGTH_7(0);
@@ -725,7 +1105,7 @@ static void tegra_dsi_setup_cmd_mode_pkt_length(struct tegra_dc *dc,
 }
 
 static void tegra_dsi_set_pkt_length(struct tegra_dc *dc,
-                                               struct tegra_dc_dsi_data *dsi)
+                               struct tegra_dc_dsi_data *dsi)
 {
        if (dsi->driven_mode == TEGRA_DSI_DRIVEN_BY_HOST)
                return;
@@ -737,7 +1117,7 @@ static void tegra_dsi_set_pkt_length(struct tegra_dc *dc,
 }
 
 static void tegra_dsi_set_pkt_seq(struct tegra_dc *dc,
-                                               struct tegra_dc_dsi_data *dsi)
+                               struct tegra_dc_dsi_data *dsi)
 {
        const u32 *pkt_seq;
        u32 rgb_info;
@@ -750,7 +1130,7 @@ static void tegra_dsi_set_pkt_seq(struct tegra_dc *dc,
        if (dsi->driven_mode == TEGRA_DSI_DRIVEN_BY_HOST)
                return;
 
-       switch(dsi->info.pixel_format) {
+       switch (dsi->info.pixel_format) {
        case TEGRA_DSI_PIXEL_FORMAT_16BIT_P:
                rgb_info = CMD_RGB_16BPP;
                break;
@@ -777,19 +1157,22 @@ static void tegra_dsi_set_pkt_seq(struct tegra_dc *dc,
                case TEGRA_DSI_VIDEO_BURST_MODE_MEDIUM_SPEED:
                case TEGRA_DSI_VIDEO_BURST_MODE_FAST_SPEED:
                case TEGRA_DSI_VIDEO_BURST_MODE_FASTEST_SPEED:
-                       pkt_seq_3_5_rgb_lo = DSI_PKT_SEQ_3_LO_PKT_32_ID(rgb_info);
-                       if(!dsi->info.no_pkt_seq_eot)
+                       pkt_seq_3_5_rgb_lo =
+                                       DSI_PKT_SEQ_3_LO_PKT_32_ID(rgb_info);
+                       if (!dsi->info.no_pkt_seq_eot)
                                pkt_seq = dsi_pkt_seq_video_burst;
                        else
                                pkt_seq = dsi_pkt_seq_video_burst_no_eot;
                        break;
                case TEGRA_DSI_VIDEO_NONE_BURST_MODE_WITH_SYNC_END:
-                       pkt_seq_3_5_rgb_hi = DSI_PKT_SEQ_3_HI_PKT_34_ID(rgb_info);
+                       pkt_seq_3_5_rgb_hi =
+                                       DSI_PKT_SEQ_3_HI_PKT_34_ID(rgb_info);
                        pkt_seq = dsi_pkt_seq_video_non_burst_syne;
                        break;
                case TEGRA_DSI_VIDEO_NONE_BURST_MODE:
                default:
-                       pkt_seq_3_5_rgb_lo = DSI_PKT_SEQ_3_LO_PKT_32_ID(rgb_info);
+                       pkt_seq_3_5_rgb_lo =
+                                       DSI_PKT_SEQ_3_LO_PKT_32_ID(rgb_info);
                        pkt_seq = dsi_pkt_seq_video_non_burst;
                        break;
                }
@@ -806,78 +1189,110 @@ static void tegra_dsi_set_pkt_seq(struct tegra_dc *dc,
        }
 }
 
+static void tegra_dsi_reset_underflow_overflow
+                               (struct tegra_dc_dsi_data *dsi)
+{
+       u32 val;
+
+       val = tegra_dsi_readl(dsi, DSI_STATUS);
+       val &= (DSI_STATUS_LB_OVERFLOW(0x1) | DSI_STATUS_LB_UNDERFLOW(0x1));
+       if (val) {
+               if (val & DSI_STATUS_LB_OVERFLOW(0x1))
+                       dev_warn(&dsi->dc->ndev->dev,
+                               "dsi: video fifo overflow. Resetting flag\n");
+               if (val & DSI_STATUS_LB_UNDERFLOW(0x1))
+                       dev_warn(&dsi->dc->ndev->dev,
+                               "dsi: video fifo underflow. Resetting flag\n");
+               val = tegra_dsi_readl(dsi, DSI_HOST_DSI_CONTROL);
+               val |= DSI_HOST_CONTROL_FIFO_STAT_RESET(0x1);
+               tegra_dsi_writel(dsi, val, DSI_HOST_DSI_CONTROL);
+               udelay(5);
+       }
+}
+
 static void tegra_dsi_stop_dc_stream(struct tegra_dc *dc,
-                                               struct tegra_dc_dsi_data *dsi)
+                                       struct tegra_dc_dsi_data *dsi)
 {
+       tegra_dc_writel(dc, DISP_CTRL_MODE_STOP, DC_CMD_DISPLAY_COMMAND);
        tegra_dc_writel(dc, 0, DC_DISP_DISP_WIN_OPTIONS);
-       tegra_dc_writel(dc, GENERAL_ACT_REQ << 8, DC_CMD_STATE_CONTROL);
-       tegra_dc_writel(dc, GENERAL_ACT_REQ, DC_CMD_STATE_CONTROL);
+       tegra_dc_writel(dc, GENERAL_UPDATE, DC_CMD_STATE_CONTROL);
+       tegra_dc_writel(dc, GENERAL_ACT_REQ , DC_CMD_STATE_CONTROL);
 
        dsi->status.dc_stream = DSI_DC_STREAM_DISABLE;
 }
 
-void tegra_dsi_stop_dc_stream_at_frame_end(struct tegra_dc *dc, struct tegra_dc_dsi_data *dsi)
+static void tegra_dsi_stop_dc_stream_at_frame_end(struct tegra_dc *dc,
+                                               struct tegra_dc_dsi_data *dsi)
 {
        int val;
        long timeout;
+       u32 frame_period = DIV_ROUND_UP(S_TO_MS(1), dsi->info.refresh_rate);
 
        /* stop dc */
        tegra_dsi_stop_dc_stream(dc, dsi);
 
-       /* enable vblank interrupt */
-       val = tegra_dc_readl(dc, DC_CMD_INT_ENABLE);
-       val |= V_BLANK_INT;
-       tegra_dc_writel(dc, val, DC_CMD_INT_ENABLE);
-
+       /* enable frame end interrupt */
        val = tegra_dc_readl(dc, DC_CMD_INT_MASK);
-       val |= V_BLANK_INT;
+       val |= FRAME_END_INT;
        tegra_dc_writel(dc, val, DC_CMD_INT_MASK);
 
-       /* wait for vblank completion */
+       /* wait for frame_end completion.
+        * timeout is 2 frame duration to accomodate for
+        * internal delay.
+        */
        timeout = wait_for_completion_interruptible_timeout(
-               &dc->vblank_complete, DSI_STOP_DC_DURATION_MSEC);
+                       &dc->frame_end_complete,
+                       msecs_to_jiffies(2 * frame_period));
 
-       /* disable vblank interrupt */
-       val = tegra_dc_readl(dc, DC_CMD_INT_ENABLE);
-       val &= ~V_BLANK_INT;
-       tegra_dc_writel(dc, val, DC_CMD_INT_ENABLE);
+       /* disable frame end interrupt */
+       val = tegra_dc_readl(dc, DC_CMD_INT_MASK);
+       val &= ~FRAME_END_INT;
+       tegra_dc_writel(dc, val, DC_CMD_INT_MASK);
+
+       if (timeout == 0)
+               dev_warn(&dc->ndev->dev,
+                       "DC doesn't stop at end of frame.\n");
 
-       if(timeout == 0)
-               printk("Warning: dc dosen't stop at the end of the frame.\n");
+       tegra_dsi_reset_underflow_overflow(dsi);
 }
 
 static void tegra_dsi_start_dc_stream(struct tegra_dc *dc,
-                                               struct tegra_dc_dsi_data *dsi)
+                                       struct tegra_dc_dsi_data *dsi)
 {
        u32 val;
 
        tegra_dc_writel(dc, DSI_ENABLE, DC_DISP_DISP_WIN_OPTIONS);
 
        /* TODO: clean up */
-       val = PIN_INPUT_LSPI_INPUT_EN;
-       tegra_dc_writel(dc, val, DC_COM_PIN_INPUT_ENABLE3);
-
-       val = PIN_OUTPUT_LSPI_OUTPUT_DIS;
-       tegra_dc_writel(dc, val, DC_COM_PIN_OUTPUT_ENABLE3);
-
        tegra_dc_writel(dc, PW0_ENABLE | PW1_ENABLE | PW2_ENABLE | PW3_ENABLE |
                        PW4_ENABLE | PM0_ENABLE | PM1_ENABLE,
                        DC_CMD_DISPLAY_POWER_CONTROL);
 
-       val = MSF_POLARITY_HIGH | MSF_ENABLE | MSF_LSPI;
-       tegra_dc_writel(dc, val, DC_CMD_DISPLAY_COMMAND_OPTION0);
+       /* Configure one-shot mode or continuous mode */
+       if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE) {
+               /* disable LSPI/LCD_DE output */
+               val = PIN_OUTPUT_LSPI_OUTPUT_DIS;
+               tegra_dc_writel(dc, val, DC_COM_PIN_OUTPUT_ENABLE3);
 
+               /* enable MSF & set MSF polarity */
+               val = MSF_ENABLE | MSF_LSPI;
+               if (!dsi->info.te_polarity_low)
+                       val |= MSF_POLARITY_HIGH;
+               else
+                       val |= MSF_POLARITY_LOW;
+               tegra_dc_writel(dc, val, DC_CMD_DISPLAY_COMMAND_OPTION0);
 
-       /* TODO: using continuous video mode for now */
-       /* if (dsi->info.panel_has_frame_buffer) {*/
-       if (0) {
-               tegra_dc_writel(dc, DISP_CTRL_MODE_NC_DISPLAY, DC_CMD_DISPLAY_COMMAND);
+               /* set non-continuous mode */
+               tegra_dc_writel(dc, DISP_CTRL_MODE_NC_DISPLAY,
+                                               DC_CMD_DISPLAY_COMMAND);
                tegra_dc_writel(dc, GENERAL_UPDATE, DC_CMD_STATE_CONTROL);
-               val = GENERAL_ACT_REQ | NC_HOST_TRIG;
-               tegra_dc_writel(dc, val, DC_CMD_STATE_CONTROL);
+               tegra_dc_writel(dc, GENERAL_ACT_REQ | NC_HOST_TRIG,
+                                               DC_CMD_STATE_CONTROL);
        } else {
-               tegra_dc_writel(dc, DISP_CTRL_MODE_C_DISPLAY, DC_CMD_DISPLAY_COMMAND);
-               tegra_dc_writel(dc, GENERAL_ACT_REQ << 8, DC_CMD_STATE_CONTROL);
+               /* set continuous mode */
+               tegra_dc_writel(dc, DISP_CTRL_MODE_C_DISPLAY,
+                                               DC_CMD_DISPLAY_COMMAND);
+               tegra_dc_writel(dc, GENERAL_UPDATE, DC_CMD_STATE_CONTROL);
                tegra_dc_writel(dc, GENERAL_ACT_REQ, DC_CMD_STATE_CONTROL);
        }
 
@@ -885,7 +1300,7 @@ static void tegra_dsi_start_dc_stream(struct tegra_dc *dc,
 }
 
 static void tegra_dsi_set_dc_clk(struct tegra_dc *dc,
-                                               struct tegra_dc_dsi_data *dsi)
+                               struct tegra_dc_dsi_data *dsi)
 {
        u32 shift_clk_div_register;
        u32 val;
@@ -893,37 +1308,44 @@ static void tegra_dsi_set_dc_clk(struct tegra_dc *dc,
        /* Get the corresponding register value of shift_clk_div. */
        shift_clk_div_register = dsi->shift_clk_div * 2 - 2;
 
-#ifdef CONFIG_TEGRA_FPGA_PLATFORM
+#ifndef CONFIG_TEGRA_SILICON_PLATFORM
        shift_clk_div_register = 1;
 #endif
 
        /* TODO: find out if PCD3 option is required */
        val = PIXEL_CLK_DIVIDER_PCD1 |
-                               SHIFT_CLK_DIVIDER(shift_clk_div_register);
+               SHIFT_CLK_DIVIDER(shift_clk_div_register);
        tegra_dc_writel(dc, val, DC_DISP_DISP_CLOCK_CONTROL);
-
-       clk_enable(dsi->dc_clk);
 }
 
 static void tegra_dsi_set_dsi_clk(struct tegra_dc *dc,
-                                       struct tegra_dc_dsi_data *dsi, u32 clk)
+                       struct tegra_dc_dsi_data *dsi, u32 clk)
 {
        u32 rm;
 
+       /* Round up to MHz */
        rm = clk % 1000;
        if (rm != 0)
                clk -= rm;
 
-       clk *= 2;       /* Value for PLLD routine is required to be twice as */
-                        /* the desired clock rate */
-
-       dc->mode.pclk = clk*1000;
+       /* Set up pixel clock */
+       dc->shift_clk_div = dsi->shift_clk_div;
+       dc->mode.pclk = (clk * 1000) / dsi->shift_clk_div;
+       /* TODO: Define one shot work delay in board file. */
+       /* Since for one-shot mode, refresh rate is usually set larger than
+        * expected refresh rate, it needs at least 3 frame period. Less
+        * delay one shot work is, more powering saving we have. */
+       dc->one_shot_delay_ms = 4 *
+                       DIV_ROUND_UP(S_TO_MS(1), dsi->info.refresh_rate);
+
+       /* Enable DSI clock */
        tegra_dc_setup_clk(dc, dsi->dsi_clk);
-       clk_enable(dsi->dsi_clk);
-       tegra_periph_reset_deassert(dsi->dsi_clk);
-
+       if (!dsi->clk_ref) {
+               dsi->clk_ref = true;
+               clk_enable(dsi->dsi_clk);
+               tegra_periph_reset_deassert(dsi->dsi_clk);
+       }
        dsi->current_dsi_clk_khz = clk_get_rate(dsi->dsi_clk) / 1000;
-
        dsi->current_bit_clk_ns =  1000*1000 / (dsi->current_dsi_clk_khz * 2);
 }
 
@@ -968,20 +1390,21 @@ static void tegra_dsi_hs_clk_out_disable(struct tegra_dc *dc,
        u32 val;
 
        if (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE)
-               tegra_dsi_stop_dc_stream(dc, dsi);
-
-       val = tegra_dsi_readl(dsi, DSI_CONTROL);
-       val &= ~DSI_CONTROL_HS_CLK_CTRL(1);
-       val |= DSI_CONTROL_HS_CLK_CTRL(TX_ONLY);
-       tegra_dsi_writel(dsi, val, DSI_CONTROL);
+               tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
 
-       /* TODO: issue a cmd */
+       tegra_dsi_writel(dsi, TEGRA_DSI_DISABLE, DSI_POWER_CONTROL);
+       /* stabilization delay */
+       udelay(300);
 
        val = tegra_dsi_readl(dsi, DSI_HOST_DSI_CONTROL);
        val &= ~DSI_HOST_DSI_CONTROL_HIGH_SPEED_TRANS(1);
        val |= DSI_HOST_DSI_CONTROL_HIGH_SPEED_TRANS(TEGRA_DSI_LOW);
        tegra_dsi_writel(dsi, val, DSI_HOST_DSI_CONTROL);
 
+       tegra_dsi_writel(dsi, TEGRA_DSI_ENABLE, DSI_POWER_CONTROL);
+       /* stabilization delay */
+       udelay(300);
+
        dsi->status.clk_mode = DSI_PHYCLK_NOT_INIT;
        dsi->status.clk_out = DSI_PHYCLK_OUT_DIS;
 }
@@ -1022,25 +1445,25 @@ static void tegra_dsi_set_control_reg_hs(struct tegra_dc_dsi_data *dsi)
        if (dsi->driven_mode == TEGRA_DSI_DRIVEN_BY_HOST) {
                dsi_control |= DSI_CTRL_HOST_DRIVEN;
                host_dsi_control |= HOST_DSI_CTRL_HOST_DRIVEN;
-               max_threshold = DSI_MAX_THRESHOLD_MAX_THRESHOLD(DSI_HOST_FIFO_DEPTH);
+               max_threshold =
+                       DSI_MAX_THRESHOLD_MAX_THRESHOLD(DSI_HOST_FIFO_DEPTH);
                dsi->status.driven = DSI_DRIVEN_MODE_HOST;
        } else {
                dsi_control |= DSI_CTRL_DC_DRIVEN;
                host_dsi_control |= HOST_DSI_CTRL_DC_DRIVEN;
-               max_threshold = DSI_MAX_THRESHOLD_MAX_THRESHOLD(DSI_VIDEO_FIFO_DEPTH);
+               max_threshold =
+                       DSI_MAX_THRESHOLD_MAX_THRESHOLD(DSI_VIDEO_FIFO_DEPTH);
                dsi->status.driven = DSI_DRIVEN_MODE_DC;
        }
 
        if (dsi->info.video_data_type == TEGRA_DSI_VIDEO_TYPE_COMMAND_MODE) {
                dsi_control |= DSI_CTRL_CMD_MODE;
-               host_dsi_control |= HOST_DSI_CTRL_CMD_MODE;
                dcs_cmd = DSI_DCS_CMDS_LT5_DCS_CMD(DSI_WRITE_MEMORY_START)|
-                               DSI_DCS_CMDS_LT3_DCS_CMD(DSI_WRITE_MEMORY_CONTINUE);
+                       DSI_DCS_CMDS_LT3_DCS_CMD(DSI_WRITE_MEMORY_CONTINUE);
                dsi->status.vtype = DSI_VIDEO_TYPE_CMD_MODE;
 
        } else {
                dsi_control |= DSI_CTRL_VIDEO_MODE;
-               host_dsi_control |= HOST_DSI_CTRL_VIDEO_MODE;
                dsi->status.vtype = DSI_VIDEO_TYPE_VIDEO_MODE;
        }
 
@@ -1050,30 +1473,10 @@ static void tegra_dsi_set_control_reg_hs(struct tegra_dc_dsi_data *dsi)
        tegra_dsi_writel(dsi, host_dsi_control, DSI_HOST_DSI_CONTROL);
 }
 
-static int tegra_dsi_init_hw(struct tegra_dc *dc,
-                                               struct tegra_dc_dsi_data *dsi)
+static void tegra_dsi_pad_calibration(struct tegra_dc_dsi_data *dsi)
 {
        u32 val;
-       u32 i;
 
-       tegra_dsi_set_dsi_clk(dc, dsi, dsi->target_lp_clk_khz);
-       if (dsi->info.dsi_instance) {
-               /* TODO:Set the misc register*/
-       }
-
-       /* TODO: only need to change the timing for bta */
-       tegra_dsi_set_phy_timing(dsi);
-
-       if (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE)
-               tegra_dsi_stop_dc_stream(dc, dsi);
-
-       /* Initializing DSI registers */
-       for (i = 0; i < ARRAY_SIZE(init_reg); i++) {
-               tegra_dsi_writel(dsi, 0, init_reg[i]);
-       }
-       tegra_dsi_writel(dsi, dsi->dsi_control_val, DSI_CONTROL);
-
-       /* Initialize DSI_PAD_CONTROL register. */
        val =   DSI_PAD_CONTROL_PAD_LPUPADJ(0x1) |
                DSI_PAD_CONTROL_PAD_LPDNADJ(0x1) |
                DSI_PAD_CONTROL_PAD_PREEMP_EN(0x1) |
@@ -1090,13 +1493,58 @@ static int tegra_dsi_init_hw(struct tegra_dc *dc,
        }
        tegra_dsi_writel(dsi, val, DSI_PAD_CONTROL);
 
-       val = DSI_POWER_CONTROL_LEG_DSI_ENABLE(TEGRA_DSI_ENABLE);
-       tegra_dsi_writel(dsi, val, DSI_POWER_CONTROL);
+       val = MIPI_CAL_TERMOSA(0x4);
+       tegra_vi_csi_writel(val, CSI_CILA_MIPI_CAL_CONFIG_0);
+
+       val = MIPI_CAL_TERMOSB(0x4);
+       tegra_vi_csi_writel(val, CSI_CILB_MIPI_CAL_CONFIG_0);
+
+       val = MIPI_CAL_HSPUOSD(0x3) | MIPI_CAL_HSPDOSD(0x4);
+       tegra_vi_csi_writel(val, CSI_DSI_MIPI_CAL_CONFIG);
 
-       while (tegra_dsi_readl(dsi, DSI_POWER_CONTROL) != val) {
-               tegra_dsi_writel(dsi, val, DSI_POWER_CONTROL);
+       val = PAD_DRIV_DN_REF(0x5) | PAD_DRIV_UP_REF(0x7);
+       tegra_vi_csi_writel(val, CSI_MIPIBIAS_PAD_CONFIG);
+
+       val = PAD_CIL_PDVREG(0x0);
+       tegra_vi_csi_writel(val, CSI_CIL_PAD_CONFIG);
+}
+
+static int tegra_dsi_init_hw(struct tegra_dc *dc,
+                                               struct tegra_dc_dsi_data *dsi)
+{
+       u32 i;
+
+       tegra_dsi_writel(dsi,
+               DSI_POWER_CONTROL_LEG_DSI_ENABLE(TEGRA_DSI_DISABLE),
+               DSI_POWER_CONTROL);
+       /* stabilization delay */
+       udelay(300);
+
+       tegra_dsi_set_dsi_clk(dc, dsi, dsi->target_lp_clk_khz);
+       if (dsi->info.dsi_instance) {
+               /* TODO:Set the misc register*/
        }
 
+       /* TODO: only need to change the timing for bta */
+       tegra_dsi_set_phy_timing(dsi, DSI_LPHS_IN_LP_MODE);
+
+       if (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE)
+               tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
+
+       /* Initializing DSI registers */
+       for (i = 0; i < ARRAY_SIZE(init_reg); i++)
+               tegra_dsi_writel(dsi, 0, init_reg[i]);
+
+       tegra_dsi_writel(dsi, dsi->dsi_control_val, DSI_CONTROL);
+
+       tegra_dsi_pad_calibration(dsi);
+
+       tegra_dsi_writel(dsi,
+               DSI_POWER_CONTROL_LEG_DSI_ENABLE(TEGRA_DSI_ENABLE),
+               DSI_POWER_CONTROL);
+       /* stabilization delay */
+       udelay(300);
+
        dsi->status.init = DSI_MODULE_INIT;
        dsi->status.lphs = DSI_LPHS_NOT_INIT;
        dsi->status.vtype = DSI_VIDEO_TYPE_NOT_INIT;
@@ -1105,51 +1553,57 @@ static int tegra_dsi_init_hw(struct tegra_dc *dc,
        dsi->status.clk_mode = DSI_PHYCLK_NOT_INIT;
        dsi->status.clk_burst = DSI_CLK_BURST_NOT_INIT;
        dsi->status.dc_stream = DSI_DC_STREAM_DISABLE;
+       dsi->status.lp_op = DSI_LP_OP_NOT_INIT;
 
        return 0;
 }
 
 static int tegra_dsi_set_to_lp_mode(struct tegra_dc *dc,
-                                               struct tegra_dc_dsi_data *dsi)
+                       struct tegra_dc_dsi_data *dsi, u8 lp_op)
 {
-       int     err;
+       int err;
 
        if (dsi->status.init != DSI_MODULE_INIT) {
                err = -EPERM;
                goto fail;
        }
 
-       if (dsi->status.lphs == DSI_LPHS_IN_LP_MODE)
+       if (dsi->status.lphs == DSI_LPHS_IN_LP_MODE &&
+                       dsi->status.lp_op == lp_op)
                goto success;
 
        if (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE)
                tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
 
-       /* disable/enable hs clock according to enable_hs_clock_on_lp_cmd_mode */
+       /* disable/enable hs clk according to enable_hs_clock_on_lp_cmd_mode */
        if ((dsi->status.clk_out == DSI_PHYCLK_OUT_EN) &&
                (!dsi->info.enable_hs_clock_on_lp_cmd_mode))
                tegra_dsi_hs_clk_out_disable(dc, dsi);
 
-       if (dsi->current_dsi_clk_khz != dsi->target_lp_clk_khz){
+       dsi->target_lp_clk_khz = tegra_dsi_get_lp_clk_rate(dsi, lp_op);
+       if (dsi->current_dsi_clk_khz != dsi->target_lp_clk_khz) {
                tegra_dsi_set_dsi_clk(dc, dsi, dsi->target_lp_clk_khz);
                tegra_dsi_set_timeout(dsi);
        }
 
+       tegra_dsi_set_phy_timing(dsi, DSI_LPHS_IN_LP_MODE);
+
        tegra_dsi_set_control_reg_lp(dsi);
 
        if ((dsi->status.clk_out == DSI_PHYCLK_OUT_DIS) &&
                (dsi->info.enable_hs_clock_on_lp_cmd_mode))
                tegra_dsi_hs_clk_out_enable_in_lp(dsi);
 
-success:
        dsi->status.lphs = DSI_LPHS_IN_LP_MODE;
+       dsi->status.lp_op = lp_op;
+success:
        err = 0;
 fail:
        return err;
 }
 
 static int tegra_dsi_set_to_hs_mode(struct tegra_dc *dc,
-                                               struct tegra_dc_dsi_data *dsi)
+                                       struct tegra_dc_dsi_data *dsi)
 {
        int err;
 
@@ -1158,6 +1612,9 @@ static int tegra_dsi_set_to_hs_mode(struct tegra_dc *dc,
                goto fail;
        }
 
+       if (dsi->status.lphs == DSI_LPHS_IN_HS_MODE)
+               goto success;
+
        if (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE)
                tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
 
@@ -1170,9 +1627,9 @@ static int tegra_dsi_set_to_hs_mode(struct tegra_dc *dc,
                tegra_dsi_set_timeout(dsi);
        }
 
-       tegra_dsi_set_phy_timing(dsi);
+       tegra_dsi_set_phy_timing(dsi, DSI_LPHS_IN_HS_MODE);
 
-       if (dsi->driven_mode == TEGRA_DSI_DRIVEN_BY_DC){
+       if (dsi->driven_mode == TEGRA_DSI_DRIVEN_BY_DC) {
                tegra_dsi_set_pkt_seq(dc, dsi);
                tegra_dsi_set_pkt_length(dc, dsi);
                tegra_dsi_set_sol_delay(dc, dsi);
@@ -1181,24 +1638,27 @@ static int tegra_dsi_set_to_hs_mode(struct tegra_dc *dc,
 
        tegra_dsi_set_control_reg_hs(dsi);
 
-       if (dsi->status.clk_out == DSI_PHYCLK_OUT_DIS)
+       if (dsi->status.clk_out == DSI_PHYCLK_OUT_DIS ||
+               dsi->info.enable_hs_clock_on_lp_cmd_mode)
                tegra_dsi_hs_clk_out_enable(dsi);
 
        dsi->status.lphs = DSI_LPHS_IN_HS_MODE;
+success:
+       dsi->status.lp_op = DSI_LP_OP_NOT_INIT;
        err = 0;
 fail:
        return err;
 }
 
-static bool tegra_dsi_is_controller_idle(struct tegra_dc_dsi_data *dsi)
+static bool tegra_dsi_write_busy(struct tegra_dc_dsi_data *dsi)
 {
        u32 timeout = 0;
-       bool retVal;
+       bool retVal = true;
 
-       retVal = false;
        while (timeout <= DSI_MAX_COMMAND_DELAY_USEC) {
-               if (!tegra_dsi_readl(dsi, DSI_TRIGGER)) {
-                       retVal = true;
+               if (!(DSI_TRIGGER_HOST_TRIGGER(0x1) &
+                       tegra_dsi_readl(dsi, DSI_TRIGGER))) {
+                       retVal = false;
                        break;
                }
                udelay(DSI_COMMAND_DELAY_STEPS_USEC);
@@ -1208,12 +1668,231 @@ static bool tegra_dsi_is_controller_idle(struct tegra_dc_dsi_data *dsi)
        return retVal;
 }
 
-static int tegra_dsi_host_trigger(struct tegra_dc_dsi_data *dsi)
+static bool tegra_dsi_read_busy(struct tegra_dc_dsi_data *dsi)
+{
+       u32 timeout = 0;
+       bool retVal = true;
+
+       while (timeout <  DSI_STATUS_POLLING_DURATION_USEC) {
+               if (!(DSI_HOST_DSI_CONTROL_IMM_BTA(0x1) &
+                       tegra_dsi_readl(dsi, DSI_HOST_DSI_CONTROL))) {
+                       retVal = false;
+                       break;
+               }
+               udelay(DSI_STATUS_POLLING_DELAY_USEC);
+               timeout += DSI_STATUS_POLLING_DELAY_USEC;
+       }
+
+       return retVal;
+}
+
+static bool tegra_dsi_host_busy(struct tegra_dc_dsi_data *dsi)
+{
+       int err = 0;
+
+       if (tegra_dsi_write_busy(dsi)) {
+               err = -EBUSY;
+               dev_err(&dsi->dc->ndev->dev,
+                       "DSI trigger bit already set\n");
+               goto fail;
+       }
+
+       if (tegra_dsi_read_busy(dsi)) {
+               err = -EBUSY;
+               dev_err(&dsi->dc->ndev->dev,
+                       "DSI immediate bta bit already set\n");
+               goto fail;
+       }
+fail:
+       return (err < 0 ? true : false);
+}
+
+static void tegra_dsi_soft_reset(struct tegra_dc_dsi_data *dsi)
+{
+       tegra_dsi_writel(dsi,
+               DSI_POWER_CONTROL_LEG_DSI_ENABLE(TEGRA_DSI_DISABLE),
+               DSI_POWER_CONTROL);
+       /* stabilization delay */
+       udelay(300);
+
+       tegra_dsi_writel(dsi,
+               DSI_POWER_CONTROL_LEG_DSI_ENABLE(TEGRA_DSI_ENABLE),
+               DSI_POWER_CONTROL);
+       /* stabilization delay */
+       udelay(300);
+}
+
+static void tegra_dsi_reset_read_count(struct tegra_dc_dsi_data *dsi)
 {
-       int status;
        u32 val;
 
-       status = 0;
+       val = tegra_dsi_readl(dsi, DSI_STATUS);
+       val &= DSI_STATUS_RD_FIFO_COUNT(0x1f);
+       if (val) {
+               dev_warn(&dsi->dc->ndev->dev,
+                       "DSI read count not zero, resetting\n");
+               tegra_dsi_soft_reset(dsi);
+       }
+}
+
+static struct dsi_status *tegra_dsi_save_state_switch_to_host_cmd_mode(
+                                               struct tegra_dc_dsi_data *dsi,
+                                               struct tegra_dc *dc,
+                                               u8 lp_op)
+{
+       struct dsi_status *init_status;
+       int err;
+
+       init_status = kzalloc(sizeof(*init_status), GFP_KERNEL);
+       if (!init_status)
+               return ERR_PTR(-ENOMEM);
+
+       *init_status = dsi->status;
+
+       if (dsi->status.lphs == DSI_LPHS_IN_HS_MODE) {
+               if (dsi->status.driven == DSI_DRIVEN_MODE_DC) {
+                       if (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE)
+                               tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
+                       dsi->driven_mode = TEGRA_DSI_DRIVEN_BY_HOST;
+                       if (dsi->info.hs_cmd_mode_supported) {
+                               err = tegra_dsi_set_to_hs_mode(dc, dsi);
+                               if (err < 0) {
+                                       dev_err(&dc->ndev->dev,
+                                       "Switch to HS host mode failed\n");
+                                       goto fail;
+                               }
+                       }
+               }
+               if (!dsi->info.hs_cmd_mode_supported) {
+                       err =
+                       tegra_dsi_set_to_lp_mode(dc, dsi, lp_op);
+                       if (err < 0) {
+                               dev_err(&dc->ndev->dev,
+                               "DSI failed to go to LP mode\n");
+                               goto fail;
+                       }
+               }
+       } else if (dsi->status.lphs == DSI_LPHS_IN_LP_MODE) {
+               if (dsi->status.lp_op != lp_op) {
+                       err = tegra_dsi_set_to_lp_mode(dc, dsi, lp_op);
+                       if (err < 0) {
+                               dev_err(&dc->ndev->dev,
+                               "DSI failed to go to LP mode\n");
+                               goto fail;
+                       }
+               }
+       }
+
+       return init_status;
+fail:
+       kfree(init_status);
+       return ERR_PTR(err);
+}
+
+static struct dsi_status *tegra_dsi_prepare_host_transmission(
+                               struct tegra_dc *dc,
+                               struct tegra_dc_dsi_data *dsi,
+                               u8 lp_op)
+{
+       int err = 0;
+       struct dsi_status *init_status;
+
+       if (dsi->status.init != DSI_MODULE_INIT ||
+               dsi->ulpm) {
+               err = -EPERM;
+               goto fail;
+       }
+
+       if (tegra_dsi_host_busy(dsi)) {
+               tegra_dsi_soft_reset(dsi);
+               if (tegra_dsi_host_busy(dsi)) {
+                       err = -EBUSY;
+                       dev_err(&dc->ndev->dev, "DSI host busy\n");
+                       goto fail;
+               }
+       }
+
+       if (lp_op == DSI_LP_OP_READ)
+               tegra_dsi_reset_read_count(dsi);
+
+       if (dsi->status.lphs == DSI_LPHS_NOT_INIT) {
+               err = tegra_dsi_set_to_lp_mode(dc, dsi, lp_op);
+               if (err < 0) {
+                       dev_err(&dc->ndev->dev, "Failed to config LP write\n");
+                       goto fail;
+               }
+       }
+
+       init_status = tegra_dsi_save_state_switch_to_host_cmd_mode
+                                       (dsi, dc, lp_op);
+       if (IS_ERR_OR_NULL(init_status)) {
+               err = PTR_ERR(init_status);
+               dev_err(&dc->ndev->dev, "DSI state saving failed\n");
+               goto fail;
+       }
+
+       return init_status;
+fail:
+       return ERR_PTR(err);
+}
+
+static int tegra_dsi_restore_state(struct tegra_dc *dc,
+                               struct tegra_dc_dsi_data *dsi,
+                               struct dsi_status *init_status)
+{
+       bool switch_back_to_dc_mode = false;
+       bool switch_back_to_hs_mode = false;
+       bool restart_dc_stream;
+       int err = 0;
+
+       switch_back_to_dc_mode = (dsi->status.driven ==
+                               DSI_DRIVEN_MODE_HOST &&
+                               init_status->driven ==
+                               DSI_DRIVEN_MODE_DC);
+       switch_back_to_hs_mode = (dsi->status.lphs ==
+                               DSI_LPHS_IN_LP_MODE &&
+                               init_status->lphs ==
+                               DSI_LPHS_IN_HS_MODE);
+       restart_dc_stream = (dsi->status.dc_stream ==
+                               DSI_DC_STREAM_DISABLE &&
+                               init_status->dc_stream ==
+                               DSI_DC_STREAM_ENABLE);
+
+       if (dsi->status.lphs == DSI_LPHS_IN_LP_MODE &&
+               init_status->lphs == DSI_LPHS_IN_LP_MODE) {
+               if (dsi->status.lp_op != init_status->lp_op) {
+                       err =
+                       tegra_dsi_set_to_lp_mode(dc, dsi, init_status->lp_op);
+                       if (err < 0) {
+                               dev_err(&dc->ndev->dev,
+                                       "Failed to config LP mode\n");
+                               goto fail;
+                       }
+               }
+               goto success;
+       }
+
+       if (switch_back_to_dc_mode)
+               dsi->driven_mode = TEGRA_DSI_DRIVEN_BY_DC;
+       if (switch_back_to_dc_mode || switch_back_to_hs_mode) {
+               err = tegra_dsi_set_to_hs_mode(dc, dsi);
+               if (err < 0) {
+                       dev_err(&dc->ndev->dev, "Failed to config HS mode\n");
+                       goto fail;
+               }
+       }
+       if (restart_dc_stream)
+               tegra_dsi_start_dc_stream(dc, dsi);
+
+success:
+fail:
+       kfree(init_status);
+       return err;
+}
+
+static int tegra_dsi_host_trigger(struct tegra_dc_dsi_data *dsi)
+{
+       int status = 0;
 
        if (tegra_dsi_readl(dsi, DSI_TRIGGER)) {
                status = -EBUSY;
@@ -1226,12 +1905,16 @@ static int tegra_dsi_host_trigger(struct tegra_dc_dsi_data *dsi)
 #if DSI_USE_SYNC_POINTS
        status = tegra_dsi_syncpt(dsi);
        if (status < 0) {
-               printk(KERN_ERR "DSI syncpt for host trigger failed\n");
+               dev_err(&dsi->dc->ndev->dev,
+                       "DSI syncpt for host trigger failed\n");
                goto fail;
        }
 #else
-       if (!tegra_dsi_is_controller_idle(dsi))
-               status = -EIO;
+       if (tegra_dsi_write_busy(dsi)) {
+               status = -EBUSY;
+               dev_err(&dsi->dc->ndev->dev,
+                       "Timeout waiting on write completion\n");
+       }
 #endif
 
 fail:
@@ -1239,7 +1922,7 @@ fail:
 }
 
 static int _tegra_dsi_write_data(struct tegra_dc_dsi_data *dsi,
-                                       u8* pdata, u8 data_id, u16 data_len)
+                                       u8 *pdata, u8 data_id, u16 data_len)
 {
        u8 virtual_channel;
        u8 *pval;
@@ -1248,7 +1931,8 @@ static int _tegra_dsi_write_data(struct tegra_dc_dsi_data *dsi,
 
        err = 0;
 
-       virtual_channel = dsi->info.virtual_channel << DSI_VIR_CHANNEL_BIT_POSITION;
+       virtual_channel = dsi->info.virtual_channel <<
+                                               DSI_VIR_CHANNEL_BIT_POSITION;
 
        /* always use hw for ecc */
        val = (virtual_channel | data_id) << 0 |
@@ -1259,15 +1943,15 @@ static int _tegra_dsi_write_data(struct tegra_dc_dsi_data *dsi,
        if (pdata != NULL) {
                while (data_len) {
                        if (data_len >= 4) {
-                               val = ((u32*) pdata)[0];
+                               val = ((u32 *) pdata)[0];
                                data_len -= 4;
                                pdata += 4;
                        } else {
                                val = 0;
-                               pval = (u8*) &val;
+                               pval = (u8 *) &val;
                                do
                                        *pval++ = *pdata++;
-                               while(--data_len);
+                               while (--data_len);
                        }
                        tegra_dsi_writel(dsi, val, DSI_WR_DATA);
                }
@@ -1275,65 +1959,42 @@ static int _tegra_dsi_write_data(struct tegra_dc_dsi_data *dsi,
 
        err = tegra_dsi_host_trigger(dsi);
        if (err < 0)
-               printk(KERN_ERR "DSI host trigger failed\n");
+               dev_err(&dsi->dc->ndev->dev, "DSI host trigger failed\n");
 
        return err;
 }
 
-static int tegra_dsi_write_data(struct tegra_dc *dc,
-                                       struct tegra_dc_dsi_data *dsi,
-                                       u8* pdata, u8 data_id, u16 data_len)
+int tegra_dsi_write_data(struct tegra_dc *dc,
+                       struct tegra_dc_dsi_data *dsi,
+                       u8 *pdata, u8 data_id, u16 data_len)
 {
-       bool switch_back_to_hs_mode;
-       bool switch_back_to_dc_mode;
-       int err;
-
-       err = 0;
-       switch_back_to_hs_mode = false;
-       switch_back_to_dc_mode = false;
+       int err = 0;
+       struct dsi_status *init_status;
 
-       if ((dsi->status.init != DSI_MODULE_INIT) ||
-               (dsi->status.lphs == DSI_LPHS_NOT_INIT)) {
-               err = -EPERM;
-               goto fail;
-       }
+       tegra_dc_io_start(dc);
 
-       if (!tegra_dsi_is_controller_idle(dsi)) {
-               err = -EBUSY;
+       init_status = tegra_dsi_prepare_host_transmission(
+                               dc, dsi, DSI_LP_OP_WRITE);
+       if (IS_ERR_OR_NULL(init_status)) {
+               err = PTR_ERR(init_status);
+               dev_err(&dc->ndev->dev, "DSI host config failed\n");
                goto fail;
        }
 
-       err = 0;
-
-       if (dsi->status.lphs == DSI_LPHS_IN_HS_MODE) {
-               if (dsi->info.hs_cmd_mode_supported) {
-                       if (dsi->status.driven == DSI_DRIVEN_MODE_DC) {
-                               dsi->driven_mode = TEGRA_DSI_DRIVEN_BY_HOST;
-                               tegra_dsi_set_to_hs_mode(dc, dsi);
-                               switch_back_to_dc_mode = true;
-                       }
-               } else {
-                       tegra_dsi_set_to_lp_mode(dc, dsi);
-                       switch_back_to_hs_mode = true;
-               }
-       }
-
        err = _tegra_dsi_write_data(dsi, pdata, data_id, data_len);
-
-
-       if (switch_back_to_dc_mode)
-               dsi->driven_mode = TEGRA_DSI_DRIVEN_BY_DC;
-       if (switch_back_to_dc_mode || switch_back_to_hs_mode)
-               tegra_dsi_set_to_hs_mode(dc, dsi);
-
 fail:
+       err = tegra_dsi_restore_state(dc, dsi, init_status);
+       if (err < 0)
+               dev_err(&dc->ndev->dev, "Failed to restore prev state\n");
+       tegra_dc_io_end(dc);
        return err;
 }
+EXPORT_SYMBOL(tegra_dsi_write_data);
 
 static int tegra_dsi_send_panel_cmd(struct tegra_dc *dc,
-                                               struct tegra_dc_dsi_data *dsi,
-                                               struct tegra_dsi_cmd *cmd,
-                                               u32 n_cmd)
+                                       struct tegra_dc_dsi_data *dsi,
+                                       struct tegra_dsi_cmd *cmd,
+                                       u32 n_cmd)
 {
        u32 i;
        int err;
@@ -1357,6 +2018,90 @@ static int tegra_dsi_send_panel_cmd(struct tegra_dc *dc,
        return err;
 }
 
+static u8 get_8bit_ecc(u32 header)
+{
+       char ecc_parity[24] = {
+               0x07, 0x0b, 0x0d, 0x0e, 0x13, 0x15, 0x16, 0x19,
+               0x1a, 0x1c, 0x23, 0x25, 0x26, 0x29, 0x2a, 0x2c,
+               0x31, 0x32, 0x34, 0x38, 0x1f, 0x2f, 0x37, 0x3b
+       };
+       u8 ecc_byte;
+       int i;
+
+       ecc_byte = 0;
+       for (i = 0; i < 24; i++)
+               ecc_byte ^= ((header >> i) & 1) ? ecc_parity[i] : 0x00;
+
+       return ecc_byte;
+}
+
+/* This function is written to send DCS short write (1 parameter) only.
+ * This means the cmd will contain only 1 byte of index and 1 byte of value.
+ * The data type ID is fixed at 0x15 and the ECC is calculated based on the
+ * data in pdata.
+ * The command will be sent by hardware every frame.
+ * pdata should contain both the index + value for each cmd.
+ * data_len will be the total number of bytes in pdata.
+ */
+int tegra_dsi_send_panel_short_cmd(struct tegra_dc *dc, u8 *pdata, u8 data_len)
+{
+       u8 ecc8bits = 0, data_len_orig = 0;
+       u32 val = 0, pkthdr = 0;
+       int err = 0, count = 0;
+       struct tegra_dc_dsi_data *dsi = tegra_dc_get_outdata(dc);
+
+       data_len_orig = data_len;
+       if (pdata != NULL) {
+               while (data_len) {
+                       if (data_len >= 2) {
+                               pkthdr = (CMD_SHORTW |
+                                       (((u16 *)pdata)[0]) << 8 | 0x00 << 24);
+                               ecc8bits = get_8bit_ecc(pkthdr);
+                               val = (pkthdr | (ecc8bits << 24));
+                               data_len -= 2;
+                               pdata += 2;
+                               count++;
+                       }
+                       switch (count) {
+                       case 1:
+                               tegra_dsi_writel(dsi, val, DSI_INIT_SEQ_DATA_0);
+                               break;
+                       case 2:
+                               tegra_dsi_writel(dsi, val, DSI_INIT_SEQ_DATA_1);
+                               break;
+                       case 3:
+                               tegra_dsi_writel(dsi, val, DSI_INIT_SEQ_DATA_2);
+                               break;
+                       case 4:
+                               tegra_dsi_writel(dsi, val, DSI_INIT_SEQ_DATA_3);
+                               break;
+                       case 5:
+                               tegra_dsi_writel(dsi, val, DSI_INIT_SEQ_DATA_4);
+                               break;
+                       case 6:
+                               tegra_dsi_writel(dsi, val, DSI_INIT_SEQ_DATA_5);
+                               break;
+                       case 7:
+                               tegra_dsi_writel(dsi, val, DSI_INIT_SEQ_DATA_6);
+                               break;
+                       case 8:
+                               tegra_dsi_writel(dsi, val, DSI_INIT_SEQ_DATA_7);
+                               break;
+                       default:
+                               err = 1;
+                               break;
+                       }
+               }
+       }
+
+       val = DSI_INIT_SEQ_CONTROL_DSI_FRAME_INIT_BYTE_COUNT(data_len_orig * 2)
+               | DSI_INIT_SEQ_CONTROL_DSI_SEND_INIT_SEQUENCE(1);
+       tegra_dsi_writel(dsi, val, DSI_INIT_SEQ_CONTROL);
+
+       return err;
+}
+EXPORT_SYMBOL(tegra_dsi_send_panel_short_cmd);
+
 static int tegra_dsi_bta(struct tegra_dc_dsi_data *dsi)
 {
        u32 val;
@@ -1373,61 +2118,34 @@ static int tegra_dsi_bta(struct tegra_dc_dsi_data *dsi)
 #if DSI_USE_SYNC_POINTS
        /* FIXME: Workaround for nvhost_syncpt_read */
        dsi->syncpt_val = nvhost_syncpt_update_min(
-                       &dsi->dc->ndev->host->syncpt, dsi->syncpt_id);
+                       &nvhost_get_host(dsi->dc->ndev)->syncpt,
+                       dsi->syncpt_id);
 
        val = DSI_INCR_SYNCPT_COND(OP_DONE) |
                DSI_INCR_SYNCPT_INDX(dsi->syncpt_id);
        tegra_dsi_writel(dsi, val, DSI_INCR_SYNCPT);
 
        /* TODO: Use interrupt rather than polling */
-       err = nvhost_syncpt_wait(&dsi->dc->ndev->host->syncpt,
+       err = nvhost_syncpt_wait(&nvhost_get_host(dsi->dc->ndev)->syncpt,
                dsi->syncpt_id, dsi->syncpt_val + 1);
        if (err < 0)
-               printk(KERN_ERR "DSI sync point failure\n");
+               dev_err(&dsi->dc->ndev->dev,
+                       "DSI sync point failure\n");
        else
                (dsi->syncpt_val)++;
 #else
-       while (poll_time <  DSI_STATUS_POLLING_DURATION_USEC) {
-               val = tegra_dsi_readl(dsi, DSI_HOST_DSI_CONTROL);
-               val &= DSI_HOST_DSI_CONTROL_IMM_BTA(TEGRA_DSI_ENABLE);
-               if (!val)
-                       break;
-               udelay(DSI_STATUS_POLLING_DELAY_USEC);
-               poll_time += DSI_STATUS_POLLING_DELAY_USEC;
-       }
-       if (poll_time > DSI_STATUS_POLLING_DURATION_USEC)
+       if (tegra_dsi_read_busy(dsi)) {
                err = -EBUSY;
+               dev_err(&dsi->dc->ndev->dev,
+                       "Timeout wating on read completion\n");
+       }
 #endif
 
        return err;
 }
 
-static void tegra_dsi_read_fifo(struct tegra_dc *dc,
-                       struct tegra_dc_dsi_data *dsi,
-                       u32 rd_fifo_cnt,
-                       u8 *read_fifo)
-{
-       u32 val;
-       u32 i;
-
-       /* Read data from FIFO */
-       for (i = 0; i < rd_fifo_cnt; i++) {
-               val = tegra_dsi_readl(dsi, DSI_RD_DATA);
-               printk(KERN_INFO "Read data[%d]: 0x%x\n", i, val);
-               memcpy(read_fifo, &val, 4);
-               read_fifo += 4;
-       }
-
-       /* Make sure all the data is read from the FIFO */
-       val = tegra_dsi_readl(dsi, DSI_STATUS);
-       val &= DSI_STATUS_RD_FIFO_COUNT(0x1f);
-       if (val)
-               dev_err(&dc->ndev->dev, "DSI FIFO_RD_CNT not zero"
-               " even after reading FIFO_RD_CNT words from read fifo\n");
-}
-
 static int tegra_dsi_parse_read_response(struct tegra_dc *dc,
-                               u32 rd_fifo_cnt, u8 *read_fifo)
+                                       u32 rd_fifo_cnt, u8 *read_fifo)
 {
        int err;
        u32 payload_size;
@@ -1435,107 +2153,133 @@ static int tegra_dsi_parse_read_response(struct tegra_dc *dc,
        payload_size = 0;
        err = 0;
 
-       printk(KERN_INFO "escape sequence[0x%x]\n", read_fifo[0]);
+       switch (read_fifo[0]) {
+       case DSI_ESCAPE_CMD:
+               dev_info(&dc->ndev->dev, "escape cmd[0x%x]\n", read_fifo[0]);
+               break;
+       case DSI_ACK_NO_ERR:
+               dev_info(&dc->ndev->dev,
+                       "Panel ack, no err[0x%x]\n", read_fifo[0]);
+               return err;
+       default:
+               dev_info(&dc->ndev->dev, "Invalid read response\n");
+               break;
+       }
+
        switch (read_fifo[4] & 0xff) {
        case GEN_LONG_RD_RES:
                /* Fall through */
        case DCS_LONG_RD_RES:
                payload_size = (read_fifo[5] |
                                (read_fifo[6] << 8)) & 0xFFFF;
-               printk(KERN_INFO "Long read response Packet\n"
+               dev_info(&dc->ndev->dev, "Long read response Packet\n"
                                "payload_size[0x%x]\n", payload_size);
                break;
        case GEN_1_BYTE_SHORT_RD_RES:
                /* Fall through */
        case DCS_1_BYTE_SHORT_RD_RES:
                payload_size = 1;
-               printk(KERN_INFO "Short read response Packet\n"
+               dev_info(&dc->ndev->dev, "Short read response Packet\n"
                        "payload_size[0x%x]\n", payload_size);
                break;
        case GEN_2_BYTE_SHORT_RD_RES:
                /* Fall through */
        case DCS_2_BYTE_SHORT_RD_RES:
                payload_size = 2;
-               printk(KERN_INFO "Short read response Packet\n"
+               dev_info(&dc->ndev->dev, "Short read response Packet\n"
                        "payload_size[0x%x]\n", payload_size);
                break;
        case ACK_ERR_RES:
                payload_size = 2;
-               printk(KERN_INFO "Acknowledge error report response\n"
+               dev_info(&dc->ndev->dev, "Acknowledge error report response\n"
                        "Packet payload_size[0x%x]\n", payload_size);
                break;
        default:
-               /*reading from RD_FIFO_COUNT*/
-               printk(KERN_INFO "Invalid read response payload_size\n");
+               dev_info(&dc->ndev->dev, "Invalid response packet\n");
                err = -EINVAL;
                break;
        }
        return err;
 }
 
-static int tegra_dsi_read_data(struct tegra_dc *dc,
-                               struct tegra_dc_dsi_data *dsi,
-                               u32 max_ret_payload_size,
-                               u32 panel_reg_addr, u8 *read_data)
+static int tegra_dsi_read_fifo(struct tegra_dc *dc,
+                       struct tegra_dc_dsi_data *dsi,
+                       u8 *read_fifo)
 {
        u32 val;
-       int err;
-       u32 poll_time;
+       u32 i;
+       u32 poll_time = 0;
        u32 rd_fifo_cnt;
-       bool switch_back_to_hs_mode;
-       bool restart_dc_stream;
-       bool switch_back_to_dc_mode;
+       int err = 0;
+       u8 *read_fifo_cp = read_fifo;
 
-       err = 0;
-       switch_back_to_hs_mode = false;
-       restart_dc_stream = false;
-       switch_back_to_dc_mode = false;
+       while (poll_time <  DSI_DELAY_FOR_READ_FIFO) {
+               mdelay(1);
+               val = tegra_dsi_readl(dsi, DSI_STATUS);
+               rd_fifo_cnt = val & DSI_STATUS_RD_FIFO_COUNT(0x1f);
+               if (rd_fifo_cnt << 2 > DSI_READ_FIFO_DEPTH)
+                       dev_err(&dc->ndev->dev,
+                       "DSI RD_FIFO_CNT is greater than RD_FIFO_DEPTH\n");
+                       break;
+               poll_time++;
+       }
 
-       if ((dsi->status.init != DSI_MODULE_INIT) ||
-               (dsi->status.lphs == DSI_LPHS_NOT_INIT) ||
-               (dsi->status.driven == DSI_DRIVEN_MODE_NOT_INIT)) {
-               err = -EPERM;
+       if (rd_fifo_cnt == 0) {
+               dev_info(&dc->ndev->dev,
+                       "DSI RD_FIFO_CNT is zero\n");
+               err = -EINVAL;
                goto fail;
        }
 
+       if (val & (DSI_STATUS_LB_UNDERFLOW(0x1) |
+               DSI_STATUS_LB_OVERFLOW(0x1))) {
+               dev_warn(&dc->ndev->dev,
+                       "DSI overflow/underflow error\n");
+       }
+
+       /* Read data from FIFO */
+       for (i = 0; i < rd_fifo_cnt; i++) {
+               val = tegra_dsi_readl(dsi, DSI_RD_DATA);
+               if (enable_read_debug)
+                       dev_info(&dc->ndev->dev,
+                       "Read data[%d]: 0x%x\n", i, val);
+               memcpy(read_fifo, &val, 4);
+               read_fifo += 4;
+       }
+
+       /* Make sure all the data is read from the FIFO */
        val = tegra_dsi_readl(dsi, DSI_STATUS);
        val &= DSI_STATUS_RD_FIFO_COUNT(0x1f);
-       if (val) {
-               err = -EBUSY;
-               dev_err(&dc->ndev->dev, "DSI fifo count not zero\n");
-               goto fail;
-       }
+       if (val)
+               dev_err(&dc->ndev->dev, "DSI FIFO_RD_CNT not zero"
+               " even after reading FIFO_RD_CNT words from read fifo\n");
 
-       if (!tegra_dsi_is_controller_idle(dsi)) {
-               err = -EBUSY;
-               dev_err(&dc->ndev->dev, "DSI trigger bit is already set\n");
-               goto fail;
+       if (enable_read_debug) {
+               err =
+               tegra_dsi_parse_read_response(dc, rd_fifo_cnt, read_fifo_cp);
+               if (err < 0)
+                       dev_warn(&dc->ndev->dev, "Unexpected read data\n");
        }
+fail:
+       return err;
+}
 
-       if (dsi->status.lphs == DSI_LPHS_IN_HS_MODE) {
-               if (dsi->status.driven == DSI_DRIVEN_MODE_DC) {
-                       if (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE)
-                               restart_dc_stream = true;
-                       dsi->driven_mode = TEGRA_DSI_DRIVEN_BY_HOST;
-                       switch_back_to_dc_mode = true;
-                       if (dsi->info.hs_cmd_mode_supported) {
-                               err = tegra_dsi_set_to_hs_mode(dc, dsi);
-                               if (err < 0) {
-                                       dev_err(&dc->ndev->dev,
-                                       "DSI failed to go to HS mode host driven\n");
-                                       goto fail;
-                               }
-                       }
-               }
-               if (!dsi->info.hs_cmd_mode_supported) {
-                       err = tegra_dsi_set_to_lp_mode(dc, dsi);
-                       if (err < 0) {
-                               dev_err(&dc->ndev->dev,
-                               "DSI failed to go to LP mode\n");
-                               goto fail;
-                       }
-                       switch_back_to_hs_mode = true;
-               }
+int tegra_dsi_read_data(struct tegra_dc *dc,
+                               struct tegra_dc_dsi_data *dsi,
+                               u32 max_ret_payload_size,
+                               u32 panel_reg_addr, u8 *read_data)
+{
+       int err = 0;
+       struct dsi_status *init_status;
+
+       tegra_dc_io_start(dc);
+
+       init_status = tegra_dsi_prepare_host_transmission(
+                               dc, dsi, DSI_LP_OP_WRITE);
+       if (IS_ERR_OR_NULL(init_status)) {
+               err = PTR_ERR(init_status);
+               dev_err(&dc->ndev->dev, "DSI host config failed\n");
+               goto fail;
        }
 
        /* Set max return payload size in words */
@@ -1558,6 +2302,17 @@ static int tegra_dsi_read_data(struct tegra_dc *dc,
                goto fail;
        }
 
+       tegra_dsi_reset_read_count(dsi);
+
+       if (dsi->status.lp_op == DSI_LP_OP_WRITE) {
+               err = tegra_dsi_set_to_lp_mode(dc, dsi, DSI_LP_OP_READ);
+               if (err < 0) {
+                       dev_err(&dc->ndev->dev,
+                       "DSI failed to go to LP read mode\n");
+                       goto fail;
+               }
+       }
+
        err = tegra_dsi_bta(dsi);
        if (err < 0) {
                dev_err(&dc->ndev->dev,
@@ -1565,46 +2320,81 @@ static int tegra_dsi_read_data(struct tegra_dc *dc,
                goto fail;
        }
 
-       poll_time = 0;
-       while (poll_time <  DSI_DELAY_FOR_READ_FIFO) {
-               mdelay(1);
-               val = tegra_dsi_readl(dsi, DSI_STATUS);
-               rd_fifo_cnt = val & DSI_STATUS_RD_FIFO_COUNT(0x1f);
-               if (rd_fifo_cnt << 2 > DSI_READ_FIFO_DEPTH)
-                       dev_err(&dc->ndev->dev,
-                       "DSI RD_FIFO_CNT is greater than RD_FIFO_DEPTH\n");
-                       break;
-               poll_time++;
+       err = tegra_dsi_read_fifo(dc, dsi, read_data);
+       if (err < 0) {
+               dev_err(&dc->ndev->dev, "DSI read fifo failure\n");
+               goto fail;
        }
+fail:
+       err = tegra_dsi_restore_state(dc, dsi, init_status);
+       if (err < 0)
+               dev_err(&dc->ndev->dev, "Failed to restore prev state\n");
+       tegra_dc_io_end(dc);
+       return err;
+}
+EXPORT_SYMBOL(tegra_dsi_read_data);
 
-       if (rd_fifo_cnt == 0) {
-               dev_info(&dc->ndev->dev,
-                       "DSI RD_FIFO_CNT is zero\n");
-               err = -EINVAL;
+int tegra_dsi_panel_sanity_check(struct tegra_dc *dc,
+                               struct tegra_dc_dsi_data *dsi)
+{
+       int err = 0;
+       u8 read_fifo[DSI_READ_FIFO_DEPTH];
+       struct dsi_status *init_status;
+       static struct tegra_dsi_cmd dsi_nop_cmd =
+                       DSI_CMD_SHORT(0x05, 0x0, 0x0);
+
+       tegra_dc_io_start(dc);
+
+       init_status = tegra_dsi_prepare_host_transmission(
+                                       dc, dsi, DSI_LP_OP_WRITE);
+       if (IS_ERR_OR_NULL(init_status)) {
+               err = PTR_ERR(init_status);
+               dev_err(&dc->ndev->dev, "DSI host config failed\n");
                goto fail;
        }
 
-       if (val & DSI_STATUS_LB_UNDERFLOW(0x1) ||
-               val & DSI_STATUS_LB_OVERFLOW(0x1)) {
-               dev_err(&dc->ndev->dev,
-                       "DSI overflow/underflow error\n");
-               err = -EINVAL;
+       err = _tegra_dsi_write_data(dsi, NULL, dsi_nop_cmd.data_id, 0x0);
+       if (err < 0) {
+               dev_err(&dc->ndev->dev, "DSI nop write failed\n");
                goto fail;
        }
 
-       tegra_dsi_read_fifo(dc, dsi, rd_fifo_cnt, read_data);
+       tegra_dsi_reset_read_count(dsi);
 
-       err = tegra_dsi_parse_read_response(dc, rd_fifo_cnt, read_data);
-fail:
-       if (switch_back_to_dc_mode)
-               dsi->driven_mode = TEGRA_DSI_DRIVEN_BY_DC;
-       if (switch_back_to_dc_mode || switch_back_to_hs_mode)
-               tegra_dsi_set_to_hs_mode(dc, dsi);
-       if (restart_dc_stream)
-               tegra_dsi_start_dc_stream(dc, dsi);
+       if (dsi->status.lp_op == DSI_LP_OP_WRITE) {
+               err = tegra_dsi_set_to_lp_mode(dc, dsi, DSI_LP_OP_READ);
+               if (err < 0) {
+                       dev_err(&dc->ndev->dev,
+                       "DSI failed to go to LP read mode\n");
+                       goto fail;
+               }
+       }
+
+       err = tegra_dsi_bta(dsi);
+       if (err < 0) {
+               dev_err(&dc->ndev->dev, "DSI BTA failed\n");
+               goto fail;
+       }
 
+       err = tegra_dsi_read_fifo(dc, dsi, read_fifo);
+       if (err < 0) {
+               dev_err(&dc->ndev->dev, "DSI read fifo failure\n");
+               goto fail;
+       }
+
+       if (read_fifo[0] != DSI_ACK_NO_ERR) {
+               dev_warn(&dc->ndev->dev,
+                       "Ack no error trigger message not received\n");
+               err = -EAGAIN;
+       }
+fail:
+       err = tegra_dsi_restore_state(dc, dsi, init_status);
+       if (err < 0)
+               dev_err(&dc->ndev->dev, "Failed to restore prev state\n");
+       tegra_dc_io_end(dc);
        return err;
 }
+EXPORT_SYMBOL(tegra_dsi_panel_sanity_check);
 
 static int tegra_dsi_enter_ulpm(struct tegra_dc_dsi_data *dsi)
 {
@@ -1621,7 +2411,8 @@ static int tegra_dsi_enter_ulpm(struct tegra_dc_dsi_data *dsi)
 #if DSI_USE_SYNC_POINTS
        ret = tegra_dsi_syncpt(dsi);
        if (ret < 0) {
-               printk(KERN_ERR "DSI syncpt for ulpm enter failed\n");
+               dev_err(&dsi->dc->ndev->dev,
+                       "DSI syncpt for ulpm enter failed\n");
                goto fail;
        }
 #else
@@ -1648,7 +2439,8 @@ static int tegra_dsi_exit_ulpm(struct tegra_dc_dsi_data *dsi)
 #if DSI_USE_SYNC_POINTS
        ret = tegra_dsi_syncpt(dsi);
        if (ret < 0) {
-               printk(KERN_ERR "DSI syncpt for ulpm exit failed\n");
+               dev_err(&dsi->dc->ndev->dev,
+                       "DSI syncpt for ulpm exit failed\n");
                goto fail;
        }
 #else
@@ -1679,22 +2471,33 @@ static void tegra_dc_dsi_enable(struct tegra_dc *dc)
         * to avoid visible glitches on panel during transition
         * from bootloader to kernel driver
         */
-       tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
+       tegra_dsi_stop_dc_stream(dc, dsi);
 
        if (dsi->enabled) {
                if (dsi->ulpm) {
                        if (tegra_dsi_exit_ulpm(dsi) < 0) {
-                               printk(KERN_ERR "DSI failed to exit ulpm\n");
+                               dev_err(&dc->ndev->dev,
+                                       "DSI failed to exit ulpm\n");
                                goto fail;
                        }
                }
+
                if (dsi->info.panel_reset) {
                        err = tegra_dsi_send_panel_cmd(dc, dsi,
                                                        dsi->info.dsi_init_cmd,
                                                        dsi->info.n_init_cmd);
                        if (err < 0) {
                                dev_err(&dc->ndev->dev,
-                               "dsi: error while sending dsi init cmd\n");
+                               "dsi: error sending dsi init cmd\n");
+                               goto fail;
+                       }
+               } else if (dsi->info.dsi_late_resume_cmd) {
+                       err = tegra_dsi_send_panel_cmd(dc, dsi,
+                                               dsi->info.dsi_late_resume_cmd,
+                                               dsi->info.n_late_resume_cmd);
+                       if (err < 0) {
+                               dev_err(&dc->ndev->dev,
+                               "dsi: error sending late resume cmd\n");
                                goto fail;
                        }
                }
@@ -1708,20 +2511,32 @@ static void tegra_dc_dsi_enable(struct tegra_dc *dc)
 
                if (dsi->ulpm) {
                        if (tegra_dsi_enter_ulpm(dsi) < 0) {
-                               printk(KERN_ERR "DSI failed to enter ulpm\n");
+                               dev_err(&dc->ndev->dev,
+                                       "DSI failed to enter ulpm\n");
                                goto fail;
                        }
-                       val = DSI_PAD_CONTROL_PAD_PDIO(0) |
+
+                       val = tegra_dsi_readl(dsi, DSI_PAD_CONTROL);
+
+                       /* erase bits we're about to set */
+                       val &= ~(DSI_PAD_CONTROL_PAD_PDIO(0x3) |
+                               DSI_PAD_CONTROL_PAD_PDIO_CLK(0x1) |
+                               DSI_PAD_CONTROL_PAD_PULLDN_ENAB(0x1));
+
+                       val |= (DSI_PAD_CONTROL_PAD_PDIO(0) |
                                DSI_PAD_CONTROL_PAD_PDIO_CLK(0) |
-                               DSI_PAD_CONTROL_PAD_PULLDN_ENAB(TEGRA_DSI_DISABLE);
+                               DSI_PAD_CONTROL_PAD_PULLDN_ENAB
+                                               (TEGRA_DSI_DISABLE));
+
                        tegra_dsi_writel(dsi, val, DSI_PAD_CONTROL);
                        if (tegra_dsi_exit_ulpm(dsi) < 0) {
-                               printk(KERN_ERR "DSI failed to exit ulpm\n");
+                               dev_err(&dc->ndev->dev,
+                                       "DSI failed to exit ulpm\n");
                                goto fail;
                        }
                }
 
-               err = tegra_dsi_set_to_lp_mode(dc, dsi);
+               err = tegra_dsi_set_to_lp_mode(dc, dsi, DSI_LP_OP_WRITE);
                if (err < 0) {
                        dev_err(&dc->ndev->dev,
                                "dsi: not able to set to lp mode\n");
@@ -1761,8 +2576,8 @@ static void _tegra_dc_dsi_init(struct tegra_dc *dc)
        /* TODO: Configure the CSI pad configuration */
 }
 
-static int tegra_dc_dsi_cp_p_cmd(struct tegra_dsi_cmd* src,
-                                       struct tegra_dsi_cmd* dst, u16 n_cmd)
+static int tegra_dc_dsi_cp_p_cmd(struct tegra_dsi_cmd *src,
+                                       struct tegra_dsi_cmd *dst, u16 n_cmd)
 {
        u16 i;
        u16 len;
@@ -1771,7 +2586,8 @@ static int tegra_dc_dsi_cp_p_cmd(struct tegra_dsi_cmd* src,
 
        for (i = 0; i < n_cmd; i++)
                if (src[i].pdata) {
-                       len = sizeof(*src[i].pdata) * src[i].sp_len_dly.data_len;
+                       len = sizeof(*src[i].pdata) *
+                                       src[i].sp_len_dly.data_len;
                        dst[i].pdata = kzalloc(len, GFP_KERNEL);
                        if (!dst[i].pdata)
                                goto free_cmd_pdata;
@@ -1781,41 +2597,86 @@ static int tegra_dc_dsi_cp_p_cmd(struct tegra_dsi_cmd* src,
        return 0;
 
 free_cmd_pdata:
-       for (--i; i >=0; i--)
+       for (--i; i >= 0; i--)
                if (dst[i].pdata)
                        kfree(dst[i].pdata);
        return -ENOMEM;
 }
 
-static int tegra_dc_dsi_cp_info(struct tegra_dc_dsi_data* dsi,
-                                               struct tegra_dsi_out* p_dsi)
+static int tegra_dc_dsi_cp_info(struct tegra_dc_dsi_data *dsi,
+                                       struct tegra_dsi_out *p_dsi)
 {
-       struct tegra_dsi_cmd* p_init_cmd;
+       struct tegra_dsi_cmd *p_init_cmd;
+       struct tegra_dsi_cmd *p_early_suspend_cmd;
+       struct tegra_dsi_cmd *p_late_resume_cmd;
        struct tegra_dsi_cmd *p_suspend_cmd;
        int err;
 
        if (p_dsi->n_data_lanes > MAX_DSI_DATA_LANES)
                return -EINVAL;
 
-       p_init_cmd = kzalloc(sizeof(*p_init_cmd) * p_dsi->n_init_cmd, GFP_KERNEL);
+       p_init_cmd = kzalloc(sizeof(*p_init_cmd) *
+                               p_dsi->n_init_cmd, GFP_KERNEL);
        if (!p_init_cmd)
                return -ENOMEM;
 
+       if (p_dsi->dsi_early_suspend_cmd) {
+               p_early_suspend_cmd = kzalloc(sizeof(*p_early_suspend_cmd) *
+                                       p_dsi->n_early_suspend_cmd,
+                                       GFP_KERNEL);
+               if (!p_early_suspend_cmd) {
+                       err = -ENOMEM;
+                       goto err_free_init_cmd;
+               }
+       }
+
+       if (p_dsi->dsi_late_resume_cmd) {
+               p_late_resume_cmd = kzalloc(sizeof(*p_late_resume_cmd) *
+                                       p_dsi->n_late_resume_cmd,
+                                       GFP_KERNEL);
+               if (!p_late_resume_cmd) {
+                       err = -ENOMEM;
+                       goto err_free_p_early_suspend_cmd;
+               }
+       }
+
        p_suspend_cmd = kzalloc(sizeof(*p_suspend_cmd) * p_dsi->n_suspend_cmd,
                                GFP_KERNEL);
        if (!p_suspend_cmd) {
                err = -ENOMEM;
-               goto err_free_p_init_cmd;
+               goto err_free_p_late_resume_cmd;
        }
 
        memcpy(&dsi->info, p_dsi, sizeof(dsi->info));
 
+       /* Copy panel init cmd */
        err = tegra_dc_dsi_cp_p_cmd(p_dsi->dsi_init_cmd,
                                                p_init_cmd, p_dsi->n_init_cmd);
        if (err < 0)
                goto err_free;
        dsi->info.dsi_init_cmd = p_init_cmd;
 
+       /* Copy panel early suspend cmd */
+       if (p_dsi->dsi_early_suspend_cmd) {
+               err = tegra_dc_dsi_cp_p_cmd(p_dsi->dsi_early_suspend_cmd,
+                                       p_early_suspend_cmd,
+                                       p_dsi->n_early_suspend_cmd);
+               if (err < 0)
+                       goto err_free;
+               dsi->info.dsi_early_suspend_cmd = p_early_suspend_cmd;
+       }
+
+       /* Copy panel late resume cmd */
+       if (p_dsi->dsi_late_resume_cmd) {
+               err = tegra_dc_dsi_cp_p_cmd(p_dsi->dsi_late_resume_cmd,
+                                               p_late_resume_cmd,
+                                               p_dsi->n_late_resume_cmd);
+               if (err < 0)
+                       goto err_free;
+               dsi->info.dsi_late_resume_cmd = p_late_resume_cmd;
+       }
+
+       /* Copy panel suspend cmd */
        err = tegra_dc_dsi_cp_p_cmd(p_dsi->dsi_suspend_cmd, p_suspend_cmd,
                                        p_dsi->n_suspend_cmd);
        if (err < 0)
@@ -1823,7 +2684,8 @@ static int tegra_dc_dsi_cp_info(struct tegra_dc_dsi_data* dsi,
        dsi->info.dsi_suspend_cmd = p_suspend_cmd;
 
        if (!dsi->info.panel_reset_timeout_msec)
-               dsi->info.panel_reset_timeout_msec = DEFAULT_PANEL_RESET_TIMEOUT;
+               dsi->info.panel_reset_timeout_msec =
+                                               DEFAULT_PANEL_RESET_TIMEOUT;
 
        if (!dsi->info.panel_buffer_size_byte)
                dsi->info.panel_buffer_size_byte = DEFAULT_PANEL_BUFFER_BYTE;
@@ -1843,14 +2705,25 @@ static int tegra_dc_dsi_cp_info(struct tegra_dc_dsi_data* dsi,
        if (!dsi->info.lp_cmd_mode_freq_khz)
                dsi->info.lp_cmd_mode_freq_khz = DEFAULT_LP_CMD_MODE_CLK_KHZ;
 
-       /* host mode is for testing only*/
-       dsi->driven_mode = TEGRA_DSI_DRIVEN_BY_DC;
+       if (!dsi->info.chip_id || !dsi->info.chip_rev)
+               dev_warn(&dsi->dc->ndev->dev,
+                       "DSI: Failed to get chip info\n");
+
+       if (!dsi->info.lp_read_cmd_mode_freq_khz)
+               dsi->info.lp_read_cmd_mode_freq_khz =
+                       dsi->info.lp_cmd_mode_freq_khz;
 
+       /* host mode is for testing only */
+       dsi->driven_mode = TEGRA_DSI_DRIVEN_BY_DC;
        return 0;
 
 err_free:
        kfree(p_suspend_cmd);
-err_free_p_init_cmd:
+err_free_p_late_resume_cmd:
+       kfree(p_late_resume_cmd);
+err_free_p_early_suspend_cmd:
+       kfree(p_early_suspend_cmd);
+err_free_init_cmd:
        kfree(p_init_cmd);
        return err;
 }
@@ -1920,10 +2793,6 @@ static int tegra_dc_dsi_init(struct tegra_dc *dc)
                goto err_clk_put;
        }
 
-       err = tegra_dc_dsi_cp_info(dsi, dsi_pdata);
-       if (err < 0)
-               goto err_dsi_data;
-
        mutex_init(&dsi->lock);
        dsi->dc = dc;
        dsi->base = base;
@@ -1931,6 +2800,10 @@ static int tegra_dc_dsi_init(struct tegra_dc *dc)
        dsi->dc_clk = dc_clk;
        dsi->dsi_clk = dsi_clk;
 
+       err = tegra_dc_dsi_cp_info(dsi, dsi_pdata);
+       if (err < 0)
+               goto err_dsi_data;
+
        tegra_dc_set_outdata(dc, dsi);
        _tegra_dc_dsi_init(dc);
 
@@ -1955,19 +2828,19 @@ static void tegra_dc_dsi_destroy(struct tegra_dc *dc)
 
        mutex_lock(&dsi->lock);
 
-       /* free up the pdata*/
-       for(i = 0; i < dsi->info.n_init_cmd; i++){
-               if(dsi->info.dsi_init_cmd[i].pdata)
+       /* free up the pdata */
+       for (i = 0; i < dsi->info.n_init_cmd; i++) {
+               if (dsi->info.dsi_init_cmd[i].pdata)
                        kfree(dsi->info.dsi_init_cmd[i].pdata);
        }
        kfree(dsi->info.dsi_init_cmd);
 
-       /* Disable dc stream*/
-       if(dsi->status.dc_stream == DSI_DC_STREAM_ENABLE)
-               tegra_dsi_stop_dc_stream(dc, dsi);
+       /* Disable dc stream */
+       if (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE)
+               tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
 
-       /* Disable dsi phy clock*/
-       if(dsi->status.clk_out == DSI_PHYCLK_OUT_EN)
+       /* Disable dsi phy clock */
+       if (dsi->status.clk_out == DSI_PHYCLK_OUT_EN)
                tegra_dsi_hs_clk_out_disable(dc, dsi);
 
        val = DSI_POWER_CONTROL_LEG_DSI_ENABLE(TEGRA_DSI_DISABLE);
@@ -1985,63 +2858,50 @@ static void tegra_dc_dsi_destroy(struct tegra_dc *dc)
        kfree(dsi);
 }
 
-static void tegra_dc_dsi_disable(struct tegra_dc *dc)
+static int tegra_dsi_deep_sleep(struct tegra_dc *dc,
+                               struct tegra_dc_dsi_data *dsi)
 {
-       struct tegra_dc_dsi_data *dsi = tegra_dc_get_outdata(dc);
-
-       mutex_lock(&dsi->lock);
-
-       if (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE)
-               tegra_dsi_stop_dc_stream(dc, dsi);
+       int err = 0;
+       int val;
+       struct clk *parent_clk = NULL;
+       struct clk *base_clk = NULL;
 
-       if (!dsi->ulpm) {
-               if (tegra_dsi_enter_ulpm(dsi) < 0)
-                       printk(KERN_ERR "DSI failed to enter ulpm\n");
+       if (!dsi->enabled) {
+               err = -EPERM;
+               goto fail;
        }
 
-       mutex_unlock(&dsi->lock);
-}
-
-#ifdef CONFIG_PM
-static void tegra_dc_dsi_suspend(struct tegra_dc *dc)
-{
-       struct tegra_dc_dsi_data *dsi;
-       int err;
-       u32 val;
-
-       dsi = tegra_dc_get_outdata(dc);
-
-       tegra_dc_io_start(dc);
-       mutex_lock(&dsi->lock);
-
-       if (!dsi->enabled)
+       err = tegra_dsi_set_to_lp_mode(dc, dsi, DSI_LP_OP_WRITE);
+       if (err < 0) {
+               dev_err(&dc->ndev->dev,
+               "DSI failed to go to LP mode\n");
                goto fail;
-
-       if (dsi->ulpm) {
-               if (tegra_dsi_exit_ulpm(dsi) < 0) {
-                       printk(KERN_ERR "DSI failed to exit ulpm");
-                       goto fail;
-               }
        }
 
-       /* Suspend Panel */
-       err = tegra_dsi_send_panel_cmd(dc, dsi, dsi->info.dsi_suspend_cmd,
-                               dsi->info.n_suspend_cmd);
+       /* Suspend panel */
+       err = tegra_dsi_send_panel_cmd(dc, dsi,
+                       dsi->info.dsi_suspend_cmd,
+                       dsi->info.n_suspend_cmd);
        if (err < 0) {
                dev_err(&dc->ndev->dev,
-                       "dsi: Error while sending dsi suspend cmd\n");
+                       "dsi: Error sending suspend cmd\n");
                goto fail;
        }
 
        if (!dsi->ulpm) {
-               if (tegra_dsi_enter_ulpm(dsi) < 0) {
-                       printk(KERN_ERR "DSI failed to enter ulpm\n");
+               err = tegra_dsi_enter_ulpm(dsi);
+               if (err < 0) {
+                       dev_err(&dc->ndev->dev,
+                               "DSI failed to enter ulpm\n");
                        goto fail;
                }
        }
 
-       /* Suspend pad */
-       val = tegra_dsi_readl(dsi, DSI_PAD_CONTROL);
+       /*
+        * Suspend pad
+        * It is ok to overwrite previous value of DSI_PAD_CONTROL reg
+        * because it will be restored properly in resume sequence
+        */
        val = DSI_PAD_CONTROL_PAD_PDIO(0x3) |
                DSI_PAD_CONTROL_PAD_PDIO_CLK(0x1) |
                DSI_PAD_CONTROL_PAD_PULLDN_ENAB(TEGRA_DSI_ENABLE);
@@ -2051,9 +2911,100 @@ static void tegra_dc_dsi_suspend(struct tegra_dc *dc)
        val = DSI_POWER_CONTROL_LEG_DSI_ENABLE(TEGRA_DSI_DISABLE);
        tegra_dsi_writel(dsi, val, DSI_POWER_CONTROL);
 
-       dsi->enabled = false;
+       /* Disable dsi fast and slow clock */
+       parent_clk = clk_get_parent(dsi->dsi_clk);
+       base_clk = clk_get_parent(parent_clk);
+       if (dsi->info.dsi_instance)
+               tegra_clk_cfg_ex(base_clk,
+                               TEGRA_CLK_PLLD_CSI_OUT_ENB,
+                               0);
+       else
+               tegra_clk_cfg_ex(base_clk,
+                               TEGRA_CLK_PLLD_DSI_OUT_ENB,
+                               0);
 
+       /* Disable dsi source clock */
        clk_disable(dsi->dsi_clk);
+
+       dsi->clk_ref = false;
+       dsi->enabled = false;
+
+       return 0;
+fail:
+       return err;
+}
+
+static void tegra_dc_dsi_disable(struct tegra_dc *dc)
+{
+       int err;
+       struct tegra_dc_dsi_data *dsi = tegra_dc_get_outdata(dc);
+
+       tegra_dc_io_start(dc);
+       mutex_lock(&dsi->lock);
+
+       if (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE)
+               tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
+
+       if (dsi->info.power_saving_suspend) {
+               if (tegra_dsi_deep_sleep(dc, dsi) < 0) {
+                       dev_err(&dc->ndev->dev,
+                               "DSI failed to enter deep sleep\n");
+                       goto fail;
+               }
+       } else {
+               if (dsi->info.dsi_early_suspend_cmd) {
+                       err = tegra_dsi_send_panel_cmd(dc, dsi,
+                               dsi->info.dsi_early_suspend_cmd,
+                               dsi->info.n_early_suspend_cmd);
+                       if (err < 0) {
+                               dev_err(&dc->ndev->dev,
+                               "dsi: Error sending early suspend cmd\n");
+                               goto fail;
+                       }
+               }
+
+               if (!dsi->ulpm) {
+                       if (tegra_dsi_enter_ulpm(dsi) < 0) {
+                               dev_err(&dc->ndev->dev,
+                                       "DSI failed to enter ulpm\n");
+                               goto fail;
+                       }
+               }
+       }
+
+fail:
+       mutex_unlock(&dsi->lock);
+       tegra_dc_io_end(dc);
+}
+
+#ifdef CONFIG_PM
+static void tegra_dc_dsi_suspend(struct tegra_dc *dc)
+{
+       struct tegra_dc_dsi_data *dsi;
+
+       dsi = tegra_dc_get_outdata(dc);
+
+       if (!dsi->enabled)
+               return;
+
+       tegra_dc_io_start(dc);
+       mutex_lock(&dsi->lock);
+
+       if (!dsi->info.power_saving_suspend) {
+               if (dsi->ulpm) {
+                       if (tegra_dsi_exit_ulpm(dsi) < 0) {
+                               dev_err(&dc->ndev->dev,
+                                       "DSI failed to exit ulpm");
+                               goto fail;
+                       }
+               }
+
+               if (tegra_dsi_deep_sleep(dc, dsi) < 0) {
+                       dev_err(&dc->ndev->dev,
+                               "DSI failed to enter deep sleep\n");
+                       goto fail;
+               }
+       }
 fail:
        mutex_unlock(&dsi->lock);
        tegra_dc_io_end(dc);