diff --git a/drivers/gpu/drm/i915/intel_dsi.c b/drivers/gpu/drm/i915/intel_dsi.c
index fc0ef49..2af5d8b 100644
--- a/drivers/gpu/drm/i915/intel_dsi.c
+++ b/drivers/gpu/drm/i915/intel_dsi.c
@@ -50,6 +50,85 @@ static u16 pixels_from_txbyteclkhs(u16 clk_hs, int bpp, int lane_count,
 	return DIV_ROUND_UP((clk_hs * lane_count * 8 * 100),
 						(bpp * burst_mode_ratio));
 }
+static void intel_dsi_prepare(struct intel_encoder *intel_encoder,
+			      struct intel_crtc_state *pipe_config);
+static void intel_dsi_prepare_madhav(struct intel_encoder *intel_encoder,
+				     struct intel_crtc_state *pipe_config);
+
+void mipi_register_dump(struct drm_i915_private *dev_priv)
+{
+	u32 tmp;
+
+	DRM_ERROR("***** MIPI REGISTERS DUMP ****** \n");
+
+	tmp = I915_READ(MIPI_DEVICE_READY(PORT_A));
+	DRM_ERROR("MIPI DEVICE READY:0x%x\n", tmp);
+
+	tmp = I915_READ(MIPI_CTRL(PORT_A));
+	DRM_ERROR("MIPI_CTRL:0x%x\n", tmp);
+
+#if 0
+	tmp = I915_READ(PIPECONF(0));
+	DRM_ERROR("PIPECONF:0x%x\n", tmp);
+	tmp = I915_READ(MIPI_INTR_STAT(PORT_A));
+	DRM_ERROR("MIPI_INTR_STAT:0x%x\n", tmp);
+
+	tmp = I915_READ(MIPI_INTR_EN(PORT_A));
+	DRM_ERROR("MIPI_INTR_EN:0x%x\n", tmp);
+
+	tmp = I915_READ(MIPI_DPHY_PARAM(PORT_A));
+	DRM_ERROR("MIPI_DPHY_PARAM:0x%x\n", tmp);
+
+	tmp = I915_READ(MIPI_DPI_RESOLUTION(PORT_A));
+	DRM_ERROR("MIPI_DPI_RESOLUTION:0x%x\n", tmp);
+
+	tmp = I915_READ(MIPI_DSI_FUNC_PRG(PORT_A));
+	DRM_ERROR("MIPI_DSI_FUNC_PRG:0x%x\n", tmp);
+
+	tmp = I915_READ(MIPI_HS_TX_TIMEOUT(PORT_A));
+	DRM_ERROR("MIPI_HS_TX_TIMEOUT:0x%x\n", tmp);
+
+	tmp = I915_READ(MIPI_LP_RX_TIMEOUT(PORT_A));
+	DRM_ERROR("MIPI_LP_RX_TIMEOUT:0x%x\n", tmp);
+
+	tmp = I915_READ(MIPI_TURN_AROUND_TIMEOUT(PORT_A));
+	DRM_ERROR("MIPI_TURN_AROUND_TIMEOUT:0x%x\n", tmp);
+
+	tmp = I915_READ(MIPI_DEVICE_RESET_TIMER(PORT_A));
+	DRM_ERROR("MIPI_DEVICE_RESET_TIMER:0x%x\n", tmp);
+
+	tmp = I915_READ(MIPI_INIT_COUNT(PORT_A));
+	DRM_ERROR("MIPI_INIT_COUNT:0x%x\n", tmp);
+
+	tmp = I915_READ(MIPI_EOT_DISABLE(PORT_A));
+	DRM_ERROR("MIPI_EOT_DISABLE:0x%x\n", tmp);
+
+	tmp = I915_READ(MIPI_HIGH_LOW_SWITCH_COUNT(PORT_A));
+	DRM_ERROR("MIPI_HIGH_LOW_SWITCH_COUNT:0x%x\n", tmp);
+
+	tmp = I915_READ(MIPI_INIT_COUNT(PORT_A));
+	DRM_ERROR("MIPI_INIT_COUNT:0x%x\n", tmp);
+
+	tmp = I915_READ(MIPI_LP_BYTECLK(PORT_A));
+	DRM_ERROR("MIPI_LP_BYTECLK:0x%x\n", tmp);
+
+	tmp = I915_READ(MIPI_DBI_BW_CTRL(PORT_A));
+	DRM_ERROR("MIPI_DBI_BW_CTRL:0x%x\n", tmp);
+
+	tmp = I915_READ(MIPI_CLK_LANE_TIMING(PORT_A));
+	DRM_ERROR("MIPI_CLK_LANE_TIMING:0x%x\n", tmp);
+
+	tmp = I915_READ(MIPI_CLK_LANE_SWITCH_TIME_CNT(PORT_A));
+	DRM_ERROR("MIPI_CLK_LANE_SWITCH_TIME_CNT:0x%x\n", tmp);
+	tmp = I915_READ(MIPI_TLPX_TIME_COUNT(PORT_A));
+	DRM_ERROR("MIPI_TLPX_TIME_COUNT:0x%x\n", tmp);
+
+	tmp = I915_READ(MIPI_VIDEO_MODE_FORMAT(PORT_A));
+	DRM_ERROR("MIPI_VIDEO_MODE_FORMAT:0x%x\n", tmp);
+
+#endif
+	DRM_ERROR("*********************** \n");
+}
 
 enum mipi_dsi_pixel_format pixel_format_from_register_bits(u32 fmt)
 {
@@ -346,12 +425,15 @@ static bool intel_dsi_compute_config(struct intel_encoder *encoder,
 	return true;
 }
 
-static void glk_dsi_device_ready(struct intel_encoder *encoder)
+static void intel_dsi_prepare_glk(struct intel_encoder *intel_encoder, struct intel_crtc_state *pipe_config);
+
+static void glk_dsi_device_ready(struct intel_encoder *encoder, struct intel_crtc_state *pipe_config)
 {
 	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
 	struct intel_dsi *intel_dsi = enc_to_intel_dsi(&encoder->base);
 	enum port port;
 	u32 tmp, val;
+	bool ret = false;
 
 	/* Set the MIPI mode
 	 * If MIPI_Mode is off, then writing to LP_Wake bit is not reflecting.
@@ -370,7 +452,14 @@ static void glk_dsi_device_ready(struct intel_encoder *encoder)
 	/* Program LP Wake */
 	for_each_dsi_port(port, intel_dsi->ports) {
 		tmp = I915_READ(MIPI_CTRL(port));
-		tmp |= GLK_LP_WAKE;
+		if (!(I915_READ(MIPI_DEVICE_READY(port)) & DEVICE_READY)) {
+			DRM_ERROR("MADHAV:Device Ready 0\n");
+			tmp &= ~GLK_LP_WAKE;
+		}
+		else {
+			DRM_ERROR("MADHAV:Device Ready 1\n");
+			tmp |= GLK_LP_WAKE;
+		}
 		I915_WRITE(MIPI_CTRL(port), tmp);
 	}
 
@@ -380,8 +469,13 @@ static void glk_dsi_device_ready(struct intel_encoder *encoder)
 				MIPI_CTRL(port), GLK_MIPIIO_PORT_POWERED,
 				GLK_MIPIIO_PORT_POWERED, 20))
 			DRM_ERROR("MIPIO port is powergated\n");
+		ret |= !(I915_READ(MIPI_DEVICE_READY(port)) & DEVICE_READY);
 	}
 
+	if(ret)
+		intel_dsi_prepare_glk(encoder, pipe_config);
+		//intel_dsi_prepare(encoder, pipe_config);
+
 	/* Wait for MIPI PHY status bit to set */
 	for_each_dsi_port(port, intel_dsi->ports) {
 		if (intel_wait_for_register(dev_priv,
@@ -403,7 +497,7 @@ static void glk_dsi_device_ready(struct intel_encoder *encoder)
 			I915_WRITE(MIPI_DEVICE_READY(port), val);
 			usleep_range(10, 15);
 		}
-
+		else {
 		/* Enter ULPS */
 		val = I915_READ(MIPI_DEVICE_READY(port));
 		val &= ~ULPS_STATE_MASK;
@@ -430,6 +524,7 @@ static void glk_dsi_device_ready(struct intel_encoder *encoder)
 		tmp = I915_READ(MIPI_CTRL(port));
 		tmp &= ~GLK_LP_WAKE;
 		I915_WRITE(MIPI_CTRL(port), tmp);
+		}
 	}
 
 	/* Wait for Stop state */
@@ -515,7 +610,7 @@ static void vlv_dsi_device_ready(struct intel_encoder *encoder)
 	}
 }
 
-static void intel_dsi_device_ready(struct intel_encoder *encoder)
+static void intel_dsi_device_ready(struct intel_encoder *encoder, struct intel_crtc_state *pipe_config)
 {
 	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
 
@@ -524,7 +619,7 @@ static void intel_dsi_device_ready(struct intel_encoder *encoder)
 	else if (IS_BROXTON(dev_priv))
 		bxt_dsi_device_ready(encoder);
 	else if (IS_GEMINILAKE(dev_priv))
-		glk_dsi_device_ready(encoder);
+		glk_dsi_device_ready(encoder, pipe_config);
 }
 
 static void glk_dsi_enter_low_power_mode(struct intel_encoder *encoder)
@@ -710,8 +805,6 @@ static void intel_dsi_port_disable(struct intel_encoder *encoder)
 	}
 }
 
-static void intel_dsi_prepare(struct intel_encoder *intel_encoder,
-			      struct intel_crtc_state *pipe_config);
 static void intel_dsi_unprepare(struct intel_encoder *encoder);
 
 static void intel_dsi_msleep(struct intel_dsi *intel_dsi, int msec)
@@ -800,7 +893,7 @@ static void intel_dsi_pre_enable(struct intel_encoder *encoder,
 		I915_WRITE(DSPCLK_GATE_D, val);
 	}
 
-	intel_dsi_prepare(encoder, pipe_config);
+	//intel_dsi_prepare(encoder, pipe_config);
 
 	/* Power on, try both CRC pmic gpio and VBT */
 	if (intel_dsi->gpio_panel)
@@ -812,7 +905,12 @@ static void intel_dsi_pre_enable(struct intel_encoder *encoder,
 	intel_dsi_vbt_exec_sequence(intel_dsi, MIPI_SEQ_DEASSERT_RESET);
 
 	/* Put device in ready state (LP-11) */
-	intel_dsi_device_ready(encoder);
+	intel_dsi_device_ready(encoder, pipe_config);
+
+	intel_dsi_prepare(encoder, pipe_config);
+	//intel_dsi_prepare_madhav(encoder, pipe_config);
+
+	mipi_register_dump(dev_priv);
 
 	/* Send initialization commands in LP mode */
 	intel_dsi_vbt_exec_sequence(intel_dsi, MIPI_SEQ_INIT_OTP);
@@ -1346,6 +1444,46 @@ static u32 pixel_format_to_reg(enum mipi_dsi_pixel_format fmt)
 	}
 }
 
+static void intel_dsi_prepare_glk(struct intel_encoder *intel_encoder,
+				  struct intel_crtc_state *pipe_config)
+{
+	struct drm_encoder *encoder = &intel_encoder->base;
+	struct intel_dsi *intel_dsi = enc_to_intel_dsi(encoder);
+	struct drm_i915_private *dev_priv = to_i915(encoder->dev);
+	struct intel_crtc *intel_crtc = to_intel_crtc(pipe_config->base.crtc);
+	u32 val, tmp;
+	enum port port;
+	const struct drm_display_mode *adjusted_mode = &pipe_config->base.adjusted_mode;
+	u16 mode_hdisplay;
+
+	DRM_ERROR("MADHAV: Inside Prepare GLK\n");
+	for_each_dsi_port(port, intel_dsi->ports) {
+
+		I915_WRITE(MIPI_DPHY_PARAM(port), intel_dsi->dphy_reg);
+		I915_WRITE(MIPI_DPI_RESOLUTION(port), 0x4b00780);
+	}
+	set_dsi_timings(encoder, adjusted_mode);
+
+	val = intel_dsi->lane_count << DATA_LANES_PRG_REG_SHIFT;
+	val |= intel_dsi->channel << VID_MODE_CHANNEL_NUMBER_SHIFT;
+	val |= pixel_format_to_reg(intel_dsi->pixel_format);
+
+	for_each_dsi_port(port, intel_dsi->ports) {
+		I915_WRITE(MIPI_DSI_FUNC_PRG(port), val);
+		I915_WRITE(MIPI_INIT_COUNT(port), intel_dsi->init_count);
+		I915_WRITE(MIPI_TLPX_TIME_COUNT(port), intel_dsi->lp_byte_clk);
+
+		/* Shadow of DPHY reg */
+		I915_WRITE(MIPI_CLK_LANE_TIMING(port), intel_dsi->dphy_reg);
+		I915_WRITE(MIPI_VIDEO_MODE_FORMAT(port),
+				intel_dsi->video_frmt_cfg_bits |
+				intel_dsi->video_mode_format |
+				IP_TG_CONFIG |
+				RANDOM_DPI_DISPLAY_RESOLUTION);
+
+	}
+}
+
 static void intel_dsi_prepare(struct intel_encoder *intel_encoder,
 			      struct intel_crtc_state *pipe_config)
 {
@@ -1361,6 +1499,8 @@ static void intel_dsi_prepare(struct intel_encoder *intel_encoder,
 	u16 mode_hdisplay;
 
 	DRM_DEBUG_KMS("pipe %c\n", pipe_name(intel_crtc->pipe));
+	tmp = I915_READ(MIPI_DEVICE_READY(PORT_A));
+	DRM_ERROR("Prepare MIPI DEVICE READY:0x%x\n", tmp);
 
 	mode_hdisplay = adjusted_mode->crtc_hdisplay;
 
@@ -1408,7 +1548,205 @@ static void intel_dsi_prepare(struct intel_encoder *intel_encoder,
 	}
 
 	set_dsi_timings(encoder, adjusted_mode);
+	val = intel_dsi->lane_count << DATA_LANES_PRG_REG_SHIFT;
+	if (is_cmd_mode(intel_dsi)) {
+		val |= intel_dsi->channel << CMD_MODE_CHANNEL_NUMBER_SHIFT;
+		val |= CMD_MODE_DATA_WIDTH_8_BIT; /* XXX */
+	} else {
+		val |= intel_dsi->channel << VID_MODE_CHANNEL_NUMBER_SHIFT;
+		val |= pixel_format_to_reg(intel_dsi->pixel_format);
+	}
+
+	tmp = 0;
+	if (intel_dsi->eotp_pkt == 0)
+		tmp |= EOT_DISABLE;
+	if (intel_dsi->clock_stop)
+		tmp |= CLOCKSTOP;
+
+	if (IS_GEN9_LP(dev_priv)) {
+		tmp |= BXT_DPHY_DEFEATURE_EN;
+		if (!is_cmd_mode(intel_dsi))
+			tmp |= BXT_DEFEATURE_DPI_FIFO_CTR;
+	}
+
+	for_each_dsi_port(port, intel_dsi->ports) {
+		I915_WRITE(MIPI_DSI_FUNC_PRG(port), val);
+
+		/* timeouts for recovery. one frame IIUC. if counter expires,
+		 * EOT and stop state. */
+
+		/*
+		 * In burst mode, value greater than one DPI line Time in byte
+		 * clock (txbyteclkhs) To timeout this timer 1+ of the above
+		 * said value is recommended.
+		 *
+		 * In non-burst mode, Value greater than one DPI frame time in
+		 * byte clock(txbyteclkhs) To timeout this timer 1+ of the above
+		 * said value is recommended.
+		 *
+		 * In DBI only mode, value greater than one DBI frame time in
+		 * byte clock(txbyteclkhs) To timeout this timer 1+ of the above
+		 * said value is recommended.
+		 */
+
+		if (is_vid_mode(intel_dsi) &&
+			intel_dsi->video_mode_format == VIDEO_MODE_BURST) {
+			I915_WRITE(MIPI_HS_TX_TIMEOUT(port),
+				txbyteclkhs(adjusted_mode->crtc_htotal, bpp,
+					    intel_dsi->lane_count,
+					    intel_dsi->burst_mode_ratio) + 1);
+		} else {
+			I915_WRITE(MIPI_HS_TX_TIMEOUT(port),
+				txbyteclkhs(adjusted_mode->crtc_vtotal *
+					    adjusted_mode->crtc_htotal,
+					    bpp, intel_dsi->lane_count,
+					    intel_dsi->burst_mode_ratio) + 1);
+		}
+		I915_WRITE(MIPI_LP_RX_TIMEOUT(port), intel_dsi->lp_rx_timeout);
+		I915_WRITE(MIPI_TURN_AROUND_TIMEOUT(port),
+						intel_dsi->turn_arnd_val);
+		I915_WRITE(MIPI_DEVICE_RESET_TIMER(port),
+						intel_dsi->rst_timer_val);
+
+		/* dphy stuff */
 
+		/* in terms of low power clock */
+		I915_WRITE(MIPI_INIT_COUNT(port),
+				txclkesc(intel_dsi->escape_clk_div, 100));
+
+		if (IS_GEN9_LP(dev_priv) && (!intel_dsi->dual_link)) {
+			/*
+			 * BXT spec says write MIPI_INIT_COUNT for
+			 * both the ports, even if only one is
+			 * getting used. So write the other port
+			 * if not in dual link mode.
+			 */
+			I915_WRITE(MIPI_INIT_COUNT(port ==
+						PORT_A ? PORT_C : PORT_A),
+					intel_dsi->init_count);
+		}
+
+		/* recovery disables */
+		I915_WRITE(MIPI_EOT_DISABLE(port), tmp);
+
+		/* in terms of low power clock */
+		I915_WRITE(MIPI_INIT_COUNT(port), intel_dsi->init_count);
+
+		/* in terms of txbyteclkhs. actual high to low switch +
+		 * MIPI_STOP_STATE_STALL * MIPI_LP_BYTECLK.
+		 *
+		 * XXX: write MIPI_STOP_STATE_STALL?
+		 */
+		I915_WRITE(MIPI_HIGH_LOW_SWITCH_COUNT(port),
+						intel_dsi->hs_to_lp_count);
+
+		/* XXX: low power clock equivalence in terms of byte clock.
+		 * the number of byte clocks occupied in one low power clock.
+		 * based on txbyteclkhs and txclkesc.
+		 * txclkesc time / txbyteclk time * (105 + MIPI_STOP_STATE_STALL
+		 * ) / 105.???
+		 */
+		I915_WRITE(MIPI_LP_BYTECLK(port), intel_dsi->lp_byte_clk);
+
+		if (IS_GEMINILAKE(dev_priv)) {
+			I915_WRITE(MIPI_TLPX_TIME_COUNT(port),
+					intel_dsi->lp_byte_clk);
+			/* Shadow of DPHY reg */
+			I915_WRITE(MIPI_CLK_LANE_TIMING(port),
+					intel_dsi->dphy_reg);
+		}
+
+		/* the bw essential for transmitting 16 long packets containing
+		 * 252 bytes meant for dcs write memory command is programmed in
+		 * this register in terms of byte clocks. based on dsi transfer
+		 * rate and the number of lanes configured the time taken to
+		 * transmit 16 long packets in a dsi stream varies. */
+		I915_WRITE(MIPI_DBI_BW_CTRL(port), intel_dsi->bw_timer);
+
+		I915_WRITE(MIPI_CLK_LANE_SWITCH_TIME_CNT(port),
+		intel_dsi->clk_lp_to_hs_count << LP_HS_SSW_CNT_SHIFT |
+		intel_dsi->clk_hs_to_lp_count << HS_LP_PWR_SW_CNT_SHIFT);
+
+		if (is_vid_mode(intel_dsi))
+			/* Some panels might have resolution which is not a
+			 * multiple of 64 like 1366 x 768. Enable RANDOM
+			 * resolution support for such panels by default */
+			I915_WRITE(MIPI_VIDEO_MODE_FORMAT(port),
+				intel_dsi->video_frmt_cfg_bits |
+				intel_dsi->video_mode_format |
+				IP_TG_CONFIG |
+				RANDOM_DPI_DISPLAY_RESOLUTION);
+	}
+}
+
+
+static void intel_dsi_prepare_madhav(struct intel_encoder *intel_encoder,
+			      struct intel_crtc_state *pipe_config)
+{
+	struct drm_encoder *encoder = &intel_encoder->base;
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = to_i915(dev);
+	struct intel_crtc *intel_crtc = to_intel_crtc(pipe_config->base.crtc);
+	struct intel_dsi *intel_dsi = enc_to_intel_dsi(encoder);
+	const struct drm_display_mode *adjusted_mode = &pipe_config->base.adjusted_mode;
+	enum port port;
+	unsigned int bpp = mipi_dsi_pixel_format_to_bpp(intel_dsi->pixel_format);
+	u32 val, tmp;
+	u16 mode_hdisplay;
+
+	DRM_DEBUG_KMS("pipe %c\n", pipe_name(intel_crtc->pipe));
+	//tmp = I915_READ(MIPI_DEVICE_READY(PORT_A));
+	//tmp &= ~DEVICE_READY;
+	//I915_WRITE(MIPI_DEVICE_READY(PORT_A), tmp);
+	tmp = I915_READ(MIPI_DEVICE_READY(PORT_A));
+	DRM_ERROR("Prepare MIPI DEVICE READY:0x%x\n", tmp);
+
+	mode_hdisplay = adjusted_mode->crtc_hdisplay;
+
+	if (intel_dsi->dual_link) {
+		mode_hdisplay /= 2;
+		if (intel_dsi->dual_link == DSI_DUAL_LINK_FRONT_BACK)
+			mode_hdisplay += intel_dsi->pixel_overlap;
+	}
+
+	for_each_dsi_port(port, intel_dsi->ports) {
+		if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
+			/*
+			 * escape clock divider, 20MHz, shared for A and C.
+			 * device ready must be off when doing this! txclkesc?
+			 */
+			tmp = I915_READ(MIPI_CTRL(PORT_A));
+			tmp &= ~ESCAPE_CLOCK_DIVIDER_MASK;
+			I915_WRITE(MIPI_CTRL(PORT_A), tmp |
+					ESCAPE_CLOCK_DIVIDER_1);
+
+			/* read request priority is per pipe */
+			tmp = I915_READ(MIPI_CTRL(port));
+			tmp &= ~READ_REQUEST_PRIORITY_MASK;
+			I915_WRITE(MIPI_CTRL(port), tmp |
+					READ_REQUEST_PRIORITY_HIGH);
+		} else if (IS_GEN9_LP(dev_priv)) {
+			enum pipe pipe = intel_crtc->pipe;
+
+			tmp = I915_READ(MIPI_CTRL(port));
+			tmp &= ~BXT_PIPE_SELECT_MASK;
+
+			tmp |= BXT_PIPE_SELECT(pipe);
+			I915_WRITE(MIPI_CTRL(port), tmp);
+		}
+
+		/* XXX: why here, why like this? handling in irq handler?! */
+		I915_WRITE(MIPI_INTR_STAT(port), 0xffffffff);
+		I915_WRITE(MIPI_INTR_EN(port), 0xffffffff);
+
+		I915_WRITE(MIPI_DPHY_PARAM(port), intel_dsi->dphy_reg);
+
+		I915_WRITE(MIPI_DPI_RESOLUTION(port),
+			adjusted_mode->crtc_vdisplay << VERTICAL_ADDRESS_SHIFT |
+			mode_hdisplay << HORIZONTAL_ADDRESS_SHIFT);
+	}
+
+	set_dsi_timings(encoder, adjusted_mode);
 	val = intel_dsi->lane_count << DATA_LANES_PRG_REG_SHIFT;
 	if (is_cmd_mode(intel_dsi)) {
 		val |= intel_dsi->channel << CMD_MODE_CHANNEL_NUMBER_SHIFT;