diff --git a/drivers/gpu/drm/mxsfb/mxsfb_kms.c b/drivers/gpu/drm/mxsfb/mxsfb_kms.c
index 7b0abd0472aae..14f5cc590a51b 100644
--- a/drivers/gpu/drm/mxsfb/mxsfb_kms.c
+++ b/drivers/gpu/drm/mxsfb/mxsfb_kms.c
@@ -111,6 +111,57 @@ static void mxsfb_set_formats(struct mxsfb_drm_private *mxsfb,
 	writel(ctrl, mxsfb->base + LCDC_CTRL);
 }
 
+static void mxsfb_set_mode(struct mxsfb_drm_private *mxsfb, u32 bus_flags)
+{
+	struct drm_display_mode *m = &mxsfb->crtc.state->adjusted_mode;
+	u32 vdctrl0, vsync_pulse_len, hsync_pulse_len;
+
+	writel(TRANSFER_COUNT_SET_VCOUNT(m->crtc_vdisplay) |
+	       TRANSFER_COUNT_SET_HCOUNT(m->crtc_hdisplay),
+	       mxsfb->base + mxsfb->devdata->transfer_count);
+
+	vsync_pulse_len = m->crtc_vsync_end - m->crtc_vsync_start;
+
+	vdctrl0 = VDCTRL0_ENABLE_PRESENT |	/* Always in DOTCLOCK mode */
+		  VDCTRL0_VSYNC_PERIOD_UNIT |
+		  VDCTRL0_VSYNC_PULSE_WIDTH_UNIT |
+		  VDCTRL0_SET_VSYNC_PULSE_WIDTH(vsync_pulse_len);
+	if (m->flags & DRM_MODE_FLAG_PHSYNC)
+		vdctrl0 |= VDCTRL0_HSYNC_ACT_HIGH;
+	if (m->flags & DRM_MODE_FLAG_PVSYNC)
+		vdctrl0 |= VDCTRL0_VSYNC_ACT_HIGH;
+	/* Make sure Data Enable is high active by default */
+	if (!(bus_flags & DRM_BUS_FLAG_DE_LOW))
+		vdctrl0 |= VDCTRL0_ENABLE_ACT_HIGH;
+	/*
+	 * DRM_BUS_FLAG_PIXDATA_DRIVE_ defines are controller centric,
+	 * controllers VDCTRL0_DOTCLK is display centric.
+	 * Drive on positive edge       -> display samples on falling edge
+	 * DRM_BUS_FLAG_PIXDATA_DRIVE_POSEDGE -> VDCTRL0_DOTCLK_ACT_FALLING
+	 */
+	if (bus_flags & DRM_BUS_FLAG_PIXDATA_DRIVE_POSEDGE)
+		vdctrl0 |= VDCTRL0_DOTCLK_ACT_FALLING;
+
+	writel(vdctrl0, mxsfb->base + LCDC_VDCTRL0);
+
+	/* Frame length in lines. */
+	writel(m->crtc_vtotal, mxsfb->base + LCDC_VDCTRL1);
+
+	/* Line length in units of clocks or pixels. */
+	hsync_pulse_len = m->crtc_hsync_end - m->crtc_hsync_start;
+	writel(set_hsync_pulse_width(mxsfb, hsync_pulse_len) |
+	       VDCTRL2_SET_HSYNC_PERIOD(m->crtc_htotal),
+	       mxsfb->base + LCDC_VDCTRL2);
+
+	writel(SET_HOR_WAIT_CNT(m->crtc_htotal - m->crtc_hsync_start) |
+	       SET_VERT_WAIT_CNT(m->crtc_vtotal - m->crtc_vsync_start),
+	       mxsfb->base + LCDC_VDCTRL3);
+
+	writel(SET_DOTCLK_H_VALID_DATA_CNT(m->hdisplay),
+	       mxsfb->base + LCDC_VDCTRL4);
+
+}
+
 static void mxsfb_enable_controller(struct mxsfb_drm_private *mxsfb)
 {
 	u32 reg;
@@ -236,7 +287,6 @@ static void mxsfb_crtc_mode_set_nofb(struct mxsfb_drm_private *mxsfb,
 	struct drm_device *drm = mxsfb->crtc.dev;
 	struct drm_display_mode *m = &mxsfb->crtc.state->adjusted_mode;
 	u32 bus_flags = mxsfb->connector->display_info.bus_flags;
-	u32 vdctrl0, vsync_pulse_len, hsync_pulse_len;
 	int err;
 
 	/* Mandatory eLCDIF reset as per the Reference Manual */
@@ -256,49 +306,7 @@ static void mxsfb_crtc_mode_set_nofb(struct mxsfb_drm_private *mxsfb,
 			     bus_flags);
 	DRM_DEV_DEBUG_DRIVER(drm->dev, "Mode flags: 0x%08X\n", m->flags);
 
-	writel(TRANSFER_COUNT_SET_VCOUNT(m->crtc_vdisplay) |
-	       TRANSFER_COUNT_SET_HCOUNT(m->crtc_hdisplay),
-	       mxsfb->base + mxsfb->devdata->transfer_count);
-
-	vsync_pulse_len = m->crtc_vsync_end - m->crtc_vsync_start;
-
-	vdctrl0 = VDCTRL0_ENABLE_PRESENT |	/* Always in DOTCLOCK mode */
-		  VDCTRL0_VSYNC_PERIOD_UNIT |
-		  VDCTRL0_VSYNC_PULSE_WIDTH_UNIT |
-		  VDCTRL0_SET_VSYNC_PULSE_WIDTH(vsync_pulse_len);
-	if (m->flags & DRM_MODE_FLAG_PHSYNC)
-		vdctrl0 |= VDCTRL0_HSYNC_ACT_HIGH;
-	if (m->flags & DRM_MODE_FLAG_PVSYNC)
-		vdctrl0 |= VDCTRL0_VSYNC_ACT_HIGH;
-	/* Make sure Data Enable is high active by default */
-	if (!(bus_flags & DRM_BUS_FLAG_DE_LOW))
-		vdctrl0 |= VDCTRL0_ENABLE_ACT_HIGH;
-	/*
-	 * DRM_BUS_FLAG_PIXDATA_DRIVE_ defines are controller centric,
-	 * controllers VDCTRL0_DOTCLK is display centric.
-	 * Drive on positive edge       -> display samples on falling edge
-	 * DRM_BUS_FLAG_PIXDATA_DRIVE_POSEDGE -> VDCTRL0_DOTCLK_ACT_FALLING
-	 */
-	if (bus_flags & DRM_BUS_FLAG_PIXDATA_DRIVE_POSEDGE)
-		vdctrl0 |= VDCTRL0_DOTCLK_ACT_FALLING;
-
-	writel(vdctrl0, mxsfb->base + LCDC_VDCTRL0);
-
-	/* Frame length in lines. */
-	writel(m->crtc_vtotal, mxsfb->base + LCDC_VDCTRL1);
-
-	/* Line length in units of clocks or pixels. */
-	hsync_pulse_len = m->crtc_hsync_end - m->crtc_hsync_start;
-	writel(set_hsync_pulse_width(mxsfb, hsync_pulse_len) |
-	       VDCTRL2_SET_HSYNC_PERIOD(m->crtc_htotal),
-	       mxsfb->base + LCDC_VDCTRL2);
-
-	writel(SET_HOR_WAIT_CNT(m->crtc_htotal - m->crtc_hsync_start) |
-	       SET_VERT_WAIT_CNT(m->crtc_vtotal - m->crtc_vsync_start),
-	       mxsfb->base + LCDC_VDCTRL3);
-
-	writel(SET_DOTCLK_H_VALID_DATA_CNT(m->hdisplay),
-	       mxsfb->base + LCDC_VDCTRL4);
+	mxsfb_set_mode(mxsfb, bus_flags);
 }
 
 static int mxsfb_crtc_atomic_check(struct drm_crtc *crtc,
