tegra: video: use modedb to specify frame buffer resolution

Allow to specify framebuffer videomode using kernel command line
parameters. NVIDIAs binary X driver later on picks up those settings
and start X with current mode settings, if no EDID data are available.

Reused some of the implementation from Stefan's work for modedb
support[1] on Tegra20/30

[1] http://git.toradex.com/cgit/linux-toradex.git/commit/?h=tegra-next&id=1d3625dd9903bcc59e2df56836565ebb682948c1

Signed-off-by: Bhuvanchandra DV <bhuvanchandra.dv@toradex.com>
Acked-by: Marcel Ziswiler <marcel.ziswiler@toradex.com>

5 files changed, 326 insertions, 102 deletions

diff --git a/drivers/video/tegra/dc/bandwidth.c b/drivers/video/tegra/dc/bandwidth.c
index df4f4d686243..bf7b26b6cf52 100644
--- a/drivers/video/tegra/dc/bandwidth.c
+++ b/drivers/video/tegra/dc/bandwidth.c
@@ -917,7 +917,8 @@ long tegra_dc_calc_min_bandwidth(struct tegra_dc *dc)
 			pclk = KHZ2PICOS(150000); /* 150MHz max */
 #endif
 		} else if ((dc->out->type == TEGRA_DC_OUT_DP) ||
-			(dc->out->type == TEGRA_DC_OUT_NVSR_DP)) {
+			(dc->out->type == TEGRA_DC_OUT_NVSR_DP) ||
+			(dc->out->type == TEGRA_DC_OUT_LVDS)) {
 			if (dc->mode.pclk)
 				pclk = KHZ2PICOS(dc->mode.pclk / 1000);
 			else
diff --git a/drivers/video/tegra/dc/dc.c b/drivers/video/tegra/dc/dc.c
index f654ae64a957..5fb2e91933f7 100644
--- a/drivers/video/tegra/dc/dc.c
+++ b/drivers/video/tegra/dc/dc.c
@@ -1398,8 +1398,14 @@ static int tegra_dc_set_out(struct tegra_dc *dc, struct tegra_dc_out *out)
 		 */
 		tegra_dc_cache_cmu(dc, tegra_dc_get_cmu(dc));
 #endif
-	} else if (out->n_modes > 0)
+	}
+/* Donot set the mode now, in later stage we parse the vidargs from kernel args
+ * and set the mode accordingly
+ */
+#if 0
+	else if (out->n_modes > 0)
 		tegra_dc_set_mode(dc, &dc->out->modes[0]);
+#endif
 
 	switch (out->type) {
 	case TEGRA_DC_OUT_RGB:
diff --git a/drivers/video/tegra/dc/dp.c b/drivers/video/tegra/dc/dp.c
index 2e47617d3006..348c0d7ca176 100644
--- a/drivers/video/tegra/dc/dp.c
+++ b/drivers/video/tegra/dc/dp.c
@@ -2119,6 +2119,7 @@ static void tegra_dc_dp_enable(struct tegra_dc *dc)
 
 error_enable:
 	tegra_dp_default_int(dp, false);
+	tegra_dp_disable_irq(dp->irq);
 	tegra_dpaux_pad_power(dp->dc, false);
 	tegra_dpaux_clk_disable(dp);
 	tegra_dc_io_end(dc);
diff --git a/drivers/video/tegra/dc/mode.c b/drivers/video/tegra/dc/mode.c
index b600c70286e3..ab8907815749 100644
--- a/drivers/video/tegra/dc/mode.c
+++ b/drivers/video/tegra/dc/mode.c
@@ -32,6 +32,115 @@
 #include "dc_reg.h"
 #include "dc_priv.h"
 
+const struct fb_videomode tegra_modes[] = {
+	/* TouchRevolution Fusion 10" aka Chunghwa Picture Tubes
+	 * CLAA100NC05 10.1 inch 1024x600 single channel LVDS panel
+	 */
+	{
+		.name = "1024x600",
+		.refresh = 60,
+		.xres = 1024,
+		.yres = 600,
+		.pixclock = 20833,
+		.left_margin = 104,
+		.right_margin = 43,
+		.upper_margin = 24,
+		.lower_margin = 20,
+		.hsync_len = 5,
+		.vsync_len = 5,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+		.flag = FB_FLAG_RATIO_16_9,
+		.vmode = FB_VMODE_NONINTERLACED
+	},
+	{
+		/* 1366x768 */
+		.refresh =      60,
+		.xres =         1366,
+		.yres =         768,
+		.pixclock =     KHZ2PICOS(72072),
+		.hsync_len =    58,    /* h_sync_width */
+		.vsync_len =    4,     /* v_sync_width */
+		.left_margin =  58,    /* h_back_porch */
+		.upper_margin = 4,     /* v_back_porch */
+		.right_margin = 58,    /* h_front_porch */
+		.lower_margin = 4,      /* v_front_porch */
+		.vmode =        FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+	{
+		/* 1680x1050p 59.94/60hz */
+		.refresh =      60,
+		.xres =         1680,
+		.yres =         1050,
+		.pixclock =     KHZ2PICOS(147140),
+		.hsync_len =    184,    /* h_sync_width */
+		.vsync_len =    3,     /* v_sync_width */
+		.left_margin =  288,    /* h_back_porch */
+		.upper_margin = 33,     /* v_back_porch */
+		.right_margin = 104,    /* h_front_porch */
+		.lower_margin = 1,      /* v_front_porch */
+		.vmode =        FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+{
+		/* 1920x1080p 59.94/60hz CVT */
+		.refresh =	60,
+		.xres =         1920,
+		.yres =         1080,
+		.pixclock =     KHZ2PICOS(148500),
+		.hsync_len =    44,     /* h_sync_width */
+		.vsync_len =    5,      /* v_sync_width */
+		.left_margin =  148,     /* h_back_porch */
+		.upper_margin = 36,     /* v_back_porch */
+		.right_margin = 88,     /* h_front_porch */
+		.lower_margin = 4,      /* v_front_porch */
+		.vmode =        FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+	{
+		/* 1920x1200p 60hz */
+		.refresh =	60,
+		.xres =         1920,
+		.yres =         1200,
+		.pixclock =     KHZ2PICOS(154000),
+		.hsync_len =    32,     /* h_sync_width */
+		.vsync_len =    6,      /* v_sync_width */
+		.left_margin =  80,     /* h_back_porch */
+		.upper_margin = 26,     /* v_back_porch */
+		.right_margin = 48,     /* h_front_porch */
+		.lower_margin = 3,      /* v_front_porch */
+		.vmode =        FB_VMODE_NONINTERLACED,
+		.sync = 0,
+	},
+};
+
+/* try to find best matching mode using our modes, VESA and CEA modes from
+ * modedb
+ */
+int tegra_fb_find_mode(struct fb_var_screeninfo *var, struct fb_info *info,
+		       const char* option, unsigned int default_bpp)
+{
+	int out;
+
+	out = fb_find_mode(var, info, option, tegra_modes,
+			ARRAY_SIZE(tegra_modes), NULL, default_bpp);
+
+	/* Only accept this mode if we found a reasonable match (resolution) */
+	if (out == 1 || out == 2)
+		return out;
+
+	out = fb_find_mode(&info->var, info, option,
+			cea_modes, CEA_MODEDB_SIZE, NULL, default_bpp);
+
+	/* Check if we found a full match */
+	if (out == 1 || out == 2)
+		return out;
+
+	return fb_find_mode(&info->var, info, option,
+			vesa_modes, VESA_MODEDB_SIZE, NULL, default_bpp);
+}
+EXPORT_SYMBOL(tegra_fb_find_mode);
+
 /* return non-zero if constraint is violated */
 static int calc_h_ref_to_sync(const struct tegra_dc_mode *mode, int *href)
 {
@@ -417,6 +526,12 @@ static int _tegra_dc_set_mode(struct tegra_dc *dc,
 	}
 
 	memcpy(&dc->mode, mode, sizeof(dc->mode));
+
+	dev_info(&dc->ndev->dev, "using mode %dx%d pclk=%d href=%d vref=%d\n",
+		mode->h_active, mode->v_active, mode->pclk,
+		mode->h_ref_to_sync, mode->v_ref_to_sync
+	);
+
 	dc->mode_dirty = true;
 
 	if (dc->out->type == TEGRA_DC_OUT_RGB)
@@ -440,6 +555,78 @@ int tegra_dc_set_mode(struct tegra_dc *dc, const struct tegra_dc_mode *mode)
 }
 EXPORT_SYMBOL(tegra_dc_set_mode);
 
+int tegra_dc_var_to_dc_mode(struct tegra_dc *dc, struct fb_var_screeninfo *var,
+		struct tegra_dc_mode *mode)
+{
+	bool stereo_mode = false;
+	int err;
+
+	if (!var->pixclock)
+		return -EINVAL;
+
+	mode->pclk = PICOS2KHZ(var->pixclock) * 1000;
+	mode->h_sync_width = var->hsync_len;
+	mode->v_sync_width = var->vsync_len;
+	mode->h_back_porch = var->left_margin;
+	mode->v_back_porch = var->upper_margin;
+	mode->h_active = var->xres;
+	mode->v_active = var->yres;
+	mode->h_front_porch = var->right_margin;
+	mode->v_front_porch = var->lower_margin;
+	mode->stereo_mode = stereo_mode;
+	if (dc->out->type == TEGRA_DC_OUT_HDMI) {
+		/* HDMI controller requires h_ref=1, v_ref=1 */
+		mode->h_ref_to_sync = 1;
+		mode->v_ref_to_sync = 1;
+	} else {
+		/*
+		 * HACK:
+		 * If v_front_porch is only 1, we would violate Constraint 5/6
+		 * in this case, increase front porch by 1
+		 */
+		if (mode->v_front_porch <= 1)
+			mode->v_front_porch = 2;
+
+		err = calc_ref_to_sync(mode);
+		if (err) {
+			dev_err(&dc->ndev->dev, "display timing ref_to_sync"
+				"calculation failed with code %d\n", err);
+			return -EINVAL;
+		}
+		dev_info(&dc->ndev->dev, "Calculated sync href=%d vref=%d\n",
+				mode->h_ref_to_sync, mode->v_ref_to_sync);
+	}
+	if (!check_ref_to_sync(mode)) {
+		dev_err(&dc->ndev->dev,
+				"display timing doesn't meet restrictions.\n");
+		return -EINVAL;
+	}
+
+#ifndef CONFIG_TEGRA_HDMI_74MHZ_LIMIT
+	/* Double the pixel clock and update v_active only for
+	 * frame packed mode */
+	if (mode->stereo_mode) {
+		mode->pclk *= 2;
+		/* total v_active = yres*2 + activespace */
+		mode->v_active = var->yres * 2 +
+				var->vsync_len +
+				var->upper_margin +
+				var->lower_margin;
+	}
+#endif
+
+	mode->flags = 0;
+
+	if (!(var->sync & FB_SYNC_HOR_HIGH_ACT))
+		mode->flags |= TEGRA_DC_MODE_FLAG_NEG_H_SYNC;
+
+	if (!(var->sync & FB_SYNC_VERT_HIGH_ACT))
+		mode->flags |= TEGRA_DC_MODE_FLAG_NEG_V_SYNC;
+
+	return 0;
+}
+EXPORT_SYMBOL(tegra_dc_var_to_dc_mode);
+
 int tegra_dc_to_fb_videomode(struct fb_videomode *fbmode,
 	const struct tegra_dc_mode *mode)
 {
@@ -554,6 +741,7 @@ EXPORT_SYMBOL(tegra_dc_set_drm_mode);
 int tegra_dc_set_fb_mode(struct tegra_dc *dc,
 		const struct fb_videomode *fbmode, bool stereo_mode)
 {
+	int err;
 	struct tegra_dc_mode mode;
 
 	if (!fbmode->pixclock)
@@ -570,6 +758,7 @@ int tegra_dc_set_fb_mode(struct tegra_dc *dc,
 	mode.h_front_porch = fbmode->right_margin;
 	mode.v_front_porch = fbmode->lower_margin;
 	mode.stereo_mode = stereo_mode;
+#if 0
 	mode.vmode = fbmode->vmode;
 	if (fbmode->flag & FB_FLAG_RATIO_16_9)
 		mode.avi_m = TEGRA_DC_MODE_AVI_M_16_9;
@@ -580,6 +769,35 @@ int tegra_dc_set_fb_mode(struct tegra_dc *dc,
 
 	if (!check_mode_timings(dc, &mode))
 		return -EINVAL;
+#endif
+
+	if (dc->out->type == TEGRA_DC_OUT_HDMI) {
+		/* HDMI controller requires h_ref=1, v_ref=1 */
+		mode.h_ref_to_sync = 1;
+		mode.v_ref_to_sync = 1;
+	} else {
+		/*
+		 * HACK:
+		 * If v_front_porch is only 1, we would violate Constraint 5/6
+		 * in this case, increase front porch by 1
+		 */
+		if (mode.v_front_porch <= 1)
+			mode.v_front_porch = 2;
+
+		err = calc_ref_to_sync(&mode);
+		if (err) {
+			dev_err(&dc->ndev->dev, "display timing ref_to_sync"
+				"calculation failed with code %d\n", err);
+			return -EINVAL;
+		}
+		dev_info(&dc->ndev->dev, "Calculated sync href=%d vref=%d\n",
+				mode.h_ref_to_sync, mode.v_ref_to_sync);
+	}
+	if (!check_ref_to_sync(&mode)) {
+		dev_err(&dc->ndev->dev,
+				"display timing doesn't meet restrictions.\n");
+		return -EINVAL;
+	}
 
 #ifndef CONFIG_TEGRA_HDMI_74MHZ_LIMIT
 	/* Double the pixel clock and update v_active only for
diff --git a/drivers/video/tegra/fb.c b/drivers/video/tegra/fb.c
index db0378d738bb..61db9fe3bc32 100644
--- a/drivers/video/tegra/fb.c
+++ b/drivers/video/tegra/fb.c
@@ -85,9 +85,36 @@ static int tegra_fb_check_var(struct fb_var_screeninfo *var,
 		var->yoffset = yoffset;
 	}
 
+	var->xres_virtual = var->xres;
 	/* Double yres_virtual to allow double buffering through pan_display */
 	var->yres_virtual = var->yres * 2;
 
+	/* we only support RGB ordering for now */
+	switch (var->bits_per_pixel) {
+	case 32:
+	case 24:
+		var->bits_per_pixel = 32;
+		var->red.offset = 0;
+		var->red.length = 8;
+		var->green.offset = 8;
+		var->green.length = 8;
+		var->blue.offset = 16;
+		var->blue.length = 8;
+		var->transp.offset = 24;
+		var->transp.length = 8;
+		break;
+	case 16:
+	default:
+		var->bits_per_pixel = 16;
+		var->red.offset = 11;
+		var->red.length = 5;
+		var->green.offset = 5;
+		var->green.length = 6;
+		var->blue.offset = 0;
+		var->blue.length = 5;
+		break;
+	}
+
 	return 0;
 }
 
@@ -96,102 +123,56 @@ static int tegra_fb_set_par(struct fb_info *info)
 	struct tegra_fb_info *tegra_fb = info->par;
 	struct fb_var_screeninfo *var = &info->var;
 	struct tegra_dc *dc = tegra_fb->win.dc;
+	int err;
 
-	if (var->bits_per_pixel) {
-		/* we only support RGB ordering for now */
-		switch (var->bits_per_pixel) {
-		case 32:
-			var->red.offset = 0;
-			var->red.length = 8;
-			var->green.offset = 8;
-			var->green.length = 8;
-			var->blue.offset = 16;
-			var->blue.length = 8;
-			var->transp.offset = 24;
-			var->transp.length = 8;
-			tegra_fb->win.fmt = TEGRA_WIN_FMT_R8G8B8A8;
-			break;
-		case 16:
-			var->red.offset = 11;
-			var->red.length = 5;
-			var->green.offset = 5;
-			var->green.length = 6;
-			var->blue.offset = 0;
-			var->blue.length = 5;
-			tegra_fb->win.fmt = TEGRA_WIN_FMT_B5G6R5;
-			break;
-
-		default:
-			return -EINVAL;
-		}
-		/* if line_length unset, then pad the stride */
-		if (!info->fix.line_length) {
-			info->fix.line_length = var->xres * var->bits_per_pixel
-				/ 8;
-			info->fix.line_length = round_up(info->fix.line_length,
-						TEGRA_LINEAR_PITCH_ALIGNMENT);
-		}
-		tegra_fb->win.stride = info->fix.line_length;
-		tegra_fb->win.stride_uv = 0;
-		tegra_fb->win.phys_addr_u = 0;
-		tegra_fb->win.phys_addr_v = 0;
-	}
+	struct tegra_dc_mode mode;
 
-	if (var->pixclock) {
-		bool stereo;
-		unsigned old_len = 0;
-		struct fb_videomode m;
-		struct fb_videomode *old_mode = NULL;
-		struct tegra_fb_info *tegra_fb = info->par;
+	switch (var->bits_per_pixel) {
+	case 32:
+		tegra_fb->win.fmt = TEGRA_WIN_FMT_R8G8B8A8;
+		break;
+	case 16:
+		tegra_fb->win.fmt = TEGRA_WIN_FMT_B5G6R5;
+		break;
+	default:
+		return -EINVAL;
+		break;
+	}
 
+	/* if line_length unset, then pad the stride */
+	info->fix.line_length = var->xres * var->bits_per_pixel / 8;
+	info->fix.line_length = round_up(info->fix.line_length,
+				TEGRA_LINEAR_PITCH_ALIGNMENT);
+	tegra_fb->win.stride = info->fix.line_length;
+	tegra_fb->win.stride_uv = 0;
+	tegra_fb->win.phys_addr_u = 0;
+	tegra_fb->win.phys_addr_v = 0;
 
-		fb_var_to_videomode(&m, var);
+	tegra_fb->win.w.full = dfixed_const(var->xres);
+	tegra_fb->win.h.full = dfixed_const(var->yres);
+	tegra_fb->win.out_w = var->xres;
+	tegra_fb->win.out_h = var->yres;
 
-		/* Load framebuffer info with new mode details*/
-		old_mode = info->mode;
-		old_len  = info->fix.line_length;
-		memcpy(&tegra_fb->mode, &m, sizeof(tegra_fb->mode));
-		info->mode = (struct fb_videomode *)&tegra_fb->mode;
-		if (!info->mode) {
-			dev_warn(&tegra_fb->ndev->dev, "can't match video mode\n");
-			info->mode = old_mode;
-			return -EINVAL;
-		}
+	dev_info(&tegra_fb->ndev->dev, "switching framebuffer to %dx%d\n",
+			var->xres, var->yres);
 
-		/* Update fix line_length and window stride as per new mode */
-		info->fix.line_length = var->xres * var->bits_per_pixel / 8;
-		info->fix.line_length = round_up(info->fix.line_length,
-			TEGRA_LINEAR_PITCH_ALIGNMENT);
-		tegra_fb->win.stride = info->fix.line_length;
+	err = tegra_dc_var_to_dc_mode(dc, var, &mode);
+	if (err) {
+		dev_warn(&tegra_fb->ndev->dev, "could not convert var %d\n", err);
+		return -EINVAL;
+	}
 
-		/*
-		 * only enable stereo if the mode supports it and
-		 * client requests it
-		 */
-		stereo = !!(var->vmode & info->mode->vmode &
-#ifndef CONFIG_TEGRA_HDMI_74MHZ_LIMIT
-					FB_VMODE_STEREO_FRAME_PACK);
-#else
-					FB_VMODE_STEREO_LEFT_RIGHT);
-#endif
+	err = tegra_dc_set_mode(dc, &mode);
+	if (err) {
+		dev_warn(&tegra_fb->ndev->dev, "could not set dc mode %d\n", err);
+		return -EINVAL;
+	}
 
-		/* Configure DC with new mode */
-		if (tegra_dc_set_fb_mode(dc, info->mode, stereo)) {
-			/* Error while configuring DC, fallback to old mode */
-			dev_warn(&tegra_fb->ndev->dev, "can't configure dc with mode %ux%u\n",
-				info->mode->xres, info->mode->yres);
-			info->mode = old_mode;
-			info->fix.line_length = old_len;
-			tegra_fb->win.stride = old_len;
-			return -EINVAL;
-		}
+	if (dc->enabled)
+		tegra_dc_disable(dc);
+	tegra_dc_enable(dc);
 
-		tegra_fb->win.w.full = dfixed_const(info->mode->xres);
-		tegra_fb->win.h.full = dfixed_const(info->mode->yres);
-		tegra_fb->win.out_w = info->mode->xres;
-		tegra_fb->win.out_h = info->mode->yres;
-	}
-	return 0;
+	return err;
 }
 
 static int tegra_fb_setcolreg(unsigned regno, unsigned red, unsigned green,
@@ -692,9 +673,10 @@ struct tegra_fb_info *tegra_fb_register(struct platform_device *ndev,
 	void __iomem *fb_base = NULL;
 	phys_addr_t fb_size = 0;
 	int ret = 0;
-	int mode_idx;
 	unsigned stride;
-	struct fb_videomode m;
+	char *param_option = NULL;
+	const char *option = NULL;
+	char driver[10];
 
 	if (!tegra_dc_get_window(dc, fb_data->win)) {
 		dev_err(&ndev->dev, "dc does not have a window at index %d\n",
@@ -711,8 +693,6 @@ struct tegra_fb_info *tegra_fb_register(struct platform_device *ndev,
 	tegra_fb = info->par;
 	tegra_fb->ndev = ndev;
 	tegra_fb->fb_mem = fb_mem;
-	tegra_fb->xres = fb_data->xres;
-	tegra_fb->yres = fb_data->yres;
 
 	tegra_fb->win.idx = fb_data->win;
 
@@ -751,10 +731,14 @@ struct tegra_fb_info *tegra_fb_register(struct platform_device *ndev,
 	info->fix.smem_start	= (u32)tegra_fb->phys_start;
 	info->fix.smem_len	= fb_size;
 	info->fix.line_length = stride;
+
+#if 0
 	INIT_LIST_HEAD(&info->modelist);
 	/* pick first mode as the default for initialization */
 	tegra_dc_to_fb_videomode(&m, &dc->mode);
 	fb_videomode_to_var(&info->var, &m);
+#endif
+
 	info->var.xres_virtual		= fb_data->xres;
 	info->var.yres_virtual		= fb_data->yres * 2;
 	info->var.bits_per_pixel	= fb_data->bits_per_pixel;
@@ -767,7 +751,7 @@ struct tegra_fb_info *tegra_fb_register(struct platform_device *ndev,
 	tegra_fb->win.y.full = dfixed_const(0);
 	tegra_fb->win.w.full = dfixed_const(fb_data->xres);
 	tegra_fb->win.h.full = dfixed_const(fb_data->yres);
-	/* TODO: set to output res dc */
+
 	tegra_fb->win.out_x = 0;
 	tegra_fb->win.out_y = 0;
 	tegra_fb->win.out_w = fb_data->xres;
@@ -782,18 +766,32 @@ struct tegra_fb_info *tegra_fb_register(struct platform_device *ndev,
 	tegra_fb->win.flags = TEGRA_WIN_FLAG_ENABLED;
 	tegra_fb->win.global_alpha = 0xFF;
 
-	for (mode_idx = 0; mode_idx < dc->out->n_modes; mode_idx++) {
-		struct tegra_dc_mode mode = dc->out->modes[mode_idx];
-		struct fb_videomode vmode;
-
-		mode.pclk = dc->mode.pclk;
+	/* try to use kernel cmd line specified mode */
+	sprintf(driver, "tegrafb%d", ndev->id);
+	fb_get_options(driver, &param_option);
+	if (param_option != NULL) {
+		option = param_option;
+		dev_info(&ndev->dev, "parse cmd options for %s: %s\n",
+				driver, option);
+	} else {
+		option = dc->out->default_mode;
+		dev_info(&ndev->dev, "use default mode for %s: %s\n",
+				driver, option);
+	}
 
-		if (mode.pclk > 1000) {
-			tegra_dc_to_fb_videomode(&vmode, &mode);
-			fb_add_videomode(&vmode, &info->modelist);
+	if (option != NULL)
+	{
+		if (!strcmp(option, "off")) {
+			ret = -ENODEV;
+			goto err_iounmap_fb;
+		}
+		if (!tegra_fb_find_mode(&info->var, info, option, 16)) {
+			ret = -EINVAL;
+			goto err_iounmap_fb;
 		}
 	}
 
+	/* activate current settings.. */
 	if (fb_mem)
 		tegra_fb_set_par(info);