gma500: resync with Medfield progress

Signed-off-by: Alan Cox <alan@linux.intel.com>
Signed-off-by: Greg Kroah-Hartman <gregkh@suse.de>

17 files changed, 917 insertions, 924 deletions

diff --git a/drivers/staging/gma500/mdfld_dsi_dbi.c b/drivers/staging/gma500/mdfld_dsi_dbi.c
index 1421f497d0a..06424f945bc 100644
--- a/drivers/staging/gma500/mdfld_dsi_dbi.c
+++ b/drivers/staging/gma500/mdfld_dsi_dbi.c
@@ -39,29 +39,10 @@ extern int gfxrtdelay;
 int enter_dsr;
 struct mdfld_dsi_dbi_output *gdbi_output;
 extern bool gbgfxsuspended;
+extern int enable_gfx_rtpm;
 extern int gfxrtdelay;
 
-#ifdef CONFIG_GFX_RTPM
-static void psb_runtimepm_wq_handler(struct work_struct *work);
-DECLARE_DELAYED_WORK(rtpm_work, psb_runtimepm_wq_handler);
-
-void psb_runtimepm_wq_handler(struct work_struct *work)
-{
-	struct drm_psb_private *dev_priv =  gpDrmDevice->dev_private;
-
-	if (drm_psb_ospm && !enable_gfx_rtpm) {
-		pr_info("Enable GFX runtime_pm\n");
-		dev_priv->rpm_enabled = 1;
-		enable_gfx_rtpm = 1;
-
-		pm_runtime_enable(&gpDrmDevice->pdev->dev);
-		pm_runtime_set_active(&gpDrmDevice->pdev->dev);
-
-		pm_runtime_allow(&gpDrmDevice->pdev->dev);
-	}
-}
-#endif
-
+#define MDFLD_DSR_MAX_IDLE_COUNT	2
 
 /*
  * set refreshing area
@@ -80,8 +61,8 @@ int mdfld_dsi_dbi_update_area(struct mdfld_dsi_dbi_output *dbi_output,
 		return -EINVAL;
 	}
 
-	/*set column*/
-	cmd = set_column_address;
+	/* Set column */
+	cmd = DCS_SET_COLUMN_ADDRESS;
 	param[0] = x1 >> 8;
 	param[1] = x1;
 	param[2] = x2 >> 8;
@@ -98,8 +79,8 @@ int mdfld_dsi_dbi_update_area(struct mdfld_dsi_dbi_output *dbi_output,
 		goto err_out;
 	}
 
-	/*set page*/
-	cmd = set_page_addr;
+	/* Set page */
+	cmd = DCS_SET_PAGE_ADDRESS;
 	param[0] = y1 >> 8;
 	param[1] = y1;
 	param[2] = y2 >> 8;
@@ -139,56 +120,46 @@ int mdfld_dsi_dbi_update_power(struct mdfld_dsi_dbi_output *dbi_output,
 								int mode)
 {
 	struct drm_device *dev = dbi_output->dev;
-	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct mdfld_dsi_pkg_sender *sender =
 		mdfld_dsi_encoder_get_pkg_sender(&dbi_output->base);
 	u8 param = 0;
 	u32 err = 0;
 
-	if (!dev_priv->dispstatus && mode != DRM_MODE_DPMS_ON) {
-		dev_err(dev->dev, "%s: already OFF ignoring\n", __func__);
-		return 0;
-	}
-	if (dev_priv->dispstatus && mode == DRM_MODE_DPMS_ON) {
-		dev_err(dev->dev, "%s: already ON ignoring\n", __func__);
-		return 0;
-	}
-
 	if (!sender) {
 		WARN_ON(1);
 		return -EINVAL;
 	}
 
 	if (mode == DRM_MODE_DPMS_ON) {
-		/*exit sleep mode*/
+		/* Exit sleep mode */
 		err = mdfld_dsi_send_dcs(sender,
-					 exit_sleep_mode,
+					 DCS_EXIT_SLEEP_MODE,
 					 NULL,
 					 0,
 					 CMD_DATA_SRC_SYSTEM_MEM,
 					 MDFLD_DSI_QUEUE_PACKAGE);
 		if (err) {
 			dev_err(dev->dev, "DCS 0x%x sent failed\n",
-						exit_sleep_mode);
+						DCS_EXIT_SLEEP_MODE);
 			goto power_err;
 		}
 
-		/*set display on*/
+		/* Set display on */
 		err = mdfld_dsi_send_dcs(sender,
-					 set_display_on,
+					 DCS_SET_DISPLAY_ON,
 					 NULL,
 					 0,
 					 CMD_DATA_SRC_SYSTEM_MEM,
 					 MDFLD_DSI_QUEUE_PACKAGE);
 		if (err) {
 			dev_err(dev->dev, "DCS 0x%x sent failed\n",
-							set_display_on);
+							DCS_SET_DISPLAY_ON);
 			goto power_err;
 		}
 
 		/* set tear effect on */
 		err = mdfld_dsi_send_dcs(sender,
-					 set_tear_on,
+					 DCS_SET_TEAR_ON,
 					 &param,
 					 1,
 					 CMD_DATA_SRC_SYSTEM_MEM,
@@ -203,53 +174,53 @@ int mdfld_dsi_dbi_update_power(struct mdfld_dsi_dbi_output *dbi_output,
 		 * FIXME: remove this later
 		 */
 		err = mdfld_dsi_send_dcs(sender,
-					 write_mem_start,
+					 DCS_WRITE_MEM_START,
 					 NULL,
 					 0,
 					 CMD_DATA_SRC_PIPE,
 					 MDFLD_DSI_QUEUE_PACKAGE);
 		if (err) {
 			dev_err(dev->dev, "DCS 0x%x sent failed\n",
-							set_display_on);
+						DCS_WRITE_MEM_START);
 			goto power_err;
 		}
 	} else {
-		/*set tear effect off */
+		/* Set tear effect off */
 		err = mdfld_dsi_send_dcs(sender,
-					 set_tear_off,
+					 DCS_SET_TEAR_OFF,
 					 NULL,
 					 0,
 					 CMD_DATA_SRC_SYSTEM_MEM,
 					 MDFLD_DSI_QUEUE_PACKAGE);
 		if (err) {
 			dev_err(dev->dev, "DCS 0x%x sent failed\n",
-							set_tear_off);
+							DCS_SET_TEAR_OFF);
 			goto power_err;
 		}
 
-		/*set display off*/
+		/* Turn display off */
 		err = mdfld_dsi_send_dcs(sender,
-					 set_display_off,
+					 DCS_SET_DISPLAY_OFF,
 					 NULL,
 					 0,
 					 CMD_DATA_SRC_SYSTEM_MEM,
 					 MDFLD_DSI_QUEUE_PACKAGE);
 		if (err) {
 			dev_err(dev->dev, "DCS 0x%x sent failed\n",
-							set_display_off);
+						DCS_SET_DISPLAY_OFF);
 			goto power_err;
 		}
 
-		/*enter sleep mode*/
+		/* Now enter sleep mode */
 		err = mdfld_dsi_send_dcs(sender,
-					 enter_sleep_mode,
+					 DCS_ENTER_SLEEP_MODE,
 					 NULL,
 					 0,
 					 CMD_DATA_SRC_SYSTEM_MEM,
 					 MDFLD_DSI_QUEUE_PACKAGE);
 		if (err) {
 			dev_err(dev->dev, "DCS 0x%x sent failed\n",
-							enter_sleep_mode);
+							DCS_ENTER_SLEEP_MODE);
 			goto power_err;
 		}
 	}
@@ -283,7 +254,6 @@ int mdfld_dsi_dbi_send_dcs(struct mdfld_dsi_dbi_output *dbi_output,
 	return ret;
 }
 
-
 /*
  * Enter DSR
  */
@@ -299,11 +269,12 @@ void mdfld_dsi_dbi_enter_dsr(struct mdfld_dsi_dbi_output *dbi_output, int pipe)
 	u32 pipeconf_reg = PIPEACONF;
 	u32 dspcntr_reg = DSPACNTR;
 
-	dev_priv->is_in_idle = true;
-
 	if (!dbi_output)
 		return;
 
+	/* FIXME check if can go */
+	dev_priv->is_in_idle = true;
+
 	gdbi_output = dbi_output;
 	if ((dbi_output->mode_flags & MODE_SETTING_ON_GOING) ||
 		(psb_crtc && psb_crtc->mode_flags & MODE_SETTING_ON_GOING))
@@ -319,16 +290,17 @@ void mdfld_dsi_dbi_enter_dsr(struct mdfld_dsi_dbi_output *dbi_output, int pipe)
 		dev_err(dev->dev, "hw begin failed\n");
 		return;
 	}
-	/*disable te interrupts. */
+	/* Disable te interrupts */
 	mdfld_disable_te(dev, pipe);
 
-	/*disable plane*/
+	/* Disable plane */
 	reg_val = REG_READ(dspcntr_reg);
 	if (!(reg_val & DISPLAY_PLANE_ENABLE)) {
 		REG_WRITE(dspcntr_reg, reg_val & ~DISPLAY_PLANE_ENABLE);
 		REG_READ(dspcntr_reg);
 	}
-	/*disable pipe*/
+
+	/* Disable pipe */
 	reg_val = REG_READ(pipeconf_reg);
 	if (!(reg_val & DISPLAY_PLANE_ENABLE)) {
 		reg_val &= ~DISPLAY_PLANE_ENABLE;
@@ -338,7 +310,7 @@ void mdfld_dsi_dbi_enter_dsr(struct mdfld_dsi_dbi_output *dbi_output, int pipe)
 		mdfldWaitForPipeDisable(dev, pipe);
 	}
 
-	/*disable DPLL*/
+	/* Disable DPLL */
 	reg_val = REG_READ(dpll_reg);
 	if (!(reg_val & DPLL_VCO_ENABLE)) {
 		reg_val &= ~DPLL_VCO_ENABLE;
@@ -357,10 +329,9 @@ void mdfld_dsi_dbi_enter_dsr(struct mdfld_dsi_dbi_output *dbi_output, int pipe)
 
 #ifndef CONFIG_MDFLD_DSI_DPU
 static void mdfld_dbi_output_exit_dsr(struct mdfld_dsi_dbi_output *dbi_output,
-			int pipe, void *p_surfaceAddr, bool check_hw_on_only)
+			int pipe)
 {
 	struct drm_device *dev = dbi_output->dev;
-	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct drm_crtc *crtc = dbi_output->base.base.crtc;
 	struct psb_intel_crtc *psb_crtc = (crtc) ?
 					to_psb_intel_crtc(crtc) : NULL;
@@ -368,7 +339,6 @@ static void mdfld_dbi_output_exit_dsr(struct mdfld_dsi_dbi_output *dbi_output,
 	u32 dpll_reg = MRST_DPLL_A;
 	u32 pipeconf_reg = PIPEACONF;
 	u32 dspcntr_reg = DSPACNTR;
-	u32 dspsurf_reg = DSPASURF;
 	u32 reg_offset = 0;
 
 	/*if mode setting on-going, back off*/
@@ -380,24 +350,17 @@ static void mdfld_dbi_output_exit_dsr(struct mdfld_dsi_dbi_output *dbi_output,
 		dpll_reg = MRST_DPLL_A;
 		pipeconf_reg = PIPECCONF;
 		dspcntr_reg = DSPCCNTR;
-		dspsurf_reg = DSPCSURF;
 		reg_offset = MIPIC_REG_OFFSET;
 	}
 
-	if (check_hw_on_only) {
-		if (0/* FIXME!ospm_power_is_hw_on(_DISPLAY_ISLAND)*/) {
-			dev_err(dev->dev, "hw begin failed\n");
-			return;
-		}
-	} else if (!gma_power_begin(dev, true)) {
+	if (!gma_power_begin(dev, true)) {
 		dev_err(dev->dev, "hw begin failed\n");
 		return;
 	}
 
-	/*enable DPLL*/
+	/* Enable DPLL */
 	reg_val = REG_READ(dpll_reg);
 	if (!(reg_val & DPLL_VCO_ENABLE)) {
-
 		if (reg_val & MDFLD_PWR_GATE_EN) {
 			reg_val &= ~MDFLD_PWR_GATE_EN;
 			REG_WRITE(dpll_reg, reg_val);
@@ -415,7 +378,7 @@ static void mdfld_dbi_output_exit_dsr(struct mdfld_dsi_dbi_output *dbi_output,
 			cpu_relax();
 	}
 
-	/*enable pipe*/
+	/* Enable pipe */
 	reg_val = REG_READ(pipeconf_reg);
 	if (!(reg_val & PIPEACONF_ENABLE)) {
 		reg_val |= PIPEACONF_ENABLE;
@@ -425,7 +388,7 @@ static void mdfld_dbi_output_exit_dsr(struct mdfld_dsi_dbi_output *dbi_output,
 		mdfldWaitForPipeEnable(dev, pipe);
 	}
 
-	/*enable plane*/
+	/* Enable plane */
 	reg_val = REG_READ(dspcntr_reg);
 	if (!(reg_val & DISPLAY_PLANE_ENABLE)) {
 		reg_val |= DISPLAY_PLANE_ENABLE;
@@ -434,15 +397,9 @@ static void mdfld_dbi_output_exit_dsr(struct mdfld_dsi_dbi_output *dbi_output,
 		udelay(500);
 	}
 
-	/* update the surface base address. */
-	if (p_surfaceAddr)
-		REG_WRITE(dspsurf_reg, *((u32 *)p_surfaceAddr));
-
-	if (!check_hw_on_only)
-		gma_power_end(dev);
-
-	/*enable TE interrupt on this pipe*/
+	/* Enable TE interrupt on this pipe */
 	mdfld_enable_te(dev, pipe);
+	gma_power_end(dev);
 
 	/*clean IN_DSR flag*/
 	dbi_output->mode_flags &= ~MODE_SETTING_IN_DSR;
@@ -451,33 +408,33 @@ static void mdfld_dbi_output_exit_dsr(struct mdfld_dsi_dbi_output *dbi_output,
 /*
  * Exit from DSR
  */
-void mdfld_dsi_dbi_exit_dsr(struct drm_device *dev, u32 update_src,
-				void *p_surfaceAddr, bool check_hw_on_only)
+void mdfld_dsi_dbi_exit_dsr(struct drm_device *dev, u32 update_src)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct mdfld_dbi_dsr_info *dsr_info = dev_priv->dbi_dsr_info;
 	struct mdfld_dsi_dbi_output **dbi_output;
 	int i;
+	int pipe;
 
+	/* FIXME can go ? */
 	dev_priv->is_in_idle = false;
 	dbi_output = dsr_info->dbi_outputs;
 
 #ifdef CONFIG_PM_RUNTIME
 	 if (!enable_gfx_rtpm) {
 /*                pm_runtime_allow(&gpDrmDevice->pdev->dev); */
-/*		schedule_delayed_work(&rtpm_work, 120 * 1000); */
+/*		schedule_delayed_work(&rtpm_work, 30 * 1000);*/ /* FIXME: HZ ? */
 	}
 #endif
 
-	/*for each output, exit dsr*/
+	/* For each output, exit dsr */
 	for (i = 0; i < dsr_info->dbi_output_num; i++) {
-		/*if panel has been turned off, skip*/
-		if (!dbi_output[i]->dbi_panel_on)
+		/* If panel has been turned off, skip */
+		if (!dbi_output[i] || !dbi_output[i]->dbi_panel_on)
 			continue;
-		if (dbi_output[i]->mode_flags & MODE_SETTING_IN_DSR) {
-			enter_dsr = 0;
-			mdfld_dbi_output_exit_dsr(dbi_output[i], dbi_output[i]->channel_num ? 2 : 0, p_surfaceAddr, check_hw_on_only);
-		}
+		pipe = dbi_output[i]->channel_num ? 2 : 0;
+		enter_dsr = 0;
+		mdfld_dbi_output_exit_dsr(dbi_output[i], pipe);
 	}
 	dev_priv->dsr_fb_update |= update_src;
 }
@@ -496,7 +453,7 @@ static bool mdfld_dbi_is_in_dsr(struct drm_device *dev)
 	return true;
 }
 
-/* Perodically update dbi panel */
+/* Periodically update dbi panel */
 void mdfld_dbi_update_panel(struct drm_device *dev, int pipe)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
@@ -504,8 +461,8 @@ void mdfld_dbi_update_panel(struct drm_device *dev, int pipe)
 	struct mdfld_dsi_dbi_output **dbi_outputs;
 	struct mdfld_dsi_dbi_output *dbi_output;
 	int i;
-	int enter_dsr = 0;
-	u32 damage_mask = 0;
+	int can_enter_dsr = 0;
+	u32 damage_mask;
 
 	dbi_outputs = dsr_info->dbi_outputs;
 	dbi_output = pipe ? dbi_outputs[1] : dbi_outputs[0];
@@ -514,13 +471,13 @@ void mdfld_dbi_update_panel(struct drm_device *dev, int pipe)
 		return;
 
 	if (pipe == 0)
-		damage_mask = dev_priv->dsr_fb_update & (MDFLD_DSR_DAMAGE_MASK_0);
+		damage_mask = dev_priv->dsr_fb_update & MDFLD_DSR_DAMAGE_MASK_0;
 	else if (pipe == 2)
-		damage_mask = dev_priv->dsr_fb_update & (MDFLD_DSR_DAMAGE_MASK_2);
+		damage_mask = dev_priv->dsr_fb_update & MDFLD_DSR_DAMAGE_MASK_2;
 	else
 		return;
 
-	/*if FB is damaged and panel is on update on-panel FB*/
+	/* If FB is damaged and panel is on update on-panel FB */
 	if (damage_mask && dbi_output->dbi_panel_on) {
 		dbi_output->dsr_fb_update_done = false;
 
@@ -538,11 +495,24 @@ void mdfld_dbi_update_panel(struct drm_device *dev, int pipe)
 		dbi_output->dsr_idle_count++;
 	}
 
-	/*try to enter DSR*/
-	if (dbi_outputs[0]->dsr_idle_count > 1
-	    && dbi_outputs[1]->dsr_idle_count > 1) {
+	switch (dsr_info->dbi_output_num) {
+	case 1:
+		if (dbi_output->dsr_idle_count > MDFLD_DSR_MAX_IDLE_COUNT)
+			can_enter_dsr = 1;
+		break;
+	case 2:
+		if (dbi_outputs[0]->dsr_idle_count > MDFLD_DSR_MAX_IDLE_COUNT
+		   && dbi_outputs[1]->dsr_idle_count > MDFLD_DSR_MAX_IDLE_COUNT)
+			can_enter_dsr = 1;
+		break;
+	default:
+		DRM_ERROR("Wrong DBI output number\n");
+	}
+
+	/* Try to enter DSR */
+	if (can_enter_dsr) {
 		for (i = 0; i < dsr_info->dbi_output_num; i++) {
-			if (!mdfld_dbi_is_in_dsr(dev) &&
+			if (!mdfld_dbi_is_in_dsr(dev) && dbi_outputs[i] &&
 			   !(dbi_outputs[i]->mode_flags & MODE_SETTING_ON_GOING)) {
 				mdfld_dsi_dbi_enter_dsr(dbi_outputs[i],
 					dbi_outputs[i]->channel_num ? 2 : 0);
@@ -565,61 +535,6 @@ void mdfld_dbi_update_panel(struct drm_device *dev, int pipe)
 	}
 }
 
-/*timers for DSR*/
-static void mdfld_dsi_dbi_dsr_timer_func(unsigned long data)
-{
-	struct drm_device *dev = (struct drm_device *)data;
-	struct drm_psb_private *dev_priv = dev->dev_private;
-	struct mdfld_dbi_dsr_info *dsr_info = dev_priv->dbi_dsr_info;
-	struct timer_list *dsr_timer = &dsr_info->dsr_timer;
-	unsigned long flags;
-
-	mdfld_dbi_update_panel(dev, 0);
-
-	if (dsr_info->dsr_idle_count > 1)
-		return;
-
-	spin_lock_irqsave(&dsr_info->dsr_timer_lock, flags);
-	if (!timer_pending(dsr_timer)) {
-		dsr_timer->expires = jiffies + MDFLD_DSR_DELAY;
-		add_timer(dsr_timer);
-	}
-	spin_unlock_irqrestore(&dsr_info->dsr_timer_lock, flags);
-}
-
-static int mdfld_dsi_dbi_dsr_timer_init(struct drm_device *dev)
-{
-	struct drm_psb_private *dev_priv = dev->dev_private;
-	struct mdfld_dbi_dsr_info *dsr_info = dev_priv->dbi_dsr_info;
-	struct timer_list *dsr_timer = &dsr_info->dsr_timer;
-	unsigned long flags;
-
-	spin_lock_init(&dsr_info->dsr_timer_lock);
-	spin_lock_irqsave(&dsr_info->dsr_timer_lock, flags);
-
-	init_timer(dsr_timer);
-
-	dsr_timer->data = (unsigned long)dev;
-	dsr_timer->function = mdfld_dsi_dbi_dsr_timer_func;
-	dsr_timer->expires = jiffies + MDFLD_DSR_DELAY;
-
-	spin_unlock_irqrestore(&dsr_info->dsr_timer_lock, flags);
-	return 0;
-}
-
-void mdfld_dbi_dsr_timer_start(struct mdfld_dbi_dsr_info *dsr_info)
-{
-	struct timer_list *dsr_timer = &dsr_info->dsr_timer;
-	unsigned long flags;
-
-	spin_lock_irqsave(&dsr_info->dsr_timer_lock, flags);
-	if (!timer_pending(dsr_timer)) {
-		dsr_timer->expires = jiffies + MDFLD_DSR_DELAY;
-		add_timer(dsr_timer);
-	}
-	spin_unlock_irqrestore(&dsr_info->dsr_timer_lock, flags);
-}
-
 int mdfld_dbi_dsr_init(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
@@ -643,7 +558,6 @@ void mdfld_dbi_dsr_exit(struct drm_device *dev)
 	struct mdfld_dbi_dsr_info *dsr_info = dev_priv->dbi_dsr_info;
 
 	if (dsr_info) {
-		del_timer_sync(&dsr_info->dsr_timer);
 		kfree(dsr_info);
 		dev_priv->dbi_dsr_info = NULL;
 	}
@@ -660,13 +574,13 @@ void mdfld_dsi_controller_dbi_init(struct mdfld_dsi_config *dsi_config,
 
 	dev_dbg(dev->dev, "Init DBI interface on pipe %d...\n", pipe);
 
-	/*un-ready device*/
+	/* Un-ready device */
 	REG_WRITE((MIPIA_DEVICE_READY_REG + reg_offset), 0x00000000);
 
-	/*init dsi adapter before kicking off*/
+	/* Init dsi adapter before kicking off */
 	REG_WRITE((MIPIA_CONTROL_REG + reg_offset), 0x00000018);
 
-	/*TODO: figure out how to setup these registers*/
+	/* TODO: figure out how to setup these registers */
 	REG_WRITE((MIPIA_DPHY_PARAM_REG + reg_offset), 0x150c3408);
 	REG_WRITE((MIPIA_CLK_LANE_SWITCH_TIME_CNT_REG + reg_offset),
 							0x000a0014);
@@ -674,16 +588,16 @@ void mdfld_dsi_controller_dbi_init(struct mdfld_dsi_config *dsi_config,
 	REG_WRITE((MIPIA_DBI_FIFO_THROTTLE_REG + reg_offset), 0x00000001);
 	REG_WRITE((MIPIA_HS_LS_DBI_ENABLE_REG + reg_offset), 0x00000000);
 
-	/*enable all interrupts*/
+	/* Enable all interrupts */
 	REG_WRITE((MIPIA_INTR_EN_REG + reg_offset), 0xffffffff);
-	/*max value: 20 clock cycles of txclkesc*/
+	/* Max value: 20 clock cycles of txclkesc */
 	REG_WRITE((MIPIA_TURN_AROUND_TIMEOUT_REG + reg_offset), 0x0000001f);
-	/*min 21 txclkesc, max: ffffh*/
+	/* Min 21 txclkesc, max: ffffh */
 	REG_WRITE((MIPIA_DEVICE_RESET_TIMER_REG + reg_offset), 0x0000ffff);
-	/*min: 7d0 max: 4e20*/
+	/* Min: 7d0 max: 4e20 */
 	REG_WRITE((MIPIA_INIT_COUNT_REG + reg_offset), 0x00000fa0);
 
-	/*set up func_prg*/
+	/* Set up func_prg */
 	val |= lane_count;
 	val |= (dsi_config->channel_num << DSI_DBI_VIRT_CHANNEL_OFFSET);
 	val |= DSI_DBI_COLOR_FORMAT_OPTION2;
@@ -692,7 +606,7 @@ void mdfld_dsi_controller_dbi_init(struct mdfld_dsi_config *dsi_config,
 	REG_WRITE((MIPIA_HS_TX_TIMEOUT_REG + reg_offset), 0x3fffff);
 	REG_WRITE((MIPIA_LP_RX_TIMEOUT_REG + reg_offset), 0xffff);
 
-	/*de-assert dbi_stall when half of DBI FIFO is empty*/
+	/* De-assert dbi_stall when half of DBI FIFO is empty */
 	/* REG_WRITE((MIPIA_DBI_FIFO_THROTTLE_REG + reg_offset), 0x00000000); */
 
 	REG_WRITE((MIPIA_HIGH_LOW_SWITCH_COUNT_REG + reg_offset), 0x46);
@@ -718,42 +632,6 @@ static const struct drm_encoder_funcs mdfld_dsi_dbi_encoder_funcs = {
 
 #endif
 
-static int mdfld_dbi_panel_reset(struct mdfld_dsi_dbi_output *output)
-{
-	unsigned gpio;
-	int ret;
-
-	switch (output->channel_num) {
-	case 0:
-		gpio = 128;
-		break;
-	case 1:
-		gpio = 34;
-		break;
-	default:
-		pr_err("Invalid output\n");
-		return -EINVAL;
-	}
-
-	ret = gpio_request(gpio, "gfx");
-	if (ret) {
-		pr_err("gpio_rqueset failed\n");
-		return ret;
-	}
-
-	ret = gpio_direction_output(gpio, 1);
-	if (ret) {
-		pr_err("gpio_direction_output failed\n");
-		goto gpio_error;
-	}
-	gpio_get_value(128);
-gpio_error:
-	if (gpio_is_valid(gpio))
-		gpio_free(gpio);
-
-	return ret;
-}
-
 /*
  * Init DSI DBI encoder.
  * Allocate an mdfld_dsi_encoder and attach it to given @dsi_connector
@@ -776,13 +654,44 @@ struct mdfld_dsi_encoder *mdfld_dsi_dbi_init(struct drm_device *dev,
 #else
 	struct mdfld_dbi_dsr_info *dsr_info = dev_priv ? (dev_priv->dbi_dsr_info) : NULL;
 #endif
+	u32 data = 0;
+	int pipe;
 	int ret;
 
-	if (!pg || !dsi_connector) {
+	if (!pg || !dsi_connector || !p_funcs) {
 		WARN_ON(1);
 		return NULL;
 	}
 
+	dsi_config = mdfld_dsi_get_config(dsi_connector);
+	pipe = dsi_connector->pipe;
+
+	/*panel hard-reset*/
+	if (p_funcs->reset) {
+		ret = p_funcs->reset(pipe);
+		if (ret) {
+			DRM_ERROR("Panel %d hard-reset failed\n", pipe);
+			return NULL;
+		}
+	}
+	/* Panel drvIC init */
+	if (p_funcs->drv_ic_init)
+		p_funcs->drv_ic_init(dsi_config, pipe);
+
+	/* Panel power mode detect */
+	ret = mdfld_dsi_get_power_mode(dsi_config,
+				       &data,
+				       MDFLD_DSI_HS_TRANSMISSION);
+	if (ret) {
+		DRM_ERROR("Panel %d get power mode failed\n", pipe);
+		dsi_connector->status = connector_status_disconnected;
+	} else {
+		DRM_INFO("pipe %d power mode 0x%x\n", pipe, data);
+		dsi_connector->status = connector_status_connected;
+	}
+
+	/*TODO: get panel info from DDB*/
+
 	dbi_output = kzalloc(sizeof(struct mdfld_dsi_dbi_output), GFP_KERNEL);
 	if (!dbi_output) {
 		dev_err(dev->dev, "No memory\n");
@@ -802,23 +711,10 @@ struct mdfld_dsi_encoder *mdfld_dsi_dbi_init(struct drm_device *dev,
 
 	dbi_output->dev = dev;
 	dbi_output->p_funcs = p_funcs;
-
-	/*panel reset*/
-	ret = mdfld_dbi_panel_reset(dbi_output);
-	if (ret) {
-		dev_err(dev->dev, "reset panel error\n");
-		goto out_err1;
-	}
-
-	/*TODO: get panel info from DDB*/
-
-	/*get fixed mode*/
-	dsi_config = mdfld_dsi_get_config(dsi_connector);
 	fixed_mode = dsi_config->fixed_mode;
-
 	dbi_output->panel_fixed_mode = fixed_mode;
 
-	/*create drm encoder object*/
+	/* Create drm encoder object */
 	connector = &dsi_connector->base.base;
 	encoder = &dbi_output->base.base;
 	drm_encoder_init(dev,
@@ -827,10 +723,10 @@ struct mdfld_dsi_encoder *mdfld_dsi_dbi_init(struct drm_device *dev,
 			DRM_MODE_ENCODER_MIPI);
 	drm_encoder_helper_add(encoder, p_funcs->encoder_helper_funcs);
 
-	/*attach to given connector*/
+	/* Attach to given connector */
 	drm_mode_connector_attach_encoder(connector, encoder);
 
-	/*set possible crtcs and clones*/
+	/* Set possible CRTCs and clones */
 	if (dsi_connector->pipe) {
 		encoder->possible_crtcs = (1 << 2);
 		encoder->possible_clones = (1 << 1);
@@ -842,28 +738,31 @@ struct mdfld_dsi_encoder *mdfld_dsi_dbi_init(struct drm_device *dev,
 	dev_priv->dsr_fb_update = 0;
 	dev_priv->dsr_enable = false;
 	dev_priv->exit_idle = mdfld_dsi_dbi_exit_dsr;
-#if defined(CONFIG_MDFLD_DSI_DPU) || defined(CONFIG_MDFLD_DSI_DSR)
-	dev_priv->dsr_enable_config = false;
-#endif /*CONFIG_MDFLD_DSI_DSR*/
 
 	dbi_output->first_boot = true;
 	dbi_output->mode_flags = MODE_SETTING_IN_ENCODER;
 
 #ifdef CONFIG_MDFLD_DSI_DPU
-	/*add this output to dpu_info*/
-	if (dsi_connector->pipe == 0)
-		dpu_info->dbi_outputs[0] = dbi_output;
-	} else {
-		dpu_info->dbi_outputs[1] = dbi_output;
+	/* Add this output to dpu_info */
+	if (dsi_connector->status == connector_status_connected) {
+		if (dsi_connector->pipe == 0)
+			dpu_info->dbi_outputs[0] = dbi_output;
+		else
+			dpu_info->dbi_outputs[1] = dbi_output;
+
+		dpu_info->dbi_output_num++;
 	}
-	dpu_info->dbi_output_num++;
+
 #else /*CONFIG_MDFLD_DSI_DPU*/
-	/*add this output to dsr_info*/
-	if (dsi_connector->pipe == 0)
-		dsr_info->dbi_outputs[0] = dbi_output;
-	else
-		dsr_info->dbi_outputs[1] = dbi_output;
-	dsr_info->dbi_output_num++;
+	if (dsi_connector->status == connector_status_connected) {
+		/* Add this output to dsr_info */
+		if (dsi_connector->pipe == 0)
+			dsr_info->dbi_outputs[0] = dbi_output;
+		else
+			dsr_info->dbi_outputs[1] = dbi_output;
+
+		dsr_info->dbi_output_num++;
+	}
 #endif
 	return &dbi_output->base;
 out_err1:
diff --git a/drivers/staging/gma500/mdfld_dsi_dbi.h b/drivers/staging/gma500/mdfld_dsi_dbi.h
index a51e2f87518..a2ec6fd94ae 100644
--- a/drivers/staging/gma500/mdfld_dsi_dbi.h
+++ b/drivers/staging/gma500/mdfld_dsi_dbi.h
@@ -59,9 +59,6 @@ struct mdfld_dsi_dbi_output {
 	/* Backlight operations */
 
 	/* DSR timer */
-	spinlock_t dsr_timer_lock;
-	struct timer_list dsr_timer;
-	void(*dsi_timer_func)(unsigned long data);
 	u32 dsr_idle_count;
 	bool dsr_fb_update_done;
 
@@ -81,22 +78,11 @@ struct mdfld_dbi_dsr_info {
 	int dbi_output_num;
 	struct mdfld_dsi_dbi_output *dbi_outputs[2];
 
-	spinlock_t dsr_timer_lock;
-	struct timer_list dsr_timer;
 	u32 dsr_idle_count;
 };
 
 #define DBI_CB_TIMEOUT_COUNT	0xffff
 
-/* DCS commands */
-#define enter_sleep_mode	0x10
-#define exit_sleep_mode		0x11
-#define set_display_off		0x28
-#define	set_dispaly_on		0x29
-#define set_column_address	0x2a
-#define set_page_addr		0x2b
-#define write_mem_start		0x2c
-
 /* Offsets */
 #define CMD_MEM_ADDR_OFFSET	0
 
@@ -132,7 +118,7 @@ static inline int mdfld_dsi_dbi_cmd_sent(struct mdfld_dsi_dbi_output *dbi_output
 
 	/* Query the command execution status */
 	while (retry--)
-		if (!(REG_READ(MIPIA_CMD_ADD_REG + reg_offset) & (1 << 10)))
+		if (!(REG_READ(MIPIA_CMD_ADD_REG + reg_offset) & (1 << 0)))
 			break;
 
 	if (!retry) {
@@ -164,13 +150,11 @@ static inline int mdfld_dsi_dbi_cb_ready(struct mdfld_dsi_dbi_output *dbi_output
 
 extern void mdfld_dsi_dbi_output_init(struct drm_device *dev,
 			struct psb_intel_mode_device *mode_dev, int pipe);
-extern void mdfld_dsi_dbi_exit_dsr(struct drm_device *dev, u32 update_src,
-			void *p_surfaceAddr, bool check_hw_on_only);
+extern void mdfld_dsi_dbi_exit_dsr(struct drm_device *dev, u32 update_src);
 extern void mdfld_dsi_dbi_enter_dsr(struct mdfld_dsi_dbi_output *dbi_output,
 			int pipe);
 extern int mdfld_dbi_dsr_init(struct drm_device *dev);
 extern void mdfld_dbi_dsr_exit(struct drm_device *dev);
-extern void mdfld_dbi_dsr_timer_start(struct mdfld_dbi_dsr_info *dsr_info);
 extern struct mdfld_dsi_encoder *mdfld_dsi_dbi_init(struct drm_device *dev,
 			struct mdfld_dsi_connector *dsi_connector,
 			struct panel_funcs *p_funcs);
@@ -178,7 +162,6 @@ extern int mdfld_dsi_dbi_send_dcs(struct mdfld_dsi_dbi_output *dbi_output,
 			u8 dcs, u8 *param, u32 num, u8 data_src);
 extern int mdfld_dsi_dbi_update_area(struct mdfld_dsi_dbi_output *dbi_output,
 			u16 x1, u16 y1, u16 x2, u16 y2);
-extern void mdfld_dbi_dsr_timer_start(struct mdfld_dbi_dsr_info *dsr_info);
 extern int mdfld_dsi_dbi_update_power(struct mdfld_dsi_dbi_output *dbi_output,
 			int mode);
 extern void mdfld_dsi_controller_dbi_init(struct mdfld_dsi_config *dsi_config,
diff --git a/drivers/staging/gma500/mdfld_dsi_dbi_dpu.h b/drivers/staging/gma500/mdfld_dsi_dbi_dpu.h
index 4ca9682c47c..42367ed48c0 100644
--- a/drivers/staging/gma500/mdfld_dsi_dbi_dpu.h
+++ b/drivers/staging/gma500/mdfld_dsi_dbi_dpu.h
@@ -90,9 +90,6 @@ static inline int mdfld_dpu_region_extent(struct psb_drm_dpu_rect *origin,
 {
 	int x1, y1, x2, y2;
 
-	/* PSB_DEBUG_ENTRY("rect (%d, %d, %d, %d)\n",
-		rect->x, rect->y, rect->width, rect->height); */
-
 	x1 = origin->x + origin->width;
 	y1 = origin->y + origin->height;
 
diff --git a/drivers/staging/gma500/mdfld_dsi_dpi.c b/drivers/staging/gma500/mdfld_dsi_dpi.c
index fce3c203134..6e03a91e947 100644
--- a/drivers/staging/gma500/mdfld_dsi_dpi.c
+++ b/drivers/staging/gma500/mdfld_dsi_dpi.c
@@ -69,26 +69,6 @@ static void mdfld_wait_for_HS_CTRL_FIFO(struct drm_device *dev, u32 pipe)
 		dev_warn(dev->dev, "MIPI: HS CMD FIFO was never cleared!\n");
 }
 
-static void mdfld_wait_for_PIPEA_DISABLE(struct drm_device *dev, u32 pipe)
-{
-        u32 pipeconf_reg = PIPEACONF;
-        int timeout = 0;
-
-        if (pipe == 2)
-                pipeconf_reg = PIPECCONF;
-
-        udelay(500);
-
-        /* This will time out after approximately 2+ seconds */
-        while ((timeout < 20000) && (REG_READ(pipeconf_reg) & 0x40000000)) {
-                udelay(100);
-                timeout++;
-        }
-
-        if (timeout == 20000)
-                dev_warn(dev->dev, "MIPI: PIPE was not disabled!\n");
-}
-
 static void mdfld_wait_for_DPI_CTRL_FIFO(struct drm_device *dev, u32 pipe)
 {
 	u32 gen_fifo_stat_reg = MIPIA_GEN_FIFO_STAT_REG;
@@ -147,8 +127,6 @@ void mdfld_dsi_tpo_ic_init(struct mdfld_dsi_config *dsi_config, u32 pipe)
 	u32 gen_ctrl_reg = MIPIA_HS_GEN_CTRL_REG;
 	u32 gen_ctrl_val = GEN_LONG_WRITE;
 
-	dev_warn(dev->dev, "Enter mrst init TPO MIPI display.\n");
-
 	if (pipe == 2) {
 		gen_data_reg = HS_GEN_DATA_REG + MIPIC_REG_OFFSET; 
 		gen_ctrl_reg = HS_GEN_CTRL_REG + MIPIC_REG_OFFSET;
@@ -331,51 +309,8 @@ void mdfld_dsi_tpo_ic_init(struct mdfld_dsi_config *dsi_config, u32 pipe)
 	REG_WRITE(gen_ctrl_reg, gen_ctrl_val | (0x03 << WORD_COUNTS_POS));
 }
 
-/* ************************************************************************* *\
- * FUNCTION: mdfld_init_TMD_MIPI
- *
- * DESCRIPTION:  This function is called only by mrst_dsi_mode_set and
- *               restore_display_registers.  since this function does not
- *               acquire the mutex, it is important that the calling function
- *               does!
-\* ************************************************************************* */
-
-static u32 tmd_cmd_mcap_off[] = {0x000000b2};
-static u32 tmd_cmd_enable_lane_switch[] = {0x000101ef};
-static u32 tmd_cmd_set_lane_num[] = {0x006360ef};
-static u32 tmd_cmd_set_mode[] = {0x000000b3};
-static u32 tmd_cmd_set_sync_pulse_mode[] = {0x000961ef};
-static u32 tmd_cmd_set_video_mode[] = {0x00000153};
-static u32 tmd_cmd_enable_backlight[] = {0x00005ab4};//no auto_bl,need add in furtrue
-static u32 tmd_cmd_set_backlight_dimming[] = {0x00000ebd}; 
-
-void mdfld_dsi_tmd_drv_ic_init(struct mdfld_dsi_config *dsi_config, u32 pipe)
-{
-	struct mdfld_dsi_pkg_sender *sender = mdfld_dsi_get_pkg_sender(dsi_config);
-
-	if(!sender) {
-		WARN_ON(1);
-		return;
-	}
-
-	if(dsi_config->dvr_ic_inited)
-		return;
-
-	msleep(3);
-
-	mdfld_dsi_send_gen_long_lp(sender, tmd_cmd_mcap_off, 1, 0);
-	mdfld_dsi_send_gen_long_lp(sender, tmd_cmd_enable_lane_switch, 1, 0);
-	mdfld_dsi_send_gen_long_lp(sender, tmd_cmd_set_lane_num, 1, 0);
-	mdfld_dsi_send_gen_long_lp(sender, tmd_cmd_set_mode, 1, 0);
-	mdfld_dsi_send_gen_long_lp(sender, tmd_cmd_set_sync_pulse_mode, 1, 0);
-	/*TODO: set page and column here*/
-	mdfld_dsi_send_gen_long_lp(sender, tmd_cmd_set_video_mode, 1, 0);
-	mdfld_dsi_send_gen_long_lp(sender, tmd_cmd_enable_backlight, 1, 0);
-	mdfld_dsi_send_gen_long_lp(sender, tmd_cmd_set_backlight_dimming,1,0);
-	dsi_config->dvr_ic_inited = 1;
-}
-
-static u16 mdfld_dsi_dpi_to_byte_clock_count(int pixel_clock_count, int num_lane, int bpp)
+static u16 mdfld_dsi_dpi_to_byte_clock_count(int pixel_clock_count,
+						int num_lane, int bpp)
 {
 	return (u16)((pixel_clock_count * bpp) / (num_lane * 8)); 
 }
@@ -387,8 +322,8 @@ static u16 mdfld_dsi_dpi_to_byte_clock_count(int pixel_clock_count, int num_lane
  * use crtc mode values later 
  */
 int mdfld_dsi_dpi_timing_calculation(struct drm_display_mode *mode, 
-									struct mdfld_dsi_dpi_timing *dpi_timing,
-									int num_lane, int bpp)
+			struct mdfld_dsi_dpi_timing *dpi_timing,
+			int num_lane, int bpp)
 {
 	int pclk_hsync, pclk_hfp, pclk_hbp, pclk_hactive;
 	int pclk_vsync, pclk_vfp, pclk_vbp, pclk_vactive;
@@ -408,16 +343,6 @@ int mdfld_dsi_dpi_timing_calculation(struct drm_display_mode *mode,
 	pclk_vsync = mode->vsync_end - mode->vsync_start;
 	pclk_vbp = mode->vtotal - mode->vsync_end;
 
-#ifdef MIPI_DEBUG_LOG
-	printk(KERN_ALERT "[DISPLAY] %s: pclk_hactive = %d\n", __func__, pclk_hactive);
-	printk(KERN_ALERT "[DISPLAY] %s: pclk_hfp = %d\n", __func__, pclk_hfp);
-	printk(KERN_ALERT "[DISPLAY] %s: pclk_hsync = %d\n", __func__, pclk_hsync);
-	printk(KERN_ALERT "[DISPLAY] %s: pclk_hbp = %d\n", __func__, pclk_hbp);
-	printk(KERN_ALERT "[DISPLAY] %s: pclk_vactive = %d\n", __func__, pclk_vactive);
-	printk(KERN_ALERT "[DISPLAY] %s: pclk_vfp = %d\n", __func__, pclk_vfp);
-	printk(KERN_ALERT "[DISPLAY] %s: pclk_vsync = %d\n", __func__, pclk_vsync);
-	printk(KERN_ALERT "[DISPLAY] %s: pclk_vbp = %d\n", __func__, pclk_vbp);
-#endif
 	/*
 	 * byte clock counts were calculated by following formula
 	 * bclock_count = pclk_count * bpp / num_lane / 8
@@ -519,7 +444,6 @@ void mdfld_dsi_dpi_controller_init(struct mdfld_dsi_config *dsi_config, int pipe
 void mdfld_dsi_dpi_turn_on(struct mdfld_dsi_dpi_output *output, int pipe)
 {
 	struct drm_device *dev = output->dev;
-	/* struct drm_psb_private *dev_priv = dev->dev_private; */
 	u32 reg_offset = 0;
 	
 	if(output->panel_on) 
@@ -555,7 +479,6 @@ void mdfld_dsi_dpi_turn_on(struct mdfld_dsi_dpi_output *output, int pipe)
 static void mdfld_dsi_dpi_shut_down(struct mdfld_dsi_dpi_output *output, int pipe)
 {
 	struct drm_device *dev = output->dev;
-	/* struct drm_psb_private *dev_priv = dev->dev_private; */
 	u32 reg_offset = 0;
 	
 	/*if output is on, or mode setting didn't happen, ignore this*/
@@