From a060e4d4a34b0fe17384e2d02e65c74fe73ee4c9 Mon Sep 17 00:00:00 2001
From: Prajwal Mohan <prajwal.karur.mohan@intel.com>
Date: Thu, 6 May 2010 14:29:26 -0700
Subject: [PATCH] Graphics changes for Aava Koski DV1 hardware

Signed-Off-By: Prajwal Mohan <prajwal.karur.mohan@intel.com>
Patch-mainline: 2.6.35?
---
 drivers/gpu/drm/mrst/drv/psb_drv.h                 |    7 +
 drivers/gpu/drm/mrst/drv/psb_intel_display.c       |   75 +-
 drivers/gpu/drm/mrst/drv/psb_intel_dsi.c           |    9 +
 drivers/gpu/drm/mrst/drv/psb_intel_dsi_aava.c      | 1356 ++++++++++----------
 .../linux_framebuffer_mrst/mrstlfb_displayclass.c  |   21 +-
 5 files changed, 735 insertions(+), 733 deletions(-)

diff --git a/drivers/gpu/drm/mrst/drv/psb_drv.h b/drivers/gpu/drm/mrst/drv/psb_drv.h
index 2ac7934..56c1e90 100644
--- a/drivers/gpu/drm/mrst/drv/psb_drv.h
+++ b/drivers/gpu/drm/mrst/drv/psb_drv.h
@@ -413,6 +413,8 @@ struct drm_psb_private {
 	uint32_t dspcntr;
 
 /* MRST_DSI private date start */
+	struct work_struct dsi_work;
+
 	/*
 	 *MRST DSI info
 	 */
@@ -430,6 +432,9 @@ struct drm_psb_private {
 
 	enum mipi_panel_type panel_make;
 
+	/* Set if MIPI encoder wants to control plane/pipe */
+	bool dsi_plane_pipe_control;
+
 	/* status */
 	uint32_t videoModeFormat:2;
 	uint32_t laneCount:3;
@@ -610,6 +615,8 @@ struct drm_psb_private {
 	uint32_t saveMIPI_CONTROL_REG;
 	uint32_t saveMIPI;
 	void (*init_drvIC)(struct drm_device *dev);
+	void (*dsi_prePowerState)(struct drm_device *dev);
+	void (*dsi_postPowerState)(struct drm_device *dev);
 
 	/* DPST Register Save */
 	uint32_t saveHISTOGRAM_INT_CONTROL_REG;
diff --git a/drivers/gpu/drm/mrst/drv/psb_intel_display.c b/drivers/gpu/drm/mrst/drv/psb_intel_display.c
index 10c6dec..72d42eb 100644
--- a/drivers/gpu/drm/mrst/drv/psb_intel_display.c
+++ b/drivers/gpu/drm/mrst/drv/psb_intel_display.c
@@ -2089,6 +2089,7 @@ static void mrst_crtc_dpms(struct drm_crtc *crtc, int mode)
 {
 	struct drm_device *dev = crtc->dev;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	int pipe = psb_intel_crtc->pipe;
 	int dpll_reg = (pipe == 0) ? MRST_DPLL_A : DPLL_B;
 	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
@@ -2130,18 +2131,22 @@ static void mrst_crtc_dpms(struct drm_crtc *crtc, int mode)
 			udelay(150);
 		}
 
-		/* Enable the pipe */
-		temp = REG_READ(pipeconf_reg);
-		if ((temp & PIPEACONF_ENABLE) == 0)
-			REG_WRITE(pipeconf_reg, temp | PIPEACONF_ENABLE);
-
-		/* Enable the plane */
-		temp = REG_READ(dspcntr_reg);
-		if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
-			REG_WRITE(dspcntr_reg,
-				  temp | DISPLAY_PLANE_ENABLE);
-			/* Flush the plane changes */
-			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+		if (dev_priv->iLVDS_enable ||
+					!dev_priv->dsi_plane_pipe_control) {
+			/* Enable the pipe */
+			temp = REG_READ(pipeconf_reg);
+			if ((temp & PIPEACONF_ENABLE) == 0)
+				REG_WRITE(pipeconf_reg,
+						temp | PIPEACONF_ENABLE);
+
+			/* Enable the plane */
+			temp = REG_READ(dspcntr_reg);
+			if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
+				REG_WRITE(dspcntr_reg,
+					  temp | DISPLAY_PLANE_ENABLE);
+				/* Flush the plane changes */
+				REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+			}
 		}
 
 		psb_intel_crtc_load_lut(crtc);
@@ -2158,30 +2163,34 @@ static void mrst_crtc_dpms(struct drm_crtc *crtc, int mode)
 		/* Disable the VGA plane that we never use */
 		REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
 
-		/* Disable display plane */
-		temp = REG_READ(dspcntr_reg);
-		if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
-			REG_WRITE(dspcntr_reg,
-				  temp & ~DISPLAY_PLANE_ENABLE);
-			/* Flush the plane changes */
-			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
-			REG_READ(dspbase_reg);
-		}
+		if (dev_priv->iLVDS_enable ||
+					!dev_priv->dsi_plane_pipe_control) {
+			/* Disable display plane */
+			temp = REG_READ(dspcntr_reg);
+			if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
+				REG_WRITE(dspcntr_reg,
+					  temp & ~DISPLAY_PLANE_ENABLE);
+				/* Flush the plane changes */
+				REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+				REG_READ(dspbase_reg);
+			}
 
-		if (!IS_I9XX(dev)) {
-			/* Wait for vblank for the disable to take effect */
-			psb_intel_wait_for_vblank(dev);
-		}
+			if (!IS_I9XX(dev)) {
+				/* Wait for vblank for the disable to take effect */
+				psb_intel_wait_for_vblank(dev);
+			}
 
-		/* Next, disable display pipes */
-		temp = REG_READ(pipeconf_reg);
-		if ((temp & PIPEACONF_ENABLE) != 0) {
-			REG_WRITE(pipeconf_reg, temp & ~PIPEACONF_ENABLE);
-			REG_READ(pipeconf_reg);
-		}
+			/* Next, disable display pipes */
+			temp = REG_READ(pipeconf_reg);
+			if ((temp & PIPEACONF_ENABLE) != 0) {
+				REG_WRITE(pipeconf_reg,
+						temp & ~PIPEACONF_ENABLE);
+				REG_READ(pipeconf_reg);
+			}
 
-		/* Wait for for the pipe disable to take effect. */
-		mrstWaitForPipeDisable(dev);
+			/* Wait for for the pipe disable to take effect. */
+			mrstWaitForPipeDisable(dev);
+		}
 
 		temp = REG_READ(dpll_reg);
 		if ((temp & DPLL_VCO_ENABLE) != 0) {
diff --git a/drivers/gpu/drm/mrst/drv/psb_intel_dsi.c b/drivers/gpu/drm/mrst/drv/psb_intel_dsi.c
index 3d45df8..eb6cb2a 100644
--- a/drivers/gpu/drm/mrst/drv/psb_intel_dsi.c
+++ b/drivers/gpu/drm/mrst/drv/psb_intel_dsi.c
@@ -18,6 +18,11 @@
  *	jim liu <jim.liu@intel.com>
  */
 
+#define USE_AAVA_VERSION
+#ifdef USE_AAVA_VERSION
+#include "psb_intel_dsi_aava.c"
+#else
+
 #include <linux/backlight.h>
 #include <drm/drmP.h>
 #include <drm/drm.h>
@@ -2448,3 +2453,7 @@ failed_find:
 	drm_connector_cleanup(connector);
 	kfree(connector);
 }
+
+#endif /* USE_AAVA_VERSION */
+
+
diff --git a/drivers/gpu/drm/mrst/drv/psb_intel_dsi_aava.c b/drivers/gpu/drm/mrst/drv/psb_intel_dsi_aava.c
index 6c21480..9e761c6 100644
--- a/drivers/gpu/drm/mrst/drv/psb_intel_dsi_aava.c
+++ b/drivers/gpu/drm/mrst/drv/psb_intel_dsi_aava.c
@@ -1,32 +1,4 @@
-/*
- * Copyright © 2006-2007 Intel Corporation
- *
- * Permission is hereby granted, free of charge, to any person obtaining a
- * copy of this software and associated documentation files (the "Software"),
- * to deal in the Software without restriction, including without limitation
- * the rights to use, copy, modify, merge, publish, distribute, sublicense,
- * and/or sell copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice (including the next
- * paragraph) shall be included in all copies or substantial portions of the
- * Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
- * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
- * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
- * DEALINGS IN THE SOFTWARE.
- *
- * Authors:
- *	jim liu <jim.liu@intel.com>
- */
-
-/* This enables setting backlights on with a delay at startup,
-   should be removed after resolving issue with backlights going off
-   after setting them on in initial mrst_dsi_set_power call */
+
 #define AAVA_BACKLIGHT_HACK
 
 #include <linux/backlight.h>
@@ -34,26 +6,33 @@
 #include <drm/drm.h>
 #include <drm/drm_crtc.h>
 #include <drm/drm_edid.h>
-
 #include <asm/ipc_defs.h>
 
-#ifdef AAVA_BACKLIGHT_HACK
-#include <linux/workqueue.h>
-#endif /* AAVA_BACKLIGHT_HACK */
-
 #include "psb_drv.h"
 #include "psb_intel_drv.h"
 #include "psb_intel_reg.h"
 #include "ospm_power.h"
 
-#define DRM_MODE_ENCODER_MIPI   5
+#ifdef AAVA_BACKLIGHT_HACK
+#include <linux/workqueue.h>
+#endif /* AAVA_BACKLIGHT_HACK */
 
-//#define DBG_PRINTS 1
-#define DBG_PRINTS 0
+/* Debug trace definitions */
+#define DBG_LEVEL 1
 
-#define NEW_CRAP_SAMPLE_SETTINGS
+#if (DBG_LEVEL > 0)
+#define DBG_TRACE(format,args...) printk(KERN_ERR "%s: " format "\n", \
+						__FUNCTION__ , ## args)
+#else
+#define DBG_TRACE(format,args...)
+#endif
+
+#define DBG_ERR(format,args...) printk(KERN_ERR "%s: " format "\n", \
+						__FUNCTION__ , ## args)
 
-#define AAVA_EV_0_5
+#define BRIGHTNESS_MAX_LEVEL 100
+
+#define DRM_MODE_ENCODER_MIPI   5
 
 #define VSIZE		480
 #define HSIZE		864
@@ -84,686 +63,633 @@
 #define DISP_VSYNC_START	(DISP_VBLANK_START + VFP_LINES - 1)
 #define DISP_VSYNC_END		(DISP_VSYNC_START + VSYNC_LINES - 1)
 
-#define BRIGHTNESS_MAX_LEVEL 100
+#define MAX_FIFO_WAIT_MS	100
 
 static unsigned int dphy_reg = 0x0d0a7f06;
 static unsigned int mipi_clock = 0x2;
 
 #ifdef AAVA_BACKLIGHT_HACK
-static void bl_work_handler(struct work_struct *work);
-DECLARE_DELAYED_WORK(bl_work, bl_work_handler);
+static void dsi_bl_work_handler(struct work_struct *work);
+DECLARE_DELAYED_WORK(bl_work, dsi_bl_work_handler);
 #endif /* AAVA_BACKLIGHT_HACK */
 
 // Temporary access from sysfs begin
-static struct drm_encoder *orig_encoder;
-static void mrst_dsi_prepare(struct drm_encoder *encoder);
-static void mrst_dsi_commit(struct drm_encoder *encoder);
-static void mrst_dsi_mode_set(struct drm_encoder *encoder,
-			      struct drm_display_mode *mode,
-			      struct drm_display_mode *adjusted_mode);
-static void panel_reset(void);
-
-static ssize_t dphy_store(struct class *class, const char *buf, size_t len)
+static struct drm_device *test_dev;
+// Temporary access from sysfs end
+
+
+static int dsi_wait_hs_data_fifo(struct drm_device *dev)
 {
-	ssize_t	status;
-	unsigned long value;
+	int fifo_wait_time = 0;
 
-	status = strict_strtoul(buf, 16, &value);
-	dphy_reg = value;
-	printk("!!! dphy_reg = %x, clock = %x\n", dphy_reg, mipi_clock);
+	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_DATA_FIFO_FULL) ==
+							HS_DATA_FIFO_FULL) {
+		if (fifo_wait_time == MAX_FIFO_WAIT_MS) {
+			DBG_ERR("timeout");
+			return -1;
+		}
+		udelay(1000);
+		fifo_wait_time++;
+	}
+	return 0;
+}
 
-	return len;
+static int dsi_wait_hs_ctrl_fifo(struct drm_device *dev)
+{
+	int fifo_wait_time = 0;
+
+	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_CTRL_FIFO_FULL) ==
+							HS_CTRL_FIFO_FULL) {
+		if (fifo_wait_time == MAX_FIFO_WAIT_MS) {
+			DBG_ERR("timeout");
+			return -1;
+		}
+		udelay(1000);
+		fifo_wait_time++;
+	}
+	return 0;
 }
 
-static ssize_t clock_store(struct class *class, const char *buf, size_t len)
+static void dsi_set_backlight_state(int state)
 {
-	ssize_t	status;
-	unsigned long value;
+	struct ipc_pmic_reg_data tmp_reg;
 
-	status = strict_strtoul(buf, 0, &value);
-	mipi_clock = value;
-	printk("!!! dphy_reg = %x, clock = %x\n", dphy_reg, mipi_clock);
+	DBG_TRACE("%d", state);
 
-	return len;
+	tmp_reg.ioc = 1;
+	tmp_reg.num_entries = 2;
+	tmp_reg.pmic_reg_data[0].register_address = 0x2a;
+	tmp_reg.pmic_reg_data[1].register_address = 0x28;
+
+	if (state) {
+		tmp_reg.pmic_reg_data[0].value = 0xaa;
+		tmp_reg.pmic_reg_data[1].value = 0x30;
+	} else {
+		tmp_reg.pmic_reg_data[0].value = 0x00;
+		tmp_reg.pmic_reg_data[1].value = 0x00;
+	}
+
+	if (ipc_pmic_register_write(&tmp_reg, TRUE))
+		DBG_ERR("pmic reg write failed");
 }
 
-static ssize_t apply_settings(struct class *class, const char *buf, size_t len)
+
+#ifdef AAVA_BACKLIGHT_HACK
+static void dsi_bl_work_handler(struct work_struct *work)
 {
-	ssize_t	status;
-	long value;
+	DBG_TRACE("");
+	dsi_set_backlight_state(1);
+}
+#endif /* AAVA_BACKLIGHT_HACK */
 
-	printk("!!! dphy_reg = %x, clock = %x\n", dphy_reg, mipi_clock);
 
-	status = strict_strtoul(buf, 0, &value);
-	if (value > 0) {
-		mrst_dsi_prepare(orig_encoder);
-		msleep(500);
-		if (value > 1) {
-			panel_reset();
-			msleep(500);
-		}
-		mrst_dsi_mode_set(orig_encoder, NULL, NULL);
-		msleep(500);
-		mrst_dsi_commit(orig_encoder);
+static void dsi_set_panel_reset_state(int state)
+{
+	struct ipc_pmic_reg_data tmp_reg = {0};
+
+	DBG_TRACE("%d", state);
+
+	tmp_reg.ioc = 1;
+	tmp_reg.num_entries = 1;
+	tmp_reg.pmic_reg_data[0].register_address = 0xe6;
+
+	if (state)
+		tmp_reg.pmic_reg_data[0].value = 0x01;
+	else
+		tmp_reg.pmic_reg_data[0].value = 0x09;
+
+	if (ipc_pmic_register_write(&tmp_reg, TRUE)) {
+		DBG_ERR("pmic reg write failed");
+		return;
 	}
 
-	return len;
+	if (state) {
+		/* Minimum active time to trigger reset is 10us */
+		udelay(10);
+	} else {
+		/* Maximum startup time from reset is 120ms */
+		msleep(120);
+	}
 }
-// Temporary access from sysfs end
 
-static void panel_init(struct drm_device *dev)
+
+static void dsi_init_panel(struct drm_device *dev)
 {
-#if DBG_PRINTS
-	printk("panel_init\n");
-#endif /* DBG_PRINTS */
+	DBG_TRACE("");
 
-	/* Flip page order */
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_DATA_FIFO_FULL) ==
-							HS_DATA_FIFO_FULL);
+	/* Flip page order to have correct image orientation */
+	if (dsi_wait_hs_data_fifo(dev) < 0)
+		return;
 	REG_WRITE(0xb068, 0x00008036);
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_CTRL_FIFO_FULL) ==
-							HS_CTRL_FIFO_FULL);
+	if (dsi_wait_hs_ctrl_fifo(dev) < 0)
+		return;
 	REG_WRITE(0xb070, 0x00000229);
 
-#ifdef NEW_CRAP_SAMPLE_SETTINGS
-	// 0xF0, for new crap displays
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_DATA_FIFO_FULL) ==
-							HS_DATA_FIFO_FULL);
-	REG_WRITE(0xb068, 0x005a5af0);
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_CTRL_FIFO_FULL) ==
-							HS_CTRL_FIFO_FULL);
+	/* Write protection key to allow DM bit setting */
+	if (dsi_wait_hs_data_fifo(dev) < 0)
+		return;
+	REG_WRITE(0xb068, 0x005a5af1);
+	if (dsi_wait_hs_ctrl_fifo(dev) < 0)
+		return;
 	REG_WRITE(0xb070, 0x00000329);
-#endif
 
-	/* Write protection key */
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_DATA_FIFO_FULL) ==
-							HS_DATA_FIFO_FULL);
-	REG_WRITE(0xb068, 0x005a5af1);
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_CTRL_FIFO_FULL) ==
-							HS_CTRL_FIFO_FULL);
+	/* Set DM bit to enable video mode */
+	if (dsi_wait_hs_data_fifo(dev) < 0)
+		return;
+	REG_WRITE(0xb068, 0x000100f7);
+	if (dsi_wait_hs_ctrl_fifo(dev) < 0)
+		return;
 	REG_WRITE(0xb070, 0x00000329);
 
-#ifdef NEW_CRAP_SAMPLE_SETTINGS
-	// 0xFC, for new crap displays
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_DATA_FIFO_FULL) ==
-							HS_DATA_FIFO_FULL);
+	/* Write protection keys to allow TCON setting */
+	if (dsi_wait_hs_data_fifo(dev) < 0)
+		return;
+	REG_WRITE(0xb068, 0x005a5af0);
+	if (dsi_wait_hs_ctrl_fifo(dev) < 0)
+		return;
+	REG_WRITE(0xb070, 0x00000329);
+
+	if (dsi_wait_hs_data_fifo(dev) < 0)
+		return;
 	REG_WRITE(0xb068, 0x005a5afc);
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_CTRL_FIFO_FULL) ==
-							HS_CTRL_FIFO_FULL);
+	if (dsi_wait_hs_ctrl_fifo(dev) < 0)
+		return;
 	REG_WRITE(0xb070, 0x00000329);
 
-	// 0xB7, for new crap displays
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_DATA_FIFO_FULL) ==
-							HS_DATA_FIFO_FULL);
-#ifdef DOES_NOT_WORK
-	/* Suggested by TPO, doesn't work as usual */
+	/* Write TCON setting */
+	if (dsi_wait_hs_data_fifo(dev) < 0)
+		return;
+#if 0
+	/* Suggested by TPO, doesn't work */
 	REG_WRITE(0xb068, 0x110000b7);
 	REG_WRITE(0xb068, 0x00000044);
 #else
 	REG_WRITE(0xb068, 0x770000b7);
 	REG_WRITE(0xb068, 0x00000044);
 #endif
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_CTRL_FIFO_FULL) ==
-							HS_CTRL_FIFO_FULL);
+	if (dsi_wait_hs_ctrl_fifo(dev) < 0)
+		return;
 	REG_WRITE(0xb070, 0x00000529);
+}
 
-	// 0xB6, for new crap displays
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_DATA_FIFO_FULL) ==
-							HS_DATA_FIFO_FULL);
-	REG_WRITE(0xb068, 0x000a0ab6);
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_CTRL_FIFO_FULL) ==
-							HS_CTRL_FIFO_FULL);
-	REG_WRITE(0xb070, 0x00000329);
 
-	// 0xF2, for new crap displays
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_DATA_FIFO_FULL) ==
-							HS_DATA_FIFO_FULL);
-	REG_WRITE(0xb068, 0x081010f2);
-	REG_WRITE(0xb068, 0x4a070708);
-	REG_WRITE(0xb068, 0x000000c5);
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_CTRL_FIFO_FULL) ==
-							HS_CTRL_FIFO_FULL);
-	REG_WRITE(0xb070, 0x00000929);
+static void dsi_set_ptarget_state(struct drm_device *dev, int state)
+{
+	u32 pp_sts_reg;
 
-	// 0xF8, for new crap displays
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_DATA_FIFO_FULL) ==
-							HS_DATA_FIFO_FULL);
-	REG_WRITE(0xb068, 0x024003f8);
-	REG_WRITE(0xb068, 0x01030a04);
-	REG_WRITE(0xb068, 0x0e020220);
-	REG_WRITE(0xb068, 0x00000004);
+	DBG_TRACE("%d", state);
 
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_CTRL_FIFO_FULL) ==
-							HS_CTRL_FIFO_FULL);
-	REG_WRITE(0xb070, 0x00000d29);
+	if (state) {
+		REG_WRITE(PP_CONTROL, (REG_READ(PP_CONTROL) | POWER_TARGET_ON));
+		do {
+			pp_sts_reg = REG_READ(PP_STATUS);
+		} while ((pp_sts_reg & (PP_ON | PP_READY)) == PP_READY);
+	} else {
+		REG_WRITE(PP_CONTROL,
+				(REG_READ(PP_CONTROL) & ~POWER_TARGET_ON));
+		do {
+			pp_sts_reg = REG_READ(PP_STATUS);
+		} while (pp_sts_reg & PP_ON);
+	}
+}
 
-	// 0xE2, for new crap displays
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_DATA_FIFO_FULL) ==
-							HS_DATA_FIFO_FULL);
-	REG_WRITE(0xb068, 0x398fc3e2);
-	REG_WRITE(0xb068, 0x0000916f);
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_CTRL_FIFO_FULL) ==
-							HS_CTRL_FIFO_FULL);
-	REG_WRITE(0xb070, 0x00000629);
 
-#ifdef DOES_NOT_WORK
-	/* Suggested by TPO, doesn't work as usual */
-	// 0xE3, for new crap displays
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_DATA_FIFO_FULL) ==
-							HS_DATA_FIFO_FULL);
-	REG_WRITE(0xb068, 0x20f684e3);
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_CTRL_FIFO_FULL) ==
-							HS_CTRL_FIFO_FULL);
-	REG_WRITE(0xb070, 0x00000429);
+static void dsi_send_turn_on_packet(struct drm_device *dev)
+{
+	DBG_TRACE("");
 
-	msleep(50);
-#endif
+	REG_WRITE(DPI_CONTROL_REG, DPI_TURN_ON);
 
-	// 0xB0, for new crap displays
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_DATA_FIFO_FULL) ==
-							HS_DATA_FIFO_FULL);
-	REG_WRITE(0xb068, 0x000000b0);
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_CTRL_FIFO_FULL) ==
-							HS_CTRL_FIFO_FULL);
-	REG_WRITE(0xb070, 0x00000229);
+	/* Short delay to wait that display turns on */
+	msleep(10);
+}
 
-	// 0xF4, for new crap displays
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_DATA_FIFO_FULL) ==
-							HS_DATA_FIFO_FULL);
-	REG_WRITE(0xb068, 0x240242f4);
-	REG_WRITE(0xb068, 0x78ee2002);
-	REG_WRITE(0xb068, 0x2a071050);
-	REG_WRITE(0xb068, 0x507fee10);
-	REG_WRITE(0xb068, 0x10300710);
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_CTRL_FIFO_FULL) ==
-							HS_CTRL_FIFO_FULL);
-	REG_WRITE(0xb070, 0x00001429);
 
-	// 0xBA, for new crap displays
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_DATA_FIFO_FULL) ==
-							HS_DATA_FIFO_FULL);
-	REG_WRITE(0xb068, 0x19fe07ba);
-	REG_WRITE(0xb068, 0x101c0a31);
-	REG_WRITE(0xb068, 0x00000010);
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_CTRL_FIFO_FULL) ==
-							HS_CTRL_FIFO_FULL);
-	REG_WRITE(0xb070, 0x00000929);
+static void dsi_send_shutdown_packet(struct drm_device *dev)
+{
+	DBG_TRACE("");
 
-	// 0xBB, for new crap displays
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_DATA_FIFO_FULL) ==
-							HS_DATA_FIFO_FULL);
-	REG_WRITE(0xb068, 0x28ff07bb);
-	REG_WRITE(0xb068, 0x24280a31);
-	REG_WRITE(0xb068, 0x00000034);
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_CTRL_FIFO_FULL) ==
-							HS_CTRL_FIFO_FULL);
-	REG_WRITE(0xb070, 0x00000929);
+	REG_WRITE(DPI_CONTROL_REG, DPI_SHUT_DOWN);
+}
 
-	// 0xFB, for new crap displays
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_DATA_FIFO_FULL) ==
-							HS_DATA_FIFO_FULL);
-	REG_WRITE(0xb068, 0x535d05fb);
-	REG_WRITE(0xb068, 0x1b1a2130);
-	REG_WRITE(0xb068, 0x221e180e);
-	REG_WRITE(0xb068, 0x131d2120);
-	REG_WRITE(0xb068, 0x535d0508);
-	REG_WRITE(0xb068, 0x1c1a2131);
-	REG_WRITE(0xb068, 0x231f160d);
-	REG_WRITE(0xb068, 0x111b2220);
-	REG_WRITE(0xb068, 0x535c2008);
-	REG_WRITE(0xb068, 0x1f1d2433);
-	REG_WRITE(0xb068, 0x2c251a10);
-	REG_WRITE(0xb068, 0x2c34372d);
-	REG_WRITE(0xb068, 0x00000023);
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_CTRL_FIFO_FULL) ==
-							HS_CTRL_FIFO_FULL);
-	REG_WRITE(0xb070, 0x00003129);
 
-	// 0xFA, for new crap displays
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_DATA_FIFO_FULL) ==
-							HS_DATA_FIFO_FULL);
-	REG_WRITE(0xb068, 0x525c0bfa);
-	REG_WRITE(0xb068, 0x1c1c232f);
-	REG_WRITE(0xb068, 0x2623190e);
-	REG_WRITE(0xb068, 0x18212625);
-	REG_WRITE(0xb068, 0x545d0d0e);
-	REG_WRITE(0xb068, 0x1e1d2333);
-	REG_WRITE(0xb068, 0x26231a10);
-	REG_WRITE(0xb068, 0x1a222725);
-	REG_WRITE(0xb068, 0x545d280f);
-	REG_WRITE(0xb068, 0x21202635);
-	REG_WRITE(0xb068, 0x31292013);
-	REG_WRITE(0xb068, 0x31393d33);
-	REG_WRITE(0xb068, 0x00000029);
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_CTRL_FIFO_FULL) ==
-							HS_CTRL_FIFO_FULL);
-	REG_WRITE(0xb070, 0x00003129);
-#endif
+static void dsi_set_pipe_plane_enable_state(struct drm_device *dev, int state)
+{
+	u32 temp_reg;
 
-	/* Set DM */
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_DATA_FIFO_FULL) ==
-							HS_DATA_FIFO_FULL);
-	REG_WRITE(0xb068, 0x000100f7);
-	while ((REG_READ(GEN_FIFO_STAT_REG) & HS_CTRL_FIFO_FULL) ==
-							HS_CTRL_FIFO_FULL);
-	REG_WRITE(0xb070, 0x00000329);
-}
+	DBG_TRACE("%d", state);
 
+	if (state) {
+		/* Enable pipe */
+		temp_reg = REG_READ(PIPEACONF);
+		temp_reg |= (PIPEACONF_ENABLE);
+		REG_WRITE(PIPEACONF, temp_reg);
+		temp_reg = REG_READ(PIPEACONF);
 
-static void panel_reset_on(void)
-{
-	struct ipc_pmic_reg_data tmp_reg = {0};
-#if DBG_PRINTS
-	printk("panel_reset_on\n");
-#endif /* DBG_PRINTS */
-	tmp_reg.ioc = 1;
-	tmp_reg.num_entries = 1;
-#ifdef AAVA_EV_0_5
-	tmp_reg.pmic_reg_data[0].register_address = 0xe6;
-	tmp_reg.pmic_reg_data[0].value = 0x01;
-#else /* CDK */
-	tmp_reg.pmic_reg_data[0].register_address = 0xf4;
-	if (ipc_pmic_register_read(&tmp_reg)) {
-		printk("panel_reset_on: failed to read pmic reg 0xf4!\n");
-		return;
-	}
-	tmp_reg.pmic_reg_data[0].value &= 0xbf;
-#endif /* AAVA_EV_0_5 */
-	if (ipc_pmic_register_write(&tmp_reg, TRUE)) {
-		printk("panel_reset_on: failed to write pmic reg 0xe6!\n");
-	}
-}
+		/* Wait for 20ms for the pipe enable to take effect. */
+		msleep(20);
 
+		/* Enable plane */
+		temp_reg = REG_READ(DSPACNTR);
+		temp_reg |= (DISPLAY_PLANE_ENABLE);
+		REG_WRITE(DSPACNTR, temp_reg);
+		temp_reg = REG_READ(DSPACNTR);
 
-static void panel_reset_off(void)
-{
-	struct ipc_pmic_reg_data tmp_reg = {0};
-#if DBG_PRINTS
-	printk("panel_reset_off\n");
-#endif /* DBG_PRINTS */
-	tmp_reg.ioc = 1;
-	tmp_reg.num_entries = 1;
-#ifdef AAVA_EV_0_5
-	tmp_reg.pmic_reg_data[0].register_address = 0xe6;
-	tmp_reg.pmic_reg_data[0].value = 0x09;
-#else /* CDK */
-	tmp_reg.pmic_reg_data[0].register_address = 0xf4;
-	if (ipc_pmic_register_read(&tmp_reg)) {
-		printk("panel_reset_off: failed to read pmic reg 0xf4!\n");
-		return;
-	}
-	tmp_reg.pmic_reg_data[0].value |= 0x40;
-#endif /* AAVA_EV_0_5 */
-	if (ipc_pmic_register_write(&tmp_reg, TRUE)) {
-		printk("panel_reset_off: failed to write pmic reg 0xe6!\n");
+		/* Flush plane change by read/write/read of BASE reg */
+		temp_reg = REG_READ(MRST_DSPABASE);
+		REG_WRITE(MRST_DSPABASE, temp_reg);
+		temp_reg = REG_READ(MRST_DSPABASE);
+
+		/* Wait for 20ms for the plane enable to take effect. */
+		msleep(20);
+	} else {
+		/* Disable plane */
+		temp_reg = REG_READ(DSPACNTR);
+		temp_reg &= ~(DISPLAY_PLANE_ENABLE);
+		REG_WRITE(DSPACNTR, temp_reg);
+		temp_reg = REG_READ(DSPACNTR);
+
+		/* Flush plane change by read/write/read of BASE reg */
+		temp_reg = REG_READ(MRST_DSPABASE);
+		REG_WRITE(MRST_DSPABASE, temp_reg);
+		temp_reg = REG_READ(MRST_DSPABASE);
+
+		/* Wait for 20ms for the plane disable to take effect. */
+		msleep(20);
+
+		/* Disable pipe */
+		temp_reg = REG_READ(PIPEACONF);
+		temp_reg &= ~(PIPEACONF_ENABLE);
+		REG_WRITE(PIPEACONF, temp_reg);
+		temp_reg = REG_READ(PIPEACONF);
+
+		/* Wait for 20ms for the pipe disable to take effect. */
+		msleep(20);
 	}
 }
 
 
-static void panel_reset(void)
+static void dsi_set_device_ready_state(struct drm_device *dev, int state)
 {
-#if DBG_PRINTS
-	printk("panel_reset\n");
-#endif /* DBG_PRINTS */
-
-	panel_reset_on();
-	msleep(20);
-	panel_reset_off();
-	msleep(20);
+	DBG_TRACE("%d", state);
+
+	if (state)
+		REG_WRITE(DEVICE_READY_REG, 0x00000001);
+	else
+		REG_WRITE(DEVICE_READY_REG, 0x00000000);
 }
 
 
-static void backlight_state(bool on)
+static void dsi_configure_mipi_block(struct drm_device *dev)
 {
-	struct ipc_pmic_reg_data tmp_reg;
+	DRM_DRIVER_PRIVATE_T *dev_priv = dev->dev_private;
+	u32 color_format = (RGB_888_FMT << FMT_DPI_POS);
+	u32 res = 0;
 
-#if DBG_PRINTS
-	printk("backlight_state\n");
-#endif /* DBG_PRINTS */
+	DBG_TRACE("");
 
-	tmp_reg.ioc = 1;
-	tmp_reg.num_entries = 2;
-	tmp_reg.pmic_reg_data[0].register_address = 0x2a;
-	tmp_reg.pmic_reg_data[1].register_address = 0x28;
+	/* MIPI clock ratio 1:1 */
+	//REG_WRITE(MIPI_CONTROL_REG, 0x00000018);
+	//REG_WRITE(0xb080, 0x0b061a02);
 
-	if( on ) {
-#if DBG_PRINTS
-		printk("backlight_state: ON\n");
-#endif /* DBG_PRINTS */
-		tmp_reg.pmic_reg_data[0].value = 0xaa;
-#ifdef AAVA_EV_0_5
-		tmp_reg.pmic_reg_data[1].value = 0x30;
-#else /* CDK */
-		tmp_reg.pmic_reg_data[1].value = 0x60;
-#endif /* AAVA_EV_0_5 */
-	} else {
-#if DBG_PRINTS
-		printk("backlight_state: OFF\n");
-#endif /* DBG_PRINTS */
-		tmp_reg.pmic_reg_data[0].value = 0x00;
-		tmp_reg.pmic_reg_data[1].value = 0x00;
-	}
+	/* MIPI clock ratio 2:1 */
+	//REG_WRITE(MIPI_CONTROL_REG, 0x00000019);
+	//REG_WRITE(0xb080, 0x3f1f1c04);
 
-	if (ipc_pmic_register_write(&tmp_reg, TRUE)) {
-		printk("backlight_state: failed to write pmic regs 0x2a and 0x28!\n");
-	}
-}
+	/* MIPI clock ratio 3:1 */
+	//REG_WRITE(MIPI_CONTROL_REG, 0x0000001a);
+	//REG_WRITE(0xb080, 0x091f7f08);
 
-#ifdef AAVA_BACKLIGHT_HACK
-static void bl_work_handler(struct work_struct *work)
-{
-	backlight_state(true);
+	/* MIPI clock ratio 4:1 */
+	REG_WRITE(MIPI_CONTROL_REG, (0x00000018 | mipi_clock));
+	REG_WRITE(0xb080, dphy_reg);
+
+	/* Enable all interrupts */
+	REG_WRITE(INTR_EN_REG, 0xffffffff);
+
+	REG_WRITE(TURN_AROUND_TIMEOUT_REG, 0x0000000A);
+	REG_WRITE(DEVICE_RESET_REG, 0x000000ff);
+	REG_WRITE(INIT_COUNT_REG, 0x00000fff);
+	REG_WRITE(HS_TX_TIMEOUT_REG, 0x90000);
+	REG_WRITE(LP_RX_TIMEOUT_REG, 0xffff);
+	REG_WRITE(HIGH_LOW_SWITCH_COUNT_REG, 0x46);
+	REG_WRITE(EOT_DISABLE_REG, 0x00000000);
+	REG_WRITE(LP_BYTECLK_REG, 0x00000004);
+
+	REG_WRITE(VIDEO_FMT_REG, dev_priv->videoModeFormat);
+
+	REG_WRITE(DSI_FUNC_PRG_REG, (dev_priv->laneCount | color_format));
+
+	res = dev_priv->HactiveArea | (dev_priv->VactiveArea << RES_V_POS);
+	REG_WRITE(DPI_RESOLUTION_REG, res);
+
+	REG_WRITE(VERT_SYNC_PAD_COUNT_REG, dev_priv->VsyncWidth);
+	REG_WRITE(VERT_BACK_PORCH_COUNT_REG, dev_priv->VbackPorch);
+	REG_WRITE(VERT_FRONT_PORCH_COUNT_REG, dev_priv->VfrontPorch);
+
+	REG_WRITE(HORIZ_SYNC_PAD_COUNT_REG, dev_priv->HsyncWidth);
+	REG_WRITE(HORIZ_BACK_PORCH_COUNT_REG, dev_priv->HbackPorch);
+	REG_WRITE(HORIZ_FRONT_PORCH_COUNT_REG, dev_priv->HfrontPorch);
+	REG_WRITE(HORIZ_ACTIVE_AREA_COUNT_REG, MIPI_HACT);
+
+	/* Enable MIPI Port */
+	REG_WRITE(MIPI, MIPI_PORT_EN);
 }
-#endif /* AAVA_BACKLIGHT_HACK */
 
 
-/**
- * Sets the power state for the panel.
- */
-static void mrst_dsi_set_power(struct drm_device *dev,
-			       struct psb_intel_output *output, bool on)
+static void dsi_configure_down(struct drm_device *dev)
 {
 	DRM_DRIVER_PRIVATE_T *dev_priv = dev->dev_private;
-	u32 pp_status;
 
-#if DBG_PRINTS
-	printk("mrst_dsi_set_power\n");
-#endif /* DBG_PRINTS */
+	DBG_TRACE("");
 
-	/*
-	 * The DIS device must be ready before we can change power state.
-	 */
-	if (!dev_priv->dsi_device_ready)
-	{
-#if DBG_PRINTS
-		printk("mrst_dsi_set_power: !dev_priv->dsi_device_ready!\n");
-#endif /* DBG_PRINTS */
+	if (!dev_priv->dpi_panel_on) {
+		DBG_TRACE("already off");
 		return;
 	}
 
-	/*
-	 * We don't support dual DSI yet. May be in POR in the future.
+	/* Disable backlight */
+	dsi_set_backlight_state(0);
+
+	/* Disable pipe and plane */
+	dsi_set_pipe_plane_enable_state(dev, 0);
+
+	/* Disable PTARGET */
+	dsi_set_ptarget_state(dev, 0);
+
+	/* Send shutdown command, can only be sent if
+	 * interface is configured
 	 */
-	if (dev_priv->dual_display)
-	{
-#if DBG_PRINTS
-		printk("mrst_dsi_set_power: dev_priv->dual_display!\n");
-#endif /* DBG_PRINTS */
-	return;
-	}
+	if (dev_priv->dsi_device_ready)
+		dsi_send_shutdown_packet(dev);
 
-	if (!ospm_power_using_hw_begin(OSPM_DISPLAY_ISLAND,
-				       OSPM_UHB_FORCE_POWER_ON))
-		return;
+	/* Clear device ready state */
+	dsi_set_device_ready_state(dev, 0);
 
-	if (on) {
-#if DBG_PRINTS
-		printk("mrst_dsi_set_power: on\n");
-#endif /* DBG_PRINTS */
-		if (dev_priv->dpi && !dev_priv->dpi_panel_on) {
-#if DBG_PRINTS
-			printk("mrst_dsi_set_power: dpi\n");
-#endif /* DBG_PRINTS */
-			REG_WRITE(DPI_CONTROL_REG, DPI_TURN_ON);
-			REG_WRITE(PP_CONTROL,
-				  (REG_READ(PP_CONTROL) | POWER_TARGET_ON));
-			do {
-				pp_status = REG_READ(PP_STATUS);
-			} while ((pp_status & (PP_ON | PP_READY)) == PP_READY);
-
-			/* Run TPO display specific initialisations */
-// MiKo TBD, this delay may need to be tuned
-			msleep(50);
-			panel_init(dev);
-
-			/* Set backlights on */
-			backlight_state( true );
-			dev_priv->dpi_panel_on = true;
-		}
-	} else {
-#if DBG_PRINTS
-		printk("mrst_dsi_set_power: off\n");
-#endif /* DBG_PRINTS */
-		if (dev_priv->dpi && dev_priv->dpi_panel_on) {
-#if DBG_PRINTS
-			printk("mrst_dsi_set_power: dpi\n");
-#endif /* DBG_PRINTS */
-			/* Set backlights off */
-			backlight_state( false );
-
-// MiKo TBD, something clever could be done here to save power, for example:
-// -Set display to sleep mode, or
-// -Set display to HW reset, or
-// -Shutdown the voltages to display
-
-			REG_WRITE(PP_CONTROL,
-				  (REG_READ(PP_CONTROL) & ~POWER_TARGET_ON));
-			do {
-				pp_status = REG_READ(PP_STATUS);
-			} while (pp_status & PP_ON);
-
-			REG_WRITE(DPI_CONTROL_REG, DPI_SHUT_DOWN);
-
-			dev_priv->dpi_panel_on = false;
-		}
-	}
+	/* Set panel to reset */
+	dsi_set_panel_reset_state(1);
 
-	ospm_power_using_hw_end(OSPM_DISPLAY_ISLAND);
+	dev_priv->dpi_panel_on = false;
 }
 
 
-static void mrst_dsi_dpms(struct drm_encoder *encoder, int mode)
+static void dsi_configure_up(struct drm_device *dev)
 {
-	struct drm_device *dev = encoder->dev;
-	struct psb_intel_output *output = enc_to_psb_intel_output(encoder);
+	DRM_DRIVER_PRIVATE_T *dev_priv = dev->dev_private;
 
-#if DBG_PRINTS
-	printk("mrst_dsi_dpms\n");
-#endif /* DBG_PRINTS */
+	DBG_TRACE("");
 
-	if (mode == DRM_MODE_DPMS_ON)
-		mrst_dsi_set_power(dev, output, true);
-	else
-		mrst_dsi_set_power(dev, output, false);
+	if (dev_priv->dpi_panel_on) {
+		DBG_TRACE("already on");
+		return;
+	}
+
+	/* Get panel from reset */
+	dsi_set_panel_reset_state(0);
+
+	/* Set device ready state */
+	dsi_set_device_ready_state(dev, 1);
+
+	/* Send turn on command */
+	dsi_send_turn_on_packet(dev);
+
+	/* Enable PTARGET */
+	dsi_set_ptarget_state(dev, 1);
+
+	/* Initialize panel */
+	dsi_init_panel(dev);
+
+	/* Enable plane and pipe */
+	dsi_set_pipe_plane_enable_state(dev, 1);
+
+	/* Enable backlight */
+	dsi_set_backlight_state(1);
+
+	dev_priv->dpi_panel_on = true;
 }
 
 
-static void mrst_dsi_save(struct drm_connector *connector)
+static void dsi_init_drv_ic(struct drm_device *dev)
 {
-#if DBG_PRINTS
-	printk("mrst_dsi_save\n");
-#endif /* DBG_PRINTS */
-	// MiKo TBD
+	DBG_TRACE("");
 }
 
 
-static void mrst_dsi_restore(struct drm_connector *connector)
+static void dsi_schedule_work(struct drm_device *dev)
 {
-#if DBG_PRINTS
-	printk("mrst_dsi_restore\n");
-#endif /* DBG_PRINTS */
-	// MiKo TBD
+	DRM_DRIVER_PRIVATE_T *dev_priv = dev->dev_private;
+
+	DBG_TRACE("");
+
+	schedule_work(&dev_priv->dsi_work);
 }
 
 
-static void mrst_dsi_prepare(struct drm_encoder *encoder)
+static void dsi_work_handler(struct work_struct *work)
 {
-	struct drm_device *dev = encoder->dev;
-	struct psb_intel_output *output = enc_to_psb_intel_output(encoder);
+	struct drm_psb_private *dev_priv = container_of(work,
+					struct drm_psb_private, dsi_work);
 
-#if DBG_PRINTS
-	printk("mrst_dsi_prepare\n");
-#endif /* DBG_PRINTS */
+	DBG_TRACE("");
 
-	if (!ospm_power_using_hw_begin(OSPM_DISPLAY_ISLAND,
-				       OSPM_UHB_FORCE_POWER_ON))
-		return;
+	dsi_configure_up(dev_priv->dev);
+}
 
-	mrst_dsi_set_power(dev, output, false);
 
-	ospm_power_using_hw_end(OSPM_DISPLAY_ISLAND);
+static void dsi_init_mipi_config(DRM_DRIVER_PRIVATE_T *dev_priv)
+{
+	DBG_TRACE("");
+
+	/* Fixed values for TPO display */
+	dev_priv->pixelClock = 33264;
+	dev_priv->HsyncWidth = MIPI_HSPAD;
+	dev_priv->HbackPorch = MIPI_HBP;
+	dev_priv->HfrontPorch = MIPI_HFP;
+	dev_priv->HactiveArea = HSIZE;
+	dev_priv->VsyncWidth = MIPI_VSPAD;
+	dev_priv->VbackPorch = MIPI_VBP;
+	dev_priv->VfrontPorch = MIPI_VFP;
+	dev_priv->VactiveArea = VSIZE;
+	dev_priv->bpp = 24;
+
+	/* video mode */
+	dev_priv->dpi = true;
+
+	/* Set this true since firmware or kboot has enabled display */
+	dev_priv->dpi_panel_on = true;
+
+	/* Set this false to ensure proper initial configuration */
+	dev_priv->dsi_device_ready = false;
+
+	/* 2 lanes */
+	dev_priv->laneCount = MIPI_LANES;
+
+	/* Burst mode */
+	dev_priv->videoModeFormat = BURST_MODE;
+
+	dev_priv->init_drvIC = dsi_init_drv_ic;
+ 	dev_priv->dsi_prePowerState = dsi_configure_down;
+	dev_priv->dsi_postPowerState = dsi_schedule_work;
 }
 
 
-static void mrst_dsi_commit(struct drm_encoder *encoder)
+static struct drm_display_mode *dsi_get_fixed_display_mode(void)
 {
-	struct drm_device *dev = encoder->dev;
-	struct psb_intel_output *output = enc_to_psb_intel_output(encoder);
+	struct drm_display_mode *mode;
+
+	DBG_TRACE("");
+
+	mode = kzalloc(sizeof(*mode), GFP_KERNEL);
+	if (!mode) {
+		DBG_ERR("kzalloc failed\n");
+		return NULL;
+	}
+
+	/* Fixed mode for TPO display
+           Note: Using defined values for easier match with ITP scripts
+                 and adding 1 since psb_intel_display.c decreases by 1
+	*/
+	mode->hdisplay = (DISP_HPIX + 1);
+	mode->vdisplay = (DISP_VPIX + 1);
+	mode->hsync_start = (DISP_HSYNC_START + 1);
+	mode->hsync_end = (DISP_HSYNC_END + 1);
+	mode->htotal = (DISP_HBLANK_END + 1);
+	mode->vsync_start = (DISP_VSYNC_START + 1);
+	mode->vsync_end = (DISP_VSYNC_END + 1);
+	mode->vtotal = (DISP_VBLANK_END + 1);
+	mode->clock = 33264;
 
-#if DBG_PRINTS
-	printk("mrst_dsi_commit\n");
-#endif /* DBG_PRINTS */
+	drm_mode_set_name(mode);
+	drm_mode_set_crtcinfo(mode, 0);
 
-	mrst_dsi_set_power(dev, output, true);
+	return mode;
 }
 
 
-static void mrst_dsi_mode_set(struct drm_encoder *encoder,
-			      struct drm_display_mode *mode,
-			      struct drm_display_mode *adjusted_mode)
+/* Encoder funcs */
+static void dsi_encoder_mode_set(struct drm_encoder *encoder,
+					struct drm_display_mode *mode,
+					struct drm_display_mode *adjusted_mode)
 {
 	struct drm_device *dev = encoder->dev;
 	DRM_DRIVER_PRIVATE_T *dev_priv = dev->dev_private;
-	u32 SupportedFormat = 0;
-	u32 resolution = 0;
-	uint64_t curValue = DRM_MODE_SCALE_FULLSCREEN;
+	uint64_t scale_mode = DRM_MODE_SCALE_FULLSCREEN;
 
-#if DBG_PRINTS
-	printk("mrst_dsi_mode_set\n");
-#endif /* DBG_PRINTS */
+	DBG_TRACE("");
 
 	if (!ospm_power_using_hw_begin(OSPM_DISPLAY_ISLAND,
-				       OSPM_UHB_FORCE_POWER_ON))
+						OSPM_UHB_FORCE_POWER_ON)) {
+		DBG_ERR("OSPM_DISPLAY_ISLAND OSPM_UHB_FORCE_POWER_ON failed");
 		return;
+	}
 
 	/* Sleep to ensure that the graphics engine is ready
 	 * since its mode_set is called before ours
 	 */
 	msleep(100);
 
-	switch (dev_priv->bpp)
-	{
-	case 24:
-		SupportedFormat = RGB_888_FMT;
-		break;
-	default:
-		printk("mrst_dsi_mode_set, invalid bpp!\n");
-		break;
-	}
-
-	if (dev_priv->dpi) {
+	/* Only one mode is supported,
+	 * so configure only if not yet configured
+	 */
+	if (!dev_priv->dsi_device_ready) {
 		drm_connector_property_get_value(
 					&enc_to_psb_intel_output(encoder)->base,
 					dev->mode_config.scaling_mode_property,
-					&curValue);
-		if (curValue == DRM_MODE_SCALE_CENTER) {
+					&scale_mode);
+		if (scale_mode == DRM_MODE_SCALE_CENTER)
 			REG_WRITE(PFIT_CONTROL, 0);
-		} else if (curValue == DRM_MODE_SCALE_FULLSCREEN) {
+		else if (scale_mode == DRM_MODE_SCALE_FULLSCREEN)
 			REG_WRITE(PFIT_CONTROL, PFIT_ENABLE);
-		} else {
-			printk("mrst_dsi_mode_set, scaling not supported!\n");
+		else {
+			DBG_ERR("unsupported scaling");
 			REG_WRITE(PFIT_CONTROL, 0);
 		}
+		dsi_configure_mipi_block(dev);
+		dev_priv->dsi_device_ready = true;
+	}
 
+	ospm_power_using_hw_end(OSPM_DISPLAY_ISLAND);
+}
 
-		/* MIPI clock ratio 1:1 */
-		//REG_WRITE(MIPI_CONTROL_REG, 0x00000018);
-		//REG_WRITE(0xb080, 0x0b061a02);
-
-		/* MIPI clock ratio 2:1 */
-		//REG_WRITE(MIPI_CONTROL_REG, 0x00000019);
-		//REG_WRITE(0xb080, 0x3f1f1c04);
 
-		/* MIPI clock ratio 3:1 */
-		//REG_WRITE(MIPI_CONTROL_REG, 0x0000001a);
-		//REG_WRITE(0xb080, 0x091f7f08);
+static void dsi_encoder_prepare(struct drm_encoder *encoder)
+{
+	struct drm_device *dev = encoder->dev;
 
-		/* MIPI clock ratio 4:1 */
-		REG_WRITE(MIPI_CONTROL_REG, (0x00000018 | mipi_clock));
-		REG_WRITE(0xb080, dphy_reg);
+	DBG_TRACE("");
 
-		/* Enable all interrupts */
-		REG_WRITE(INTR_EN_REG, 0xffffffff);
+	if (!ospm_power_using_hw_begin(OSPM_DISPLAY_ISLAND,
+						OSPM_UHB_FORCE_POWER_ON)) {
+		DBG_ERR("OSPM_DISPLAY_ISLAND OSPM_UHB_FORCE_POWER_ON failed");
+		return;
+	}
 
-		REG_WRITE(TURN_AROUND_TIMEOUT_REG, 0x0000000A);
-		REG_WRITE(DEVICE_RESET_REG, 0x000000ff);
-		REG_WRITE(INIT_COUNT_REG, 0x00000fff);
-		REG_WRITE(HS_TX_TIMEOUT_REG, 0x90000);
-		REG_WRITE(LP_RX_TIMEOUT_REG, 0xffff);
-		REG_WRITE(HIGH_LOW_SWITCH_COUNT_REG, 0x46);
-		REG_WRITE(EOT_DISABLE_REG, 0x00000000);
-		REG_WRITE(LP_BYTECLK_REG, 0x00000004);
+	dsi_configure_down(dev);
 
-		REG_WRITE(VIDEO_FMT_REG, dev_priv->videoModeFormat);
+	ospm_power_using_hw_end(OSPM_DISPLAY_ISLAND);
+}
 
-		SupportedFormat <<= FMT_DPI_POS;
-		REG_WRITE(DSI_FUNC_PRG_REG,
-				(dev_priv->laneCount | SupportedFormat));
 
-		resolution = dev_priv->HactiveArea |
-				(dev_priv->VactiveArea << RES_V_POS);
-		REG_WRITE(DPI_RESOLUTION_REG, resolution);
+static void dsi_encoder_commit(struct drm_encoder *encoder)
+{
+	struct drm_device *dev = encoder->dev;
+	DRM_DRIVER_PRIVATE_T *dev_priv = dev->dev_private;
 
-		REG_WRITE(VERT_SYNC_PAD_COUNT_REG, dev_priv->VsyncWidth);
-		REG_WRITE(VERT_BACK_PORCH_COUNT_REG, dev_priv->VbackPorch);
-		REG_WRITE(VERT_FRONT_PORCH_COUNT_REG, dev_priv->VfrontPorch);
+	DBG_TRACE("");
 
-		REG_WRITE(HORIZ_SYNC_PAD_COUNT_REG, dev_priv->HsyncWidth);
-		REG_WRITE(HORIZ_BACK_PORCH_COUNT_REG, dev_priv->HbackPorch);
-		REG_WRITE(HORIZ_FRONT_PORCH_COUNT_REG, dev_priv->HfrontPorch);
-		REG_WRITE(HORIZ_ACTIVE_AREA_COUNT_REG, MIPI_HACT);
+	if (!ospm_power_using_hw_begin(OSPM_DISPLAY_ISLAND,
+						OSPM_UHB_FORCE_POWER_ON)) {
+		DBG_ERR("OSPM_DISPLAY_ISLAND OSPM_UHB_FORCE_POWER_ON failed");
+		return;
 	}
 
-	/* Enable MIPI Port */
-	REG_WRITE(MIPI, MIPI_PORT_EN);
+	if (!work_pending(&dev_priv->dsi_work))
+		dsi_configure_up(dev);
 
-	REG_WRITE(DEVICE_READY_REG, 0x00000001);
-	REG_WRITE(DPI_CONTROL_REG, 0x00000002); /* Turn On */
+	ospm_power_using_hw_end(OSPM_DISPLAY_ISLAND);
+}
 
-	dev_priv->dsi_device_ready = true;
 
-	/* Enable pipe */
-	REG_WRITE(PIPEACONF, dev_priv->pipeconf);
-	REG_READ(PIPEACONF);
+static void dsi_encoder_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct drm_device *dev = encoder->dev;
+	DRM_DRIVER_PRIVATE_T *dev_priv = dev->dev_private;
 
-	/* Wait for 20ms for the pipe enable to take effect. */
-	udelay(20000);
+	DBG_TRACE("%s", ((mode == DRM_MODE_DPMS_ON) ? "ON" : "OFF"));
 
-	/* Enable plane */
-	REG_WRITE(DSPACNTR, dev_priv->dspcntr);
+	if (!ospm_power_using_hw_begin(OSPM_DISPLAY_ISLAND,
+						OSPM_UHB_FORCE_POWER_ON)) {
+		DBG_ERR("OSPM_DISPLAY_ISLAND OSPM_UHB_FORCE_POWER_ON failed");
+		return;
+	}
 
-	/* Wait for 20ms for the plane enable to take effect. */
-	udelay(20000);
+	if (mode == DRM_MODE_DPMS_ON) {
+		if (!work_pending(&dev_priv->dsi_work))
+			dsi_configure_up(dev);
+	} else
+		dsi_configure_down(dev);
 
 	ospm_power_using_hw_end(OSPM_DISPLAY_ISLAND);
 }
 
 
-/**
- * Detect the MIPI connection.
- *
- * This always returns CONNECTOR_STATUS_CONNECTED.
- * This connector should only have
- * been set up if the MIPI was actually connected anyway.
- */
-static enum drm_connector_status mrst_dsi_detect(struct drm_connector
-						 *connector)
+/* Connector funcs */
+static enum drm_connector_status dsi_connector_detect(struct drm_connector
+								*connector)
 {
-#if DBG_PRINTS
-	printk("mrst_dsi_detect\n");
-#endif /* DBG_PRINTS */
+	DBG_TRACE("");
 	return connector_status_connected;
 }
 
 
-/**
- * Return the list of MIPI DDB modes if available.
- */
-static int mrst_dsi_get_modes(struct drm_connector *connector)
+static int dsi_connector_get_modes(struct drm_connector *connector)
 {
 	struct drm_device *dev = connector->dev;
-	struct psb_intel_output *psb_intel_output = to_psb_intel_output(connector);
-	struct psb_intel_mode_device *mode_dev = psb_intel_output->mode_dev;
+	struct psb_intel_output *psb_output = to_psb_intel_output(connector);
+	struct psb_intel_mode_device *mode_dev = psb_output->mode_dev;
+	struct drm_display_mode *mode;
+
+	DBG_TRACE("");
 
 	/* Didn't get an DDB, so
 	 * Set wide sync ranges so we get all modes
@@ -775,8 +701,7 @@ static int mrst_dsi_get_modes(struct drm_connector *connector)
 	connector->display_info.max_hfreq = 200;
 
 	if (mode_dev->panel_fixed_mode != NULL) {
-		struct drm_display_mode *mode =
-			drm_mode_duplicate(dev, mode_dev->panel_fixed_mode);
+		mode = drm_mode_duplicate(dev, mode_dev->panel_fixed_mode);
 		drm_mode_probed_add(connector, mode);
 		return 1;
 	}
@@ -784,180 +709,116 @@ static int mrst_dsi_get_modes(struct drm_connector *connector)
 }
 
 
-static const struct drm_encoder_helper_funcs mrst_dsi_helper_funcs = {
-	.dpms		= mrst_dsi_dpms,
+static void dsi_connector_save(struct drm_connector *connector)
+{
+	DBG_TRACE("");
+}
+
+
+static void dsi_connector_restore(struct drm_connector *connector)
+{
+	DBG_TRACE("");
+}
+
+
+static const struct drm_encoder_helper_funcs encoder_helper_funcs = {
+	.dpms		= dsi_encoder_dpms,
 	.mode_fixup	= psb_intel_lvds_mode_fixup,
-	.prepare	= mrst_dsi_prepare,
-	.mode_set	= mrst_dsi_mode_set,
-	.commit	= mrst_dsi_commit,
+	.prepare	= dsi_encoder_prepare,
+	.mode_set	= dsi_encoder_mode_set,
+	.commit		= dsi_encoder_commit,
 };
 
-
-static const struct drm_connector_helper_funcs
-	mrst_dsi_connector_helper_funcs = {
-		.get_modes	= mrst_dsi_get_modes,
-		.mode_valid	= psb_intel_lvds_mode_valid,
-		.best_encoder	= psb_intel_best_encoder,
+static const struct drm_connector_helper_funcs connector_helper_funcs = {
+	.get_modes	= dsi_connector_get_modes,
+	.mode_valid	= psb_intel_lvds_mode_valid,
+	.best_encoder	= psb_intel_best_encoder,
 };
 
 
-static const struct drm_connector_funcs mrst_dsi_connector_funcs = {
+static const struct drm_connector_funcs connector_funcs = {
 	.dpms		= drm_helper_connector_dpms,
-	.save		= mrst_dsi_save,
-	.restore	= mrst_dsi_restore,
-	.detect		= mrst_dsi_detect,
+	.save		= dsi_connector_save,
+	.restore	= dsi_connector_restore,
+	.detect		= dsi_connector_detect,
 	.fill_modes	= drm_helper_probe_single_connector_modes,
 	.set_property	= psb_intel_lvds_set_property,
 	.destroy	= psb_intel_lvds_destroy,
 };
 
 
-/** Returns the panel fixed mode from configuration. */
-struct drm_display_mode *mrst_dsi_get_configuration_mode(struct drm_device *dev)
-{
-	struct drm_display_mode *mode;
-
-	mode = kzalloc(sizeof(*mode), GFP_KERNEL);
-	if (!mode)
-		return NULL;
-
-	/* MiKo, fixed mode for TPO display
-           Note: Using defined values for easier match with ITP scripts
-                 and adding 1 since psb_intel_display.c decreases by 1
-	*/
-	mode->hdisplay = (DISP_HPIX + 1);
-	mode->vdisplay = (DISP_VPIX + 1);
-	mode->hsync_start = (DISP_HSYNC_START + 1);
-	mode->hsync_end = (DISP_HSYNC_END + 1);
-	mode->htotal = (DISP_HBLANK_END + 1);
-	mode->vsync_start = (DISP_VSYNC_START + 1);
-	mode->vsync_end = (DISP_VSYNC_END + 1);
-	mode->vtotal = (DISP_VBLANK_END + 1);
-	mode->clock = 33264;
-
-	drm_mode_set_name(mode);
-	drm_mode_set_crtcinfo(mode, 0);
-
-	return mode;
-}
-
-
-/* ************************************************************************* *\
-FUNCTION: mrst_mipi_settings_init
-																`
-DESCRIPTION:
-
-\* ************************************************************************* */
-static bool mrst_mipi_settings_init(DRM_DRIVER_PRIVATE_T *dev_priv)
-{
-	/* MiKo, fixed values for TPO display */
-	dev_priv->pixelClock = 33264;
-	dev_priv->HsyncWidth = MIPI_HSPAD;
-	dev_priv->HbackPorch = MIPI_HBP;
-	dev_priv->HfrontPorch = MIPI_HFP;
-	dev_priv->HactiveArea = HSIZE;
-	dev_priv->VsyncWidth = MIPI_VSPAD;
-	dev_priv->VbackPorch = MIPI_VBP;
-	dev_priv->VfrontPorch = MIPI_VFP;
-	dev_priv->VactiveArea = VSIZE;
-	dev_priv->bpp = 24;
-
-	/* video mode */
-	dev_priv->dpi = true;
-
-	/* MiKo, set these true by default to ensure that first mode set is done
-		 cleanly
-	*/
-	dev_priv->dpi_panel_on = true;
-	dev_priv->dsi_device_ready = true;
-
-	/* 2 lanes */
-	dev_priv->laneCount = MIPI_LANES;
-
-	/* Burst mode */
-	dev_priv->videoModeFormat = BURST_MODE;
-
-	return true;
-}
-
-
-/**
- * mrst_dsi_init - setup MIPI connectors on this device
- * @dev: drm device
- *
- * Create the connector, try to figure out what
- * modes we can display on the MIPI panel (if present).
- */
 void mrst_dsi_init(struct drm_device *dev,
-		    struct psb_intel_mode_device *mode_dev)
+					struct psb_intel_mode_device *mode_dev)
 {
 	DRM_DRIVER_PRIVATE_T *dev_priv = dev->dev_private;
-	struct psb_intel_output *psb_intel_output;
+	struct psb_intel_output *psb_output;
 	struct drm_connector *connector;
 	struct drm_encoder *encoder;
 
-#if DBG_PRINTS
-	printk("mrst_dsi_init\n");
-#endif /* DBG_PRINTS */
+	DBG_TRACE("");
 
-	psb_intel_output = kzalloc(sizeof(struct psb_intel_output), GFP_KERNEL);
-	if (!psb_intel_output)
+	psb_output = kzalloc(sizeof(struct psb_intel_output), GFP_KERNEL);
+	if (!psb_output) {
+		DBG_ERR("kzalloc failed\n");
 		return;
-
-	panel_reset();
+	}
 
 #ifdef AAVA_BACKLIGHT_HACK
 	schedule_delayed_work(&bl_work, 2*HZ);
 #endif /* AAVA_BACKLIGHT_HACK */
 
-	psb_intel_output->mode_dev = mode_dev;
-	connector = &psb_intel_output->base;
-	encoder = &psb_intel_output->enc;
-	drm_connector_init(dev,
-			   &psb_intel_output->base,
-			   &mrst_dsi_connector_funcs,
-			   DRM_MODE_CONNECTOR_MIPI);
-
-	drm_encoder_init(dev,
-			 &psb_intel_output->enc,
-			 &psb_intel_lvds_enc_funcs,
-			 DRM_MODE_ENCODER_MIPI);
-
-	drm_mode_connector_attach_encoder(&psb_intel_output->base,
-					  &psb_intel_output->enc);
-	psb_intel_output->type = INTEL_OUTPUT_MIPI;
-
-	drm_encoder_helper_add(encoder, &mrst_dsi_helper_funcs);
-	drm_connector_helper_add(connector, &mrst_dsi_connector_helper_funcs);
+	psb_output->mode_dev = mode_dev;
+	connector = &psb_output->base;
+	encoder = &psb_output->enc;
+	drm_connector_init(dev, &psb_output->base, &connector_funcs,
+						DRM_MODE_CONNECTOR_MIPI);
+
+	drm_encoder_init(dev, &psb_output->enc, &psb_intel_lvds_enc_funcs,
+							DRM_MODE_ENCODER_MIPI);
+
+	drm_mode_connector_attach_encoder(&psb_output->base, &psb_output->enc);
+	psb_output->type = INTEL_OUTPUT_MIPI;
+
+	drm_encoder_helper_add(encoder, &encoder_helper_funcs);
+	drm_connector_helper_add(connector, &connector_helper_funcs);
 	connector->display_info.subpixel_order = SubPixelHorizontalRGB;
 	connector->interlace_allowed = false;
 	connector->doublescan_allowed = false;
 
 	drm_connector_attach_property(connector,
-				      dev->mode_config.scaling_mode_property,
-				      DRM_MODE_SCALE_FULLSCREEN);
-	drm_connector_attach_property(connector,
-				      dev_priv->backlight_property,
-				      BRIGHTNESS_MAX_LEVEL);
+					dev->mode_config.scaling_mode_property,
+						DRM_MODE_SCALE_FULLSCREEN);
+	drm_connector_attach_property(connector, dev_priv->backlight_property,
+							BRIGHTNESS_MAX_LEVEL);
 
-	if (!mrst_mipi_settings_init(dev_priv))
-		printk("Can't initialize MIPI settings\n");
+	mode_dev->panel_wants_dither = false;
+
+	dsi_init_mipi_config(dev_priv);
 
 	/* No config phase */
 	dev_priv->config_phase = false;
 
 	/* Get the fixed mode */
-	mode_dev->panel_fixed_mode = mrst_dsi_get_configuration_mode(dev);
-	if (mode_dev->panel_fixed_mode) {
+	mode_dev->panel_fixed_mode = dsi_get_fixed_display_mode();
+	if (mode_dev->panel_fixed_mode)
 		mode_dev->panel_fixed_mode->type |= DRM_MODE_TYPE_PREFERRED;
-	} else {
-		printk("Found no modes for MIPI!\n");
+	else {
+		DBG_ERR("Fixed mode not available!\n");
 		goto failed_find;
 	}
+
+	/* Set this true since we enable/disable plane and pipe */
+	dev_priv->dsi_plane_pipe_control = true;
+
+	drm_sysfs_connector_add(connector);
+
+	/* Initialize work queue */
+	INIT_WORK(&dev_priv->dsi_work, dsi_work_handler);
+
 // Temporary access from sysfs begin
-	orig_encoder = encoder;
+	test_dev = dev;
 // Temporary access from sysfs end
-	drm_sysfs_connector_add(connector);
+
 	return;
 
 failed_find:
@@ -966,31 +827,140 @@ failed_find:
 	kfree(connector);
 }
 
+
 // Temporary access from sysfs begin
-static struct class_attribute miko_class_attrs[] = {
-	__ATTR(dphy, 0644, NULL, dphy_store),
-	__ATTR(clock, 0644, NULL, clock_store),
-	__ATTR(apply, 0200, NULL, apply_settings),
+static ssize_t dsi_run_test(struct class *class, const char *buf, size_t len)
+{
+	struct drm_device *dev = test_dev;
+	ssize_t	status;
+	long test_id;
+	int i;
+
+	status = strict_strtoul(buf, 0, &test_id);
+
+	DBG_TRACE("test_id %li", test_id);
+
+	switch(test_id) {
+	case 1:
+		/* BL on */
+		dsi_set_backlight_state(1);
+		break;
+	case 2:
+		/* BL off */
+		dsi_set_backlight_state(0);
+		break;
+	case 3:
+		/* Reset off */
+		dsi_set_panel_reset_state(0);
+		break;
+	case 4:
+		/* Reset on */
+		dsi_set_panel_reset_state(1);
+		break;
+	case 5:
+		/* Set device ready state */
+		dsi_set_device_ready_state(dev, 1);
+		break;
+	case 6:
+		/* Clear device ready state */
+		dsi_set_device_ready_state(dev, 0);
+		break;
+	case 7:
+		/* Send turn on command */
+		dsi_send_turn_on_packet(dev);
+		break;
+	case 8:
+		/* Send shutdown command */
+		dsi_send_shutdown_packet(dev);
+		break;
+	case 9:
+		/* Enable PTARGET */
+		dsi_set_ptarget_state(dev, 1);
+		break;
+	case 10:
+		/* Disable PTARGET */
+		dsi_set_ptarget_state(dev, 0);
+		break;
+	case 11:
+		/* Initialize panel */
+		dsi_init_panel(dev);
+		break;
+	case 12:
+		/* Enable plane and pipe */
+		dsi_set_pipe_plane_enable_state(dev, 1);
+		break;
+	case 13:
+		/* Disable plane and pipe */
+		dsi_set_pipe_plane_enable_state(dev, 0);
+		break;
+	case 14:
+		/* configure up */
+		dsi_configure_up(dev);
+		break;
+	case 15:
+		/* configure down */
+		dsi_configure_down(dev);
+		break;
+	case 16:
+		/* Draw pixels */
+		for (i = 0 ; i < (864*40) ; i++) {
+			if (dsi_wait_hs_data_fifo(dev) < 0)
+				break;
+			if (i == 0)
+			        REG_WRITE(0xb068, 0x0f0f0f2c);
+			else
+			        REG_WRITE(0xb068, 0x0f0f0f3c);
+			if (dsi_wait_hs_ctrl_fifo(dev) < 0)
+				break;
+			REG_WRITE(0xb070, 0x00000429);
+		}
+	case 17:
+		/* Sleep out command */
+		if (dsi_wait_hs_data_fifo(dev) < 0)
+			break;
+		REG_WRITE(0xb068, 0x00000011);
+		if (dsi_wait_hs_ctrl_fifo(dev) < 0)
+			break;
+		REG_WRITE(0xb070, 0x00000129);
+		break;
+	case 18:
+		/* Display on command */
+		if (dsi_wait_hs_data_fifo(dev) < 0)
+			break;
+		REG_WRITE(0xb068, 0x00000029);
+		if (dsi_wait_hs_ctrl_fifo(dev) < 0)
+			break;
+		REG_WRITE(0xb070, 0x00000129);
+		break;
+	default:
+		break;
+	}
+	return len;
+}
+
+
+
+static struct class_attribute tpo_class_attrs[] = {
+	__ATTR(test, 0200, NULL, dsi_run_test),
 	__ATTR_NULL,
 };
 
-static struct class miko_class = {
-	.name =		"miko",
+static struct class tpo_class = {
+	.name =		"tpo",
 	.owner =	THIS_MODULE,
 
-	.class_attrs =	miko_class_attrs,
+	.class_attrs =	tpo_class_attrs,
 };
 
-static int __init miko_sysfs_init(void)
+static int __init tpo_sysfs_init(void)
 {
-	int		status;
+	int status;
 
-	status = class_register(&miko_class);
+	status = class_register(&tpo_class);
 	if (status < 0)
 		return status;
 
 	return status;
 }
-postcore_initcall(miko_sysfs_init);
+postcore_initcall(tpo_sysfs_init);
 // Temporary access from sysfs end
-
diff --git a/drivers/gpu/drm/mrst/pvr/services4/3rdparty/linux_framebuffer_mrst/mrstlfb_displayclass.c b/drivers/gpu/drm/mrst/pvr/services4/3rdparty/linux_framebuffer_mrst/mrstlfb_displayclass.c
index adca7e2..7831183 100644
--- a/drivers/gpu/drm/mrst/pvr/services4/3rdparty/linux_framebuffer_mrst/mrstlfb_displayclass.c
+++ b/drivers/gpu/drm/mrst/pvr/services4/3rdparty/linux_framebuffer_mrst/mrstlfb_displayclass.c
@@ -1977,6 +1977,9 @@ PVRSRV_ERROR MRSTLFBPrePowerState(IMG_HANDLE 		 hDevHandle,
 	    (eNewPowerState == PVRSRV_DEV_POWER_STATE_ON))
 		return PVRSRV_OK;
 
+	if (!dev_priv->iLVDS_enable && dev_priv->dsi_prePowerState != NULL)
+		dev_priv->dsi_prePowerState(dev);
+
 	save_display_registers(dev);
 
 	if (dev_priv->iLVDS_enable) {
@@ -1999,14 +2002,15 @@ PVRSRV_ERROR MRSTLFBPrePowerState(IMG_HANDLE 		 hDevHandle,
 		/*turn off PLLs*/
 		PSB_WVDC32(0, MRST_DPLL_A);
 	} else {
-		PSB_WVDC32(DPI_SHUT_DOWN, DPI_CONTROL_REG);
-		PSB_WVDC32(0x0, PIPEACONF);
-		PSB_WVDC32(0x2faf0000, BLC_PWM_CTL);
-		while (REG_READ(0x70008) & 0x40000000);
-		while ((PSB_RVDC32(GEN_FIFO_STAT_REG) & DPI_FIFO_EMPTY)
+		if (dev_priv->dsi_prePowerState == NULL) {
+			PSB_WVDC32(DPI_SHUT_DOWN, DPI_CONTROL_REG);
+			PSB_WVDC32(0x0, PIPEACONF);
+			PSB_WVDC32(0x2faf0000, BLC_PWM_CTL);
+			while (REG_READ(0x70008) & 0x40000000);
+			while ((PSB_RVDC32(GEN_FIFO_STAT_REG) & DPI_FIFO_EMPTY)
 							!= DPI_FIFO_EMPTY);
-		PSB_WVDC32(0, DEVICE_READY_REG);
-
+			PSB_WVDC32(0, DEVICE_READY_REG);
+		}
 		/* turn off mipi panel power */
 		ret = lnw_ipc_single_cmd(IPC_MSG_PANEL_ON_OFF, IPC_CMD_PANEL_OFF, 0, 0);
 		if (ret)
@@ -2052,5 +2056,8 @@ PVRSRV_ERROR MRSTLFBPostPowerState(IMG_HANDLE 		  hDevHandle,
 
 	restore_display_registers(dev);
 
+	if (!dev_priv->iLVDS_enable && dev_priv->dsi_postPowerState != NULL)
+		dev_priv->dsi_postPowerState(dev);
+
 	return PVRSRV_OK;
 }
-- 
1.6.2.5

