fixup: moduleparam.h/export.h (dsi.c)
[linux-2.6.git] / drivers / video / tegra / dc / dsi.c
index e46166a..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/csi.h>
 #include <linux/nvhost.h>
-#include <../gpio-names.h>
 
 #include "dc_reg.h"
 #include "dc_priv.h"
 #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;
 
@@ -112,6 +118,8 @@ struct tegra_dc_dsi_data {
 
        struct dsi_status status;
 
+       struct dsi_phy_timing_inclk phy_timing;
+
        u8 driven_mode;
        u8 controller_index;
 
@@ -247,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,
@@ -277,12 +289,14 @@ const u32 init_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);
 
 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);
@@ -295,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;
        }
 
@@ -448,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) /
@@ -493,70 +513,408 @@ static void tegra_dsi_init_sw(struct tegra_dc *dc,
        if (dsi->info.enable_hs_clock_on_lp_cmd_mode) {
                if (dsi->info.video_clock_mode !=
                                        TEGRA_DSI_VIDEO_CLOCK_CONTINUOUS)
-                       printk(KERN_WARNING "Force clock continuous mode\n");
+                       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;
+}
+
+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;
 
-       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));
+       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 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)
+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) |
@@ -574,6 +932,8 @@ 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,
@@ -633,8 +993,10 @@ static void tegra_dsi_set_sol_delay(struct tegra_dc *dc,
        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);
@@ -827,17 +1189,39 @@ 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)
 {
+       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,
+static void tegra_dsi_stop_dc_stream_at_frame_end(struct tegra_dc *dc,
                                                struct tegra_dc_dsi_data *dsi)
 {
        int val;
@@ -848,26 +1232,28 @@ void tegra_dsi_stop_dc_stream_at_frame_end(struct tegra_dc *dc,
        tegra_dsi_stop_dc_stream(dc, dsi);
 
        /* enable frame end interrupt */
-       val = tegra_dc_readl(dc, DC_CMD_INT_ENABLE);
-       val |= FRAME_END_INT;
-       tegra_dc_writel(dc, val, DC_CMD_INT_ENABLE);
-
        val = tegra_dc_readl(dc, DC_CMD_INT_MASK);
        val |= FRAME_END_INT;
        tegra_dc_writel(dc, val, DC_CMD_INT_MASK);
 
-       /* wait for frame_end completion */
+       /* wait for frame_end completion.
+        * timeout is 2 frame duration to accomodate for
+        * internal delay.
+        */
        timeout = wait_for_completion_interruptible_timeout(
                        &dc->frame_end_complete,
-                       msecs_to_jiffies(frame_period));
+                       msecs_to_jiffies(2 * frame_period));
 
        /* disable frame end interrupt */
-       val = tegra_dc_readl(dc, DC_CMD_INT_ENABLE);
+       val = tegra_dc_readl(dc, DC_CMD_INT_MASK);
        val &= ~FRAME_END_INT;
-       tegra_dc_writel(dc, val, DC_CMD_INT_ENABLE);
+       tegra_dc_writel(dc, val, DC_CMD_INT_MASK);
 
        if (timeout == 0)
-               printk(KERN_WARNING "DC doesn't stop at end of frame.\n");
+               dev_warn(&dc->ndev->dev,
+                       "DC doesn't stop at end of frame.\n");
+
+       tegra_dsi_reset_underflow_overflow(dsi);
 }
 
 static void tegra_dsi_start_dc_stream(struct tegra_dc *dc,
@@ -922,7 +1308,7 @@ 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
 
@@ -937,26 +1323,29 @@ static void tegra_dsi_set_dsi_clk(struct tegra_dc *dc,
 {
        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);
-       if (dsi->clk_ref == true)
-               clk_disable(dsi->dsi_clk);
-       else
+       if (!dsi->clk_ref) {
                dsi->clk_ref = true;
-       clk_enable(dsi->dsi_clk);
-       tegra_periph_reset_deassert(dsi->dsi_clk);
-
+               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);
 }
 
@@ -1001,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;
 }
@@ -1083,7 +1473,7 @@ 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 void tegra_dsi_pad_caliberation(struct tegra_dc_dsi_data *dsi)
+static void tegra_dsi_pad_calibration(struct tegra_dc_dsi_data *dsi)
 {
        u32 val;
 
@@ -1114,16 +1504,21 @@ static void tegra_dsi_pad_caliberation(struct tegra_dc_dsi_data *dsi)
 
        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 val;
        u32 i;
 
-       val = DSI_POWER_CONTROL_LEG_DSI_ENABLE(TEGRA_DSI_DISABLE);
-       tegra_dsi_writel(dsi, val, DSI_POWER_CONTROL);
+       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) {
@@ -1131,10 +1526,10 @@ static int tegra_dsi_init_hw(struct tegra_dc *dc,
        }
 
        /* TODO: only need to change the timing for bta */
-       tegra_dsi_set_phy_timing(dsi);
+       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(dc, dsi);
+               tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
 
        /* Initializing DSI registers */
        for (i = 0; i < ARRAY_SIZE(init_reg); i++)
@@ -1142,13 +1537,13 @@ static int tegra_dsi_init_hw(struct tegra_dc *dc,
 
        tegra_dsi_writel(dsi, dsi->dsi_control_val, DSI_CONTROL);
 
-       tegra_dsi_pad_caliberation(dsi);
+       tegra_dsi_pad_calibration(dsi);
 
-       val = DSI_POWER_CONTROL_LEG_DSI_ENABLE(TEGRA_DSI_ENABLE);
-       tegra_dsi_writel(dsi, val, DSI_POWER_CONTROL);
-
-       while (tegra_dsi_readl(dsi, DSI_POWER_CONTROL) != val)
-               tegra_dsi_writel(dsi, val, DSI_POWER_CONTROL);
+       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;
@@ -1191,15 +1586,17 @@ static int tegra_dsi_set_to_lp_mode(struct tegra_dc *dc,
                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;
@@ -1215,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);
 
@@ -1227,7 +1627,7 @@ 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) {
                tegra_dsi_set_pkt_seq(dc, dsi);
@@ -1238,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);
@@ -1265,6 +1668,228 @@ static bool tegra_dsi_is_controller_idle(struct tegra_dc_dsi_data *dsi)
        return retVal;
 }
 
+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)
+{
+       u32 val;
+
+       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;
@@ -1280,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:
@@ -1330,7 +1959,7 @@ 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;
 }
@@ -1339,50 +1968,25 @@ 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;
+       int err = 0;
+       struct dsi_status *init_status;
 
-       err = 0;
-       switch_back_to_hs_mode = false;
-       switch_back_to_dc_mode = false;
+       tegra_dc_io_start(dc);
 
-       if ((dsi->status.init != DSI_MODULE_INIT) ||
-               (dsi->status.lphs == DSI_LPHS_NOT_INIT) ||
-               (dsi->status.lp_op == DSI_LP_OP_NOT_INIT)) {
-               err = -EPERM;
+       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 (!tegra_dsi_is_controller_idle(dsi)) {
-               err = -EBUSY;
-               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, DSI_LP_OP_WRITE);
-                       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);
@@ -1414,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;
@@ -1430,58 +2118,32 @@ 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)
 {
@@ -1491,123 +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;
 }
 
-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;
-
-       if ((dsi->status.init != DSI_MODULE_INIT) ||
-               (dsi->status.lphs == DSI_LPHS_NOT_INIT) ||
-               (dsi->status.driven == DSI_DRIVEN_MODE_NOT_INIT) ||
-               (dsi->status.lp_op == DSI_LP_OP_NOT_INIT)) {
-               err = -EPERM;
-               goto fail;
+       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++;
        }
 
-       tegra_dc_io_start(dc);
-
-       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");
+       if (rd_fifo_cnt == 0) {
+               dev_info(&dc->ndev->dev,
+                       "DSI RD_FIFO_CNT is zero\n");
+               err = -EINVAL;
                goto fail;
        }
 
-       if (!tegra_dsi_is_controller_idle(dsi)) {
-               err = -EBUSY;
-               dev_err(&dc->ndev->dev, "DSI trigger bit is already set\n");
-               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_LB_OVERFLOW(0x1) | DSI_STATUS_LB_UNDERFLOW(0x1));
-       if (val) {
-               dev_warn(&dc->ndev->dev, "Reset overflow/underflow\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);
-               ndelay(200);
+       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");
+
+       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) {
-                               tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
-                               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 host driven mode\n");
-                                       goto fail;
-                               }
-                       }
-               }
-               if (!dsi->info.hs_cmd_mode_supported) {
-                       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;
-                       }
-                       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 */
@@ -1630,11 +2302,13 @@ int tegra_dsi_read_data(struct tegra_dc *dc,
                goto fail;
        }
 
-       if (switch_back_to_hs_mode) {
+       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 mode\n");
+                       "DSI failed to go to LP read mode\n");
                        goto fail;
                }
        }
@@ -1646,49 +2320,81 @@ 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;
+               }
+       }
 
-       tegra_dc_io_end(dc);
+       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_read_data);
+EXPORT_SYMBOL(tegra_dsi_panel_sanity_check);
 
 static int tegra_dsi_enter_ulpm(struct tegra_dc_dsi_data *dsi)
 {
@@ -1705,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
@@ -1732,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
@@ -1763,12 +2471,13 @@ 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;
                        }
                }
@@ -1802,16 +2511,27 @@ 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);
+                                               (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;
                        }
                }
@@ -1986,7 +2706,8 @@ static int tegra_dc_dsi_cp_info(struct tegra_dc_dsi_data *dsi,
                dsi->info.lp_cmd_mode_freq_khz = DEFAULT_LP_CMD_MODE_CLK_KHZ;
 
        if (!dsi->info.chip_id || !dsi->info.chip_rev)
-               printk(KERN_WARNING "DSI: Failed to get chip info\n");
+               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 =
@@ -2116,7 +2837,7 @@ static void tegra_dc_dsi_destroy(struct tegra_dc *dc)
 
        /* Disable dc stream */
        if (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE)
-               tegra_dsi_stop_dc_stream(dc, dsi);
+               tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
 
        /* Disable dsi phy clock */
        if (dsi->status.clk_out == DSI_PHYCLK_OUT_EN)
@@ -2176,8 +2897,11 @@ static int tegra_dsi_deep_sleep(struct tegra_dc *dc,
                }
        }
 
-       /* 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);
@@ -2223,7 +2947,8 @@ static void tegra_dc_dsi_disable(struct tegra_dc *dc)
 
        if (dsi->info.power_saving_suspend) {
                if (tegra_dsi_deep_sleep(dc, dsi) < 0) {
-                       printk(KERN_ERR "DSI failed to enter deep sleep\n");
+                       dev_err(&dc->ndev->dev,
+                               "DSI failed to enter deep sleep\n");
                        goto fail;
                }
        } else {
@@ -2240,7 +2965,8 @@ static void tegra_dc_dsi_disable(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;
                        }
                }
@@ -2258,22 +2984,24 @@ static void tegra_dc_dsi_suspend(struct tegra_dc *dc)
 
        dsi = tegra_dc_get_outdata(dc);
 
+       if (!dsi->enabled)
+               return;
+
        tegra_dc_io_start(dc);
        mutex_lock(&dsi->lock);
 
-       if (!dsi->enabled)
-               goto fail;
-
        if (!dsi->info.power_saving_suspend) {
                if (dsi->ulpm) {
                        if (tegra_dsi_exit_ulpm(dsi) < 0) {
-                               printk(KERN_ERR "DSI failed to exit ulpm");
+                               dev_err(&dc->ndev->dev,
+                                       "DSI failed to exit ulpm");
                                goto fail;
                        }
                }
 
                if (tegra_dsi_deep_sleep(dc, dsi) < 0) {
-                       printk(KERN_ERR "DSI failed to enter deep sleep\n");
+                       dev_err(&dc->ndev->dev,
+                               "DSI failed to enter deep sleep\n");
                        goto fail;
                }
        }