diff --git a/drivers/video/mxc/mxc_edid.c b/drivers/video/mxc/mxc_edid.c
index 1363403bc228b6..bd0020b7713d6d 100644
--- a/drivers/video/mxc/mxc_edid.c
+++ b/drivers/video/mxc/mxc_edid.c
@@ -192,6 +192,21 @@ const struct fb_videomode mxc_cea_mode[64] = {
 	},
 };
 
+/* 0x8 is FB_VMODE_FRACT (not yet merged) */
+#define FB_VMODE_MASK_SIMPLE (FB_VMODE_NONINTERLACED | FB_VMODE_INTERLACED | 0x8)
+
+int mxc_fb_mode_is_equal_res(const struct fb_videomode *mode1,
+			     const struct fb_videomode *mode2)
+{
+	return (mode1->xres         == mode2->xres &&
+		mode1->yres         == mode2->yres &&
+		mode1->refresh      == mode2->refresh &&
+		mode1->sync         == mode2->sync &&
+		(mode1->vmode & FB_VMODE_MASK_SIMPLE) ==
+		(mode2->vmode & FB_VMODE_MASK_SIMPLE));
+}
+EXPORT_SYMBOL(mxc_fb_mode_is_equal_res);
+
 /*
  * We have a special version of fb_mode_is_equal that ignores
  * pixclock, since for many CEA modes, 2 frequencies are supported
@@ -793,3 +808,50 @@ int mxc_edid_read(struct i2c_adapter *adp, unsigned short addr,
 }
 EXPORT_SYMBOL(mxc_edid_read);
 
+const struct fb_videomode *mxc_fb_find_nearest_mode(const struct fb_videomode *mode,
+						    struct list_head *head, bool relax)
+{
+	struct list_head *pos;
+	struct fb_modelist *modelist;
+	struct fb_videomode *cmode;
+	static struct fb_videomode *best;
+	static u32 diff, diff_refresh;
+	u32 mask = relax ? FB_VMODE_MASK_SIMPLE | FB_VMODE_ASPECT_MASK : ~0;
+
+	if (!relax) {
+		diff = -1;
+		diff_refresh = -1;
+		best = NULL;
+	}
+
+	list_for_each(pos, head) {
+		u32 d;
+
+		modelist = list_entry(pos, struct fb_modelist, list);
+		cmode = &modelist->mode;
+
+		if ((mode->vmode ^ cmode->vmode) & mask)
+				continue;
+
+		d = abs(cmode->xres - mode->xres) +
+			abs(cmode->yres - mode->yres);
+		if (diff > d) {
+			diff = d;
+			diff_refresh = abs(cmode->refresh - mode->refresh);
+			best = cmode;
+		} else if (diff == d) {
+			d = abs(cmode->refresh - mode->refresh);
+			if (diff_refresh > d) {
+				diff_refresh = d;
+				best = cmode;
+			}
+		}
+	}
+
+	if ((!relax && (diff_refresh || diff)) || !best)
+		mxc_fb_find_nearest_mode(mode, head, true);
+
+	return best;
+}
+EXPORT_SYMBOL(mxc_fb_find_nearest_mode);
+
diff --git a/drivers/video/mxc/mxc_hdmi.c b/drivers/video/mxc/mxc_hdmi.c
index 6d3e5d8f7c367f..dcda11b8703b53 100644
--- a/drivers/video/mxc/mxc_hdmi.c
+++ b/drivers/video/mxc/mxc_hdmi.c
@@ -187,6 +187,10 @@ struct mxc_hdmi {
 	bool phy_enabled;
 	struct fb_videomode default_mode;
 	struct fb_videomode previous_non_vga_mode;
+	struct prev_virtual_t {
+		u32 xres_virtual;
+		u32 yres_virtual;
+	} prev_virtual;
 	bool requesting_vga_for_initialization;
 
 	int *gpr_base;
@@ -2011,7 +2015,7 @@ static void mxc_hdmi_set_mode_to_vga_dvi(struct mxc_hdmi *hdmi)
 	hdmi->requesting_vga_for_initialization = false;
 }
 
-static void mxc_hdmi_set_mode(struct mxc_hdmi *hdmi)
+static void mxc_hdmi_set_mode(struct mxc_hdmi *hdmi, int edid_status)
 {
 	const struct fb_videomode *mode;
 	struct fb_videomode m;
@@ -2027,14 +2031,16 @@ static void mxc_hdmi_set_mode(struct mxc_hdmi *hdmi)
 		fb_videomode_to_var(&var, &hdmi->previous_non_vga_mode);
 	}
 
-	if (var.xres) {
+	/* check for active xBuffer, look for similar (x/y/hz/vmode) mode
+	 * otherwise be nice looking for best (new) display mode */
+	if (edid_status == HDMI_EDID_SUCCESS && !hdmi->prev_virtual.xres_virtual) {
+		dev_dbg(&hdmi->pdev->dev,
+				"xBuffer not active, trying new best display mode\n");
+		mode = fb_find_best_display(&hdmi->fbi->monspecs, &hdmi->fbi->modelist);
+	} else {
 		fb_var_to_videomode(&m, &var);
 		dump_fb_videomode(&m);
 		mode = fb_find_nearest_mode(&m, &hdmi->fbi->modelist);
-	} else {
-		dev_dbg(&hdmi->pdev->dev,
-				"No default mode using the best for the display\n");
-		mode = fb_find_best_display(&hdmi->fbi->monspecs, &hdmi->fbi->modelist);
 	}
 
 	if (!mode) {
@@ -2046,19 +2052,29 @@ static void mxc_hdmi_set_mode(struct mxc_hdmi *hdmi)
 	fb_blank(hdmi->fbi, FB_BLANK_UNBLANK);
 	console_unlock();
 
+	/* update fbi mode in case modelist is updated */
+	hdmi->fbi->mode = (struct fb_videomode *)mode;
+	dump_fb_videomode(hdmi->fbi->mode);
+
 	/* If video mode same as previous, init HDMI again */
-	if (fb_mode_is_equal(&hdmi->previous_non_vga_mode, mode)) {
+	if (fb_mode_is_equal(&hdmi->previous_non_vga_mode, mode) && edid_status == HDMI_EDID_SAME) {
+		dev_dbg(&hdmi->pdev->dev,
+				"%s: Video mode + EDID same as previous\n", __func__);
+		if (hdmi->prev_virtual.xres_virtual)
+			memcpy(&hdmi->fbi->var.xres_virtual, &hdmi->prev_virtual, sizeof(hdmi->prev_virtual));
+		mxc_hdmi_setup(hdmi, 0);
+	} else if (mxc_fb_mode_is_equal_res(&hdmi->previous_non_vga_mode, mode) && edid_status != HDMI_EDID_SAME) {
 		dev_dbg(&hdmi->pdev->dev,
 				"%s: Video mode same as previous\n", __func__);
-		/* update fbi mode in case modelist is updated */
-		hdmi->fbi->mode = (struct fb_videomode *)mode;
+		fb_videomode_to_var(&hdmi->fbi->var, mode);
+		if (hdmi->prev_virtual.xres_virtual)
+			memcpy(&hdmi->fbi->var.xres_virtual, &hdmi->prev_virtual, sizeof(hdmi->prev_virtual));
 		/* update hdmi setting in case EDID data updated  */
-		mxc_hdmi_setup(hdmi, 0);
+		mxc_hdmi_notify_fb(hdmi);
 	} else {
 		dev_dbg(&hdmi->pdev->dev, "%s: New video mode\n", __func__);
 		mxc_hdmi_set_mode_to_vga_dvi(hdmi);
 		fb_videomode_to_var(&hdmi->fbi->var, mode);
-		dump_fb_videomode((struct fb_videomode *)mode);
 		mxc_hdmi_notify_fb(hdmi);
 	}
 
@@ -2120,7 +2136,7 @@ static void mxc_hdmi_cable_connected(struct mxc_hdmi *hdmi)
 	hdmi_set_edid_cfg(edid_status, &hdmi->edid_cfg);
 
 	/* Setting video mode */
-	mxc_hdmi_set_mode(hdmi);
+	mxc_hdmi_set_mode(hdmi, edid_status);
 
 	dev_dbg(&hdmi->pdev->dev, "%s exit\n", __func__);
 }
@@ -2158,10 +2174,6 @@ static void mxc_hdmi_cable_disconnected(struct mxc_hdmi *hdmi)
 
 	hdmi_disable_overflow_interrupts();
 
-	/* Prepare driver for next connection */
-	hdmi->dft_mode_set = false;
-	memset(&hdmi->previous_non_vga_mode, 0, sizeof(struct fb_videomode));
-
 	console_lock();
 	fb_blank(hdmi->fbi, FB_BLANK_POWERDOWN);
 	console_unlock();
@@ -2336,10 +2348,14 @@ static void mxc_hdmi_setup(struct mxc_hdmi *hdmi, unsigned long event)
 		 * vga default. */
 		memcpy(&hdmi->previous_non_vga_mode, &m,
 		       sizeof(struct fb_videomode));
+		/* Save any double/tripple buffer configuration (if active) */
+		if (&hdmi->fbi->var.xres_virtual != &hdmi->fbi->var.xres)
+			memcpy(&hdmi->prev_virtual, &hdmi->fbi->var.xres_virtual, sizeof(hdmi->prev_virtual));
+		else
+			memset(&hdmi->prev_virtual, 0, sizeof(hdmi->prev_virtual));
 		if (!list_empty(&hdmi->fbi->modelist)) {
 			edid_mode = fb_find_nearest_mode(&m, &hdmi->fbi->modelist);
 			pr_debug("edid mode ");
-			dump_fb_videomode((struct fb_videomode *)edid_mode);
 			/* update fbi mode */
 			hdmi->fbi->mode = (struct fb_videomode *)edid_mode;
 			hdmi->vic = mxc_edid_mode_to_vic(edid_mode);
@@ -2795,19 +2811,19 @@ static int mxc_hdmi_disp_init(struct mxc_dispdrv_handle *disp,
 
 	/* Find a nearest mode in default modelist */
 	fb_var_to_videomode(&m, &hdmi->fbi->var);
-	dump_fb_videomode(&m);
-
 	hdmi->dft_mode_set = false;
-	/* Save default video mode */
-	memcpy(&hdmi->default_mode, &m, sizeof(struct fb_videomode));
 
 	mode = fb_find_nearest_mode(&m, &hdmi->fbi->modelist);
 	if (!mode) {
 		pr_err("%s: could not find mode in modelist\n", __func__);
 		return -1;
 	}
+	dump_fb_videomode((struct fb_videomode *)mode);
+	/* Save default video mode */
+	memcpy(&hdmi->default_mode, mode, sizeof(struct fb_videomode));
 
 	fb_videomode_to_var(&hdmi->fbi->var, mode);
+	memcpy(&hdmi->prev_virtual, &hdmi->fbi->var.xres_virtual, sizeof(hdmi->prev_virtual));
 
 	/* update fbi mode */
 	hdmi->fbi->mode = (struct fb_videomode *)mode;
diff --git a/include/video/mxc_edid.h b/include/video/mxc_edid.h
index b3e5dedbb61bec..446e040b5a3ced 100644
--- a/include/video/mxc_edid.h
+++ b/include/video/mxc_edid.h
@@ -105,4 +105,7 @@ int mxc_edid_read(struct i2c_adapter *adp, unsigned short addr,
 	unsigned char *edid, struct mxc_edid_cfg *cfg, struct fb_info *fbi);
 int mxc_edid_parse_ext_blk(unsigned char *edid, struct mxc_edid_cfg *cfg,
 	struct fb_monspecs *specs);
+const struct fb_videomode *mxc_fb_find_nearest_mode(const struct fb_videomode *mode,
+                                                    struct list_head *head, bool relax);
+int mxc_fb_mode_is_equal_res(const struct fb_videomode *mode1, const struct fb_videomode *mode2);
 #endif