ENGR00240987: video: port LDB and LCD FB display drivers from 3.5.7 kernel

This is a fast-forward porting of LDB and LCD frame buffer drivers from
3.5.7 kernel to kernel 3.10.  The change set is kept as minimum as
possible with only making necessary code changes, which are mostly for
solving compile problems.

 * Remove <mach/*> inclusions
 * Drop __devinit and __devexit
 * Drop vm flag VM_RESERVED
 * Protect ldb_setup() with #ifndef MODULE to fix "warning: ‘ldb_setup’
   defined but not used" in module build
 * Remove fb_prepare_logo() and fb_show_logo() calls, because it breaks
   module build, and penguin logo works fine without the calls in
   mxc_ipuv3_fb.c

Signed-off-by: Shawn Guo <shawn.guo@freescale.com>

9 files changed, 4027 insertions, 0 deletions

diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index 2e937bdace6f..120811cbc063 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -2476,6 +2476,10 @@ source "drivers/video/exynos/Kconfig"
 source "drivers/video/mmp/Kconfig"
 source "drivers/video/backlight/Kconfig"
 
+if ARCH_MXC
+source "drivers/video/mxc/Kconfig"
+endif
+
 if VT
 	source "drivers/video/console/Kconfig"
 endif
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index e8bae8dd4804..ba3809d5853b 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -53,6 +53,7 @@ obj-$(CONFIG_FB_KYRO)             += kyro/
 obj-$(CONFIG_FB_SAVAGE)		  += savage/
 obj-$(CONFIG_FB_GEODE)		  += geode/
 obj-$(CONFIG_FB_MBX)		  += mbx/
+obj-$(CONFIG_FB_MXC)		  += mxc/
 obj-$(CONFIG_FB_NEOMAGIC)         += neofb.o
 obj-$(CONFIG_FB_3DFX)             += tdfxfb.o
 obj-$(CONFIG_FB_CONTROL)          += controlfb.o
diff --git a/drivers/video/mxc/Kconfig b/drivers/video/mxc/Kconfig
new file mode 100644
index 000000000000..0b4eaa97e094
--- /dev/null
+++ b/drivers/video/mxc/Kconfig
@@ -0,0 +1,24 @@
+config FB_MXC
+	tristate "MXC Framebuffer support"
+	depends on FB
+	select FB_CFB_FILLRECT
+	select FB_CFB_COPYAREA
+	select FB_CFB_IMAGEBLIT
+	select FB_MODE_HELPERS
+	default y
+	help
+	  This is a framebuffer device for the MXC LCD Controller.
+	  See <http://www.linux-fbdev.org/> for information on framebuffer
+	  devices.
+
+	  If you plan to use the LCD display with your MXC system, say
+	  Y here.
+
+config FB_MXC_SYNC_PANEL
+	depends on FB_MXC
+	tristate "Synchronous Panel Framebuffer"
+
+config FB_MXC_LDB
+	tristate "MXC LDB"
+	depends on FB_MXC_SYNC_PANEL
+	depends on MXC_IPU_V3
diff --git a/drivers/video/mxc/Makefile b/drivers/video/mxc/Makefile
new file mode 100644
index 000000000000..5fc36bbf6ef3
--- /dev/null
+++ b/drivers/video/mxc/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_FB_MXC_LDB) += ldb.o
+obj-$(CONFIG_FB_MXC_SYNC_PANEL) += mxc_dispdrv.o mxc_lcdif.o mxc_ipuv3_fb.o
diff --git a/drivers/video/mxc/ldb.c b/drivers/video/mxc/ldb.c
new file mode 100644
index 000000000000..c2412418ceda
--- /dev/null
+++ b/drivers/video/mxc/ldb.c
@@ -0,0 +1,1012 @@
+/*
+ * Copyright (C) 2012-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+/*!
+ * @file mxc_ldb.c
+ *
+ * @brief This file contains the LDB driver device interface and fops
+ * functions.
+ */
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/console.h>
+#include <linux/io.h>
+#include <linux/ipu.h>
+#include <linux/mxcfb.h>
+#include <linux/regulator/consumer.h>
+#include <linux/spinlock.h>
+#include <linux/of_device.h>
+#include <linux/mod_devicetable.h>
+#include "mxc_dispdrv.h"
+
+#define DISPDRV_LDB	"ldb"
+
+#define LDB_BGREF_RMODE_MASK		0x00008000
+#define LDB_BGREF_RMODE_INT		0x00008000
+#define LDB_BGREF_RMODE_EXT		0x0
+
+#define LDB_DI1_VS_POL_MASK		0x00000400
+#define LDB_DI1_VS_POL_ACT_LOW		0x00000400
+#define LDB_DI1_VS_POL_ACT_HIGH		0x0
+#define LDB_DI0_VS_POL_MASK		0x00000200
+#define LDB_DI0_VS_POL_ACT_LOW		0x00000200
+#define LDB_DI0_VS_POL_ACT_HIGH		0x0
+
+#define LDB_BIT_MAP_CH1_MASK		0x00000100
+#define LDB_BIT_MAP_CH1_JEIDA		0x00000100
+#define LDB_BIT_MAP_CH1_SPWG		0x0
+#define LDB_BIT_MAP_CH0_MASK		0x00000040
+#define LDB_BIT_MAP_CH0_JEIDA		0x00000040
+#define LDB_BIT_MAP_CH0_SPWG		0x0
+
+#define LDB_DATA_WIDTH_CH1_MASK		0x00000080
+#define LDB_DATA_WIDTH_CH1_24		0x00000080
+#define LDB_DATA_WIDTH_CH1_18		0x0
+#define LDB_DATA_WIDTH_CH0_MASK		0x00000020
+#define LDB_DATA_WIDTH_CH0_24		0x00000020
+#define LDB_DATA_WIDTH_CH0_18		0x0
+
+#define LDB_CH1_MODE_MASK		0x0000000C
+#define LDB_CH1_MODE_EN_TO_DI1		0x0000000C
+#define LDB_CH1_MODE_EN_TO_DI0		0x00000004
+#define LDB_CH1_MODE_DISABLE		0x0
+#define LDB_CH0_MODE_MASK		0x00000003
+#define LDB_CH0_MODE_EN_TO_DI1		0x00000003
+#define LDB_CH0_MODE_EN_TO_DI0		0x00000001
+#define LDB_CH0_MODE_DISABLE		0x0
+
+#define LDB_SPLIT_MODE_EN		0x00000010
+
+enum {
+	IMX6_LDB,
+};
+
+enum {
+	LDB_IMX6 = 1,
+};
+
+struct fsl_mxc_ldb_platform_data {
+	int devtype;
+	u32 ext_ref;
+#define LDB_SPL_DI0	1
+#define LDB_SPL_DI1	2
+#define LDB_DUL_DI0	3
+#define LDB_DUL_DI1	4
+#define LDB_SIN0	5
+#define LDB_SIN1	6
+#define LDB_SEP0	7
+#define LDB_SEP1	8
+	int mode;
+	int ipu_id;
+	int disp_id;
+
+	/*only work for separate mode*/
+	int sec_ipu_id;
+	int sec_disp_id;
+};
+
+struct ldb_data {
+	struct platform_device *pdev;
+	struct mxc_dispdrv_handle *disp_ldb;
+	uint32_t *reg;
+	uint32_t *control_reg;
+	uint32_t *gpr3_reg;
+	uint32_t control_reg_data;
+	struct regulator *lvds_bg_reg;
+	int mode;
+	bool inited;
+	struct ldb_setting {
+		struct clk *di_clk;
+		struct clk *ldb_di_clk;
+		bool active;
+		bool clk_en;
+		int ipu;
+		int di;
+		uint32_t ch_mask;
+		uint32_t ch_val;
+	} setting[2];
+	struct notifier_block nb;
+};
+
+static int g_ldb_mode;
+
+static struct fb_videomode ldb_modedb[] = {
+	{
+	 "LDB-WXGA", 60, 1280, 800, 14065,
+	 40, 40,
+	 10, 3,
+	 80, 10,
+	 0,
+	 FB_VMODE_NONINTERLACED,
+	 FB_MODE_IS_DETAILED,},
+	{
+	 "LDB-XGA", 60, 1024, 768, 15385,
+	 220, 40,
+	 21, 7,
+	 60, 10,
+	 0,
+	 FB_VMODE_NONINTERLACED,
+	 FB_MODE_IS_DETAILED,},
+	{
+	 "LDB-1080P60", 60, 1920, 1080, 7692,
+	 100, 40,
+	 30, 3,
+	 10, 2,
+	 0,
+	 FB_VMODE_NONINTERLACED,
+	 FB_MODE_IS_DETAILED,},
+};
+static int ldb_modedb_sz = ARRAY_SIZE(ldb_modedb);
+
+static inline int is_imx6_ldb(struct fsl_mxc_ldb_platform_data *plat_data)
+{
+	return (plat_data->devtype == LDB_IMX6);
+}
+
+static int bits_per_pixel(int pixel_fmt)
+{
+	switch (pixel_fmt) {
+	case IPU_PIX_FMT_BGR24:
+	case IPU_PIX_FMT_RGB24:
+		return 24;
+		break;
+	case IPU_PIX_FMT_BGR666:
+	case IPU_PIX_FMT_RGB666:
+	case IPU_PIX_FMT_LVDS666:
+		return 18;
+		break;
+	default:
+		break;
+	}
+	return 0;
+}
+
+static int valid_mode(int pixel_fmt)
+{
+	return ((pixel_fmt == IPU_PIX_FMT_RGB24) ||
+		(pixel_fmt == IPU_PIX_FMT_BGR24) ||
+		(pixel_fmt == IPU_PIX_FMT_LVDS666) ||
+		(pixel_fmt == IPU_PIX_FMT_RGB666) ||
+		(pixel_fmt == IPU_PIX_FMT_BGR666));
+}
+
+static int parse_ldb_mode(char *mode)
+{
+	int ldb_mode;
+
+	if (!strcmp(mode, "spl0"))
+		ldb_mode = LDB_SPL_DI0;
+	else if (!strcmp(mode, "spl1"))
+		ldb_mode = LDB_SPL_DI1;
+	else if (!strcmp(mode, "dul0"))
+		ldb_mode = LDB_DUL_DI0;
+	else if (!strcmp(mode, "dul1"))
+		ldb_mode = LDB_DUL_DI1;
+	else if (!strcmp(mode, "sin0"))
+		ldb_mode = LDB_SIN0;
+	else if (!strcmp(mode, "sin1"))
+		ldb_mode = LDB_SIN1;
+	else if (!strcmp(mode, "sep0"))
+		ldb_mode = LDB_SEP0;
+	else if (!strcmp(mode, "sep1"))
+		ldb_mode = LDB_SEP1;
+	else
+		ldb_mode = -EINVAL;
+
+	return ldb_mode;
+}
+
+#ifndef MODULE
+/*
+ *    "ldb=spl0/1"       --      split mode on DI0/1
+ *    "ldb=dul0/1"       --      dual mode on DI0/1
+ *    "ldb=sin0/1"       --      single mode on LVDS0/1
+ *    "ldb=sep0/1" 	 --      separate mode begin from LVDS0/1
+ *
+ *    there are two LVDS channels(LVDS0 and LVDS1) which can transfer video
+ *    datas, there two channels can be used as split/dual/single/separate mode.
+ *
+ *    split mode means display data from DI0 or DI1 will send to both channels
+ *    LVDS0+LVDS1.
+ *    dual mode means display data from DI0 or DI1 will be duplicated on LVDS0
+ *    and LVDS1, it said, LVDS0 and LVDS1 has the same content.
+ *    single mode means only work for DI0/DI1->LVDS0 or DI0/DI1->LVDS1.
+ *    separate mode means you can make DI0/DI1->LVDS0 and DI0/DI1->LVDS1 work
+ *    at the same time.
+ */
+static int __init ldb_setup(char *options)
+{
+	g_ldb_mode = parse_ldb_mode(options);
+	return (g_ldb_mode < 0) ? 0 : 1;
+}
+__setup("ldb=", ldb_setup);
+#endif
+
+static int ldb_get_of_property(struct platform_device *pdev,
+				struct fsl_mxc_ldb_platform_data *plat_data)
+{
+	struct device_node *np = pdev->dev.of_node;
+	int err;
+	u32 ipu_id, disp_id;
+	u32 sec_ipu_id, sec_disp_id;
+	char *mode;
+	u32 ext_ref;
+
+	err = of_property_read_string(np, "mode", (const char **)&mode);
+	if (err) {
+		dev_dbg(&pdev->dev, "get of property mode fail\n");
+		return err;
+	}
+	err = of_property_read_u32(np, "ext_ref", &ext_ref);
+	if (err) {
+		dev_dbg(&pdev->dev, "get of property ext_ref fail\n");
+		return err;
+	}
+	err = of_property_read_u32(np, "ipu_id", &ipu_id);
+	if (err) {
+		dev_dbg(&pdev->dev, "get of property ipu_id fail\n");
+		return err;
+	}
+	err = of_property_read_u32(np, "disp_id", &disp_id);
+	if (err) {
+		dev_dbg(&pdev->dev, "get of property disp_id fail\n");
+		return err;
+	}
+	err = of_property_read_u32(np, "sec_ipu_id", &sec_ipu_id);
+	if (err) {
+		dev_dbg(&pdev->dev, "get of property sec_ipu_id fail\n");
+		return err;
+	}
+	err = of_property_read_u32(np, "sec_disp_id", &sec_disp_id);
+	if (err) {
+		dev_dbg(&pdev->dev, "get of property sec_disp_id fail\n");
+		return err;
+	}
+
+	plat_data->mode = parse_ldb_mode(mode);
+	plat_data->ext_ref = ext_ref;
+	plat_data->ipu_id = ipu_id;
+	plat_data->disp_id = disp_id;
+	plat_data->sec_ipu_id = sec_ipu_id;
+	plat_data->sec_disp_id = sec_disp_id;
+
+	return err;
+}
+
+static int find_ldb_setting(struct ldb_data *ldb, struct fb_info *fbi)
+{
+	char *id_di[] = {
+		 "DISP3 BG",
+		 "DISP3 BG - DI1",
+		};
+	char id[16];
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		if (ldb->setting[i].active) {
+			memset(id, 0, 16);
+			memcpy(id, id_di[ldb->setting[i].di],
+				strlen(id_di[ldb->setting[i].di]));
+			id[4] += ldb->setting[i].ipu;
+			if (!strcmp(id, fbi->fix.id))
+				return i;
+		}
+	}
+	return -EINVAL;
+}
+
+static int ldb_disp_setup(struct mxc_dispdrv_handle *disp, struct fb_info *fbi)
+{
+	uint32_t reg, val;
+	uint32_t pixel_clk, rounded_pixel_clk;
+	struct clk *ldb_clk_parent;
+	struct ldb_data *ldb = mxc_dispdrv_getdata(disp);
+	int setting_idx, di;
+	int ret;
+
+	setting_idx = find_ldb_setting(ldb, fbi);
+	if (setting_idx < 0)
+		return setting_idx;
+
+	di = ldb->setting[setting_idx].di;
+
+	/* restore channel mode setting */
+	val = readl(ldb->control_reg);
+	val |= ldb->setting[setting_idx].ch_val;
+	writel(val, ldb->control_reg);
+	dev_dbg(&ldb->pdev->dev, "LDB setup, control reg:0x%x\n",
+			readl(ldb->control_reg));
+
+	/* vsync setup */
+	reg = readl(ldb->control_reg);
+	if (fbi->var.sync & FB_SYNC_VERT_HIGH_ACT) {
+		if (di == 0)
+			reg = (reg & ~LDB_DI0_VS_POL_MASK)
+				| LDB_DI0_VS_POL_ACT_HIGH;
+		else
+			reg = (reg & ~LDB_DI1_VS_POL_MASK)
+				| LDB_DI1_VS_POL_ACT_HIGH;
+	} else {
+		if (di == 0)
+			reg = (reg & ~LDB_DI0_VS_POL_MASK)
+				| LDB_DI0_VS_POL_ACT_LOW;
+		else
+			reg = (reg & ~LDB_DI1_VS_POL_MASK)
+				| LDB_DI1_VS_POL_ACT_LOW;
+	}
+	writel(reg, ldb->control_reg);
+
+	/* clk setup */
+	if (ldb->setting[setting_idx].clk_en)
+		 clk_disable_unprepare(ldb->setting[setting_idx].ldb_di_clk);
+	pixel_clk = (PICOS2KHZ(fbi->var.pixclock)) * 1000UL;
+	ldb_clk_parent = clk_get_parent(ldb->setting[setting_idx].ldb_di_clk);
+	if (IS_ERR(ldb_clk_parent)) {
+		dev_err(&ldb->pdev->dev, "get ldb di parent clk fail\n");
+		return PTR_ERR(ldb_clk_parent);
+	}
+	if ((ldb->mode == LDB_SPL_DI0) || (ldb->mode == LDB_SPL_DI1))
+		ret = clk_set_rate(ldb_clk_parent, pixel_clk * 7 / 2);
+	else
+		ret = clk_set_rate(ldb_clk_parent, pixel_clk * 7);
+	if (ret < 0) {
+		dev_err(&ldb->pdev->dev, "set ldb parent clk fail:%d\n", ret);
+		return ret;
+	}
+	rounded_pixel_clk = clk_round_rate(ldb->setting[setting_idx].ldb_di_clk,
+						pixel_clk);
+	dev_dbg(&ldb->pdev->dev, "pixel_clk:%d, rounded_pixel_clk:%d\n",
+			pixel_clk, rounded_pixel_clk);
+	ret = clk_set_rate(ldb->setting[setting_idx].ldb_di_clk,
+				rounded_pixel_clk);
+	if (ret < 0) {
+		dev_err(&ldb->pdev->dev, "set ldb di clk fail:%d\n", ret);
+		return ret;
+	}
+	ret = clk_prepare_enable(ldb->setting[setting_idx].ldb_di_clk);
+	if (ret < 0) {
+		dev_err(&ldb->pdev->dev, "enable ldb di clk fail:%d\n", ret);
+		return ret;
+	}
+
+	if (!ldb->setting[setting_idx].clk_en)
+		ldb->setting[setting_idx].clk_en = true;
+
+	return 0;
+}
+
+int ldb_fb_event(struct notifier_block *nb, unsigned long val, void *v)
+{
+	struct ldb_data *ldb = container_of(nb, struct ldb_data, nb);
+	struct fb_event *event = v;
+	struct fb_info *fbi = event->info;
+	int index;
+	uint32_t data;
+
+	index = find_ldb_setting(ldb, fbi);
+	if (index < 0)
+		return 0;
+
+	fbi->mode = (struct fb_videomode *)fb_match_mode(&fbi->var,
+			&fbi->modelist);
+
+	if (!fbi->mode) {
+		dev_warn(&ldb->pdev->dev,
+				"LDB: can not find mode for xres=%d, yres=%d\n",
+				fbi->var.xres, fbi->var.yres);
+		if (ldb->setting[index].clk_en) {
+			clk_disable(ldb->setting[index].ldb_di_clk);
+			ldb->setting[index].clk_en = false;
+			data = readl(ldb->control_reg);
+			data &= ~ldb->setting[index].ch_mask;
+			writel(data, ldb->control_reg);
+		}
+		return 0;
+	}
+
+	switch (val) {
+	case FB_EVENT_BLANK:
+	{
+		if (*((int *)event->data) == FB_BLANK_UNBLANK) {
+			if (!ldb->setting[index].clk_en) {
+				clk_enable(ldb->setting[index].ldb_di_clk);
+				ldb->setting[index].clk_en = true;
+			}
+		} else {
+			if (ldb->setting[index].clk_en) {
+				clk_disable(ldb->setting[index].ldb_di_clk);
+				ldb->setting[index].clk_en = false;
+				data = readl(ldb->control_reg);
+				data &= ~ldb->setting[index].ch_mask;
+				writel(data, ldb->control_reg);
+				dev_dbg(&ldb->pdev->dev,
+					"LDB blank, control reg:0x%x\n",
+						readl(ldb->control_reg));
+			}
+		}
+		break;
+	}
+	case FB_EVENT_SUSPEND:
+		if (ldb->setting[index].clk_en) {
+			clk_disable(ldb->setting[index].ldb_di_clk);
+			ldb->setting[index].clk_en = false;
+		}
+		break;
+	default:
+		break;
+	}
+	return 0;
+}
+
+#define LVDS_MUX_CTL_WIDTH	2
+#define LVDS_MUX_CTL_MASK	3
+#define LVDS0_MUX_CTL_OFFS	6
+#define LVDS1_MUX_CTL_OFFS	8
+#define LVDS0_MUX_CTL_MASK	(LVDS_MUX_CTL_MASK << 6)
+#define LVDS1_MUX_CTL_MASK	(LVDS_MUX_CTL_MASK << 8)
+#define ROUTE_IPU_DI(ipu, di)	(((ipu << 1) | di) & LVDS_MUX_CTL_MASK)
+static int ldb_ipu_ldb_route(int ipu, int di, struct ldb_data *ldb)
+{
+	uint32_t reg;
+	int channel;
+	int shift;
+	int mode = ldb->mode;
+
+	reg = readl(ldb->gpr3_reg);
+	if (mode < LDB_SIN0) {
+		reg &= ~(LVDS0_MUX_CTL_MASK | LVDS1_MUX_CTL_MASK);
+		reg |= (ROUTE_IPU_DI(ipu, di) << LVDS0_MUX_CTL_OFFS) |
+			(ROUTE_IPU_DI(ipu, di) << LVDS1_MUX_CTL_OFFS);
+		dev_dbg(&ldb->pdev->dev,
+			"Dual/Split mode both channels route to IPU%d-DI%d\n",
+			ipu, di);
+	} else if ((mode == LDB_SIN0) || (mode == LDB_SIN1)) {
+		reg &= ~(LVDS0_MUX_CTL_MASK | LVDS1_MUX_CTL_MASK);
+		channel = mode - LDB_SIN0;
+		shift = LVDS0_MUX_CTL_OFFS + channel * LVDS_MUX_CTL_WIDTH;
+		reg |= ROUTE_IPU_DI(ipu, di) << shift;
+		dev_dbg(&ldb->pdev->dev,
+			"Single mode channel %d route to IPU%d-DI%d\n",
+				channel, ipu, di);
+	} else {
+		static bool first = true;
+
+		if (first) {
+			if (mode == LDB_SEP0) {
+				reg &= ~LVDS0_MUX_CTL_MASK;
+				channel = 0;
+			} else {
+				reg &= ~LVDS1_MUX_CTL_MASK;
+				channel = 1;
+			}
+			first = false;
+		} else {
+			if (mode == LDB_SEP0) {
+				reg &= ~LVDS1_MUX_CTL_MASK;
+				channel = 1;
+			} else {
+				reg &= ~LVDS0_MUX_CTL_MASK;
+				channel = 0;
+			}
+		}
+
+		shift = LVDS0_MUX_CTL_OFFS + channel * LVDS_MUX_CTL_WIDTH;
+		reg |= ROUTE_IPU_DI(ipu, di) << shift;
+
+		dev_dbg(&ldb->pdev->dev,
+			"Separate mode channel %d route to IPU%d-DI%d\n",
+			channel, ipu, di);
+	}
+	writel(reg, ldb->gpr3_reg);
+
+	return 0;
+}
+
+static int ldb_disp_init(struct mxc_dispdrv_handle *disp,
+	struct mxc_dispdrv_setting *setting)
+{
+	int ret = 0, i;
+	struct ldb_data *ldb = mxc_dispdrv_getdata(disp);
+	struct fsl_mxc_ldb_platform_data *plat_data = ldb->pdev->dev.platform_data;
+	struct resource *res;
+	uint32_t base_addr;
+	uint32_t reg, setting_idx;
+	uint32_t ch_mask = 0, ch_val = 0;
+	uint32_t ipu_id, disp_id;
+
+	/* if input format not valid, make RGB666 as default*/
+	if (!valid_mode(setting->if_fmt)) {
+		dev_warn(&ldb->pdev->dev, "Input pixel format not valid"
+					" use default RGB666\n");
+		setting->if_fmt = IPU_PIX_FMT_RGB666;
+	}
+
+	if (!ldb->inited) {
+		char di_clk[] = "ipu1_di0_sel";
+		char ldb_clk[] = "ldb_di0";
+		int lvds_channel = 0;
+
+		setting_idx = 0;
+		res = platform_get_resource(ldb->pdev, IORESOURCE_MEM, 0);
+		if (!res) {
+			dev_err(&ldb->pdev->dev, "get iomem fail.\n");
+			return -ENOMEM;
+		}
+
+		base_addr = res->start;
+		ldb->reg = ioremap(base_addr, res->end - res->start + 1);
+		ldb->control_reg = ldb->reg + 2;
+		ldb->gpr3_reg = ldb->reg + 3;
+
+		/* ipu selected by platform data setting */
+		setting->dev_id = plat_data->ipu_id;
+
+		reg = readl(ldb->control_reg);
+
+		/* refrence resistor select */
+		reg &= ~LDB_BGREF_RMODE_MASK;
+		if (plat_data->ext_ref)
+			reg |= LDB_BGREF_RMODE_EXT;
+		else
+			reg |= LDB_BGREF_RMODE_INT;
+
+		/* TODO: now only use SPWG data mapping for both channel */
+		reg &= ~(LDB_BIT_MAP_CH0_MASK | LDB_BIT_MAP_CH1_MASK);
+		reg |= LDB_BIT_MAP_CH0_SPWG | LDB_BIT_MAP_CH1_SPWG;
+
+		/* channel mode setting */
+		reg &= ~(LDB_CH0_MODE_MASK | LDB_CH1_MODE_MASK);
+		reg &= ~(LDB_DATA_WIDTH_CH0_MASK | LDB_DATA_WIDTH_CH1_MASK);
+
+		if (bits_per_pixel(setting->if_fmt) == 24)
+			reg |= LDB_DATA_WIDTH_CH0_24 | LDB_DATA_WIDTH_CH1_24;
+		else
+			reg |= LDB_DATA_WIDTH_CH0_18 | LDB_DATA_WIDTH_CH1_18;
+
+		if (g_ldb_mode >= LDB_SPL_DI0)
+			ldb->mode = g_ldb_mode;
+		else
+			ldb->mode = plat_data->mode;
+
+		if ((ldb->mode == LDB_SIN0) || (ldb->mode == LDB_SIN1)) {
+			ret = ldb->mode - LDB_SIN0;
+			if (plat_data->disp_id != ret) {
+				dev_warn(&ldb->pdev->dev,
+					"change IPU DI%d to IPU DI%d for LDB "
+					"channel%d.\n",
+					plat_data->disp_id, ret, ret);
+				plat_data->disp_id = ret;
+			}
+		} else if (((ldb->mode == LDB_SEP0) || (ldb->mode == LDB_SEP1))
+				&& is_imx6_ldb(plat_data)) {
+			if (plat_data->disp_id == plat_data->sec_disp_id) {
+				dev_err(&ldb->pdev->dev,
+					"For LVDS separate mode,"
+					"two DIs should be different!\n");
+				return -EINVAL;
+			}
+
+			if (((!plat_data->disp_id) && (ldb->mode == LDB_SEP1))
+				|| ((plat_data->disp_id) &&
+					(ldb->mode == LDB_SEP0))) {
+				dev_dbg(&ldb->pdev->dev,
+					"LVDS separate mode:"
+					"swap DI configuration!\n");
+				ipu_id = plat_data->ipu_id;
+				disp_id = plat_data->disp_id;
+				plat_data->ipu_id = plat_data->sec_ipu_id;
+				plat_data->disp_id = plat_data->sec_disp_id;
+				plat_data->sec_ipu_id = ipu_id;
+				plat_data->sec_disp_id = disp_id;
+			}
+		}
+
+		if (ldb->mode == LDB_SPL_DI0) {
+			reg |= LDB_SPLIT_MODE_EN | LDB_CH0_MODE_EN_TO_DI0
+				| LDB_CH1_MODE_EN_TO_DI0;
+			setting->disp_id = 0;
+		} else if (ldb->mode == LDB_SPL_DI1) {
+			reg |= LDB_SPLIT_MODE_EN | LDB_CH0_MODE_EN_TO_DI1
+				| LDB_CH1_MODE_EN_TO_DI1;
+			setting->disp_id = 1;
+		} else if (ldb->mode == LDB_DUL_DI0) {
+			reg &= ~LDB_SPLIT_MODE_EN;
+			reg |= LDB_CH0_MODE_EN_TO_DI0 | LDB_CH1_MODE_EN_TO_DI0;
+			setting->disp_id = 0;
+		} else if (ldb->mode == LDB_DUL_DI1) {
+			reg &= ~LDB_SPLIT_MODE_EN;
+			reg |= LDB_CH0_MODE_EN_TO_DI1 | LDB_CH1_MODE_EN_TO_DI1;
+			setting->disp_id = 1;
+		} else if (ldb->mode == LDB_SIN0) {
+			reg &= ~LDB_SPLIT_MODE_EN;
+			setting->disp_id = plat_data->disp_id;
+			if (setting->disp_id == 0)
+				reg |= LDB_CH0_MODE_EN_TO_DI0;
+			else
+				reg |= LDB_CH0_MODE_EN_TO_DI1;
+			ch_mask = LDB_CH0_MODE_MASK;
+			ch_val = reg & LDB_CH0_MODE_MASK;
+		} else if (ldb->mode == LDB_SIN1) {
+			reg &= ~LDB_SPLIT_MODE_EN;
+			setting->disp_id = plat_data->disp_id;
+			if (setting->disp_id == 0)
+				reg |= LDB_CH1_MODE_EN_TO_DI0;
+			else
+				reg |= LDB_CH1_MODE_EN_TO_DI1;
+			ch_mask = LDB_CH1_MODE_MASK;
+			ch_val = reg & LDB_CH1_MODE_MASK;
+		} else { /* separate mode*/
+			setting->disp_id = plat_data->disp_id;
+
+			/* first output is LVDS0 or LVDS1 */
+			if (ldb->mode == LDB_SEP0)
+				lvds_channel = 0;
+			else
+				lvds_channel = 1;
+
+			reg &= ~LDB_SPLIT_MODE_EN;
+
+			if ((lvds_channel == 0) && (setting->disp_id == 0))
+				reg |= LDB_CH0_MODE_EN_TO_DI0;
+			else if ((lvds_channel == 0) && (setting->disp_id == 1))
+				reg |= LDB_CH0_MODE_EN_TO_DI1;
+			else if ((lvds_channel == 1) && (setting->disp_id == 0))
+				reg |= LDB_CH1_MODE_EN_TO_DI0;
+			else
+				reg |= LDB_CH1_MODE_EN_TO_DI1;
+			ch_mask = lvds_channel ? LDB_CH1_MODE_MASK :
+					LDB_CH0_MODE_MASK;
+			ch_val = reg & ch_mask;
+
+			if (bits_per_pixel(setting->if_fmt) == 24) {
+				if (lvds_channel == 0)
+					reg &= ~LDB_DATA_WIDTH_CH1_24;
+				else
+					reg &= ~LDB_DATA_WIDTH_CH0_24;
+			} else {
+				if (lvds_channel == 0)
+					reg &= ~LDB_DATA_WIDTH_CH1_18;
+				else
+					reg &= ~LDB_DATA_WIDTH_CH0_18;
+			}
+		}
+
+		writel(reg, ldb->control_reg);
+		if (ldb->mode <  LDB_SIN0) {
+			ch_mask = LDB_CH0_MODE_MASK | LDB_CH1_MODE_MASK;
+			ch_val = reg & (LDB_CH0_MODE_MASK | LDB_CH1_MODE_MASK);
+		}
+
+		/* clock setting */
+		if (is_imx6_ldb(plat_data) &&
+			((ldb->mode == LDB_SEP0) || (ldb->mode == LDB_SEP1)))
+			ldb_clk[6] += lvds_channel;
+		else
+			ldb_clk[6] += setting->disp_id;
+		ldb->setting[setting_idx].ldb_di_clk = clk_get(&ldb->pdev->dev,
+								ldb_clk);
+		if (IS_ERR(ldb->setting[setting_idx].ldb_di_clk)) {
+			dev_err(&ldb->pdev->dev, "get ldb clk0 failed\n");
+			iounmap(ldb->reg);
+			return PTR_ERR(ldb->setting[setting_idx].ldb_di_clk);
+		}
+		di_clk[3] += setting->dev_id;
+		di_clk[7] += setting->disp_id;
+		ldb->setting[setting_idx].di_clk = clk_get(&ldb->pdev->dev,
+								di_clk);
+		if (IS_ERR(ldb->setting[setting_idx].di_clk)) {
+			dev_err(&ldb->pdev->dev, "get di clk0 failed\n");
+			iounmap(ldb->reg);
+			return PTR_ERR(ldb->setting[setting_idx].di_clk);
+		}
+
+		dev_dbg(&ldb->pdev->dev, "ldb_clk to di clk: %s -> %s\n", ldb_clk, di_clk);
+
+		/* fb notifier for clk setting */
+		ldb->nb.notifier_call = ldb_fb_event,
+		ret = fb_register_client(&ldb->nb);
+		if (ret < 0) {
+			iounmap(ldb->reg);
+			return ret;
+		}
+
+		ldb->inited = true;
+	} else { /* second time for separate mode */
+		char di_clk[] = "ipu1_di0_sel";
+		char ldb_clk[] = "ldb_di0";
+		int lvds_channel;
+
+		if ((ldb->mode == LDB_SPL_DI0) ||
+			(ldb->mode == LDB_SPL_DI1) ||
+			(ldb->mode == LDB_DUL_DI0) ||
+			(ldb->mode == LDB_DUL_DI1) ||
+			(ldb->mode == LDB_SIN0) ||
+			(ldb->mode == LDB_SIN1)) {
+			dev_err(&ldb->pdev->dev, "for second ldb disp"
+					"ldb mode should in separate mode\n");
+			return -EINVAL;
+		}
+
+		setting_idx = 1;
+		if (is_imx6_ldb(plat_data)) {
+			setting->dev_id = plat_data->sec_ipu_id;
+			setting->disp_id = plat_data->sec_disp_id;
+		} else {
+			setting->dev_id = plat_data->ipu_id;
+			setting->disp_id = !plat_data->disp_id;
+		}
+		if (setting->disp_id == ldb->setting[0].di) {
+			dev_err(&ldb->pdev->dev, "Err: for second ldb disp in"
+				"separate mode, DI should be different!\n");
+			return -EINVAL;
+		}
+
+		/* second output is LVDS0 or LVDS1 */
+		if (ldb->mode == LDB_SEP0)
+			lvds_channel = 1;
+		else
+			lvds_channel = 0;
+
+		reg = readl(ldb->control_reg);
+		if ((lvds_channel == 0) && (setting->disp_id == 0))
+			reg |= LDB_CH0_MODE_EN_TO_DI0;
+		else if ((lvds_channel == 0) && (setting->disp_id == 1))
+			reg |= LDB_CH0_MODE_EN_TO_DI1;
+		else if ((lvds_channel == 1) && (setting->disp_id == 0))
+			reg |= LDB_CH1_MODE_EN_TO_DI0;
+		else
+			reg |= LDB_CH1_MODE_EN_TO_DI1;
+		ch_mask = lvds_channel ?  LDB_CH1_MODE_MASK :
+				LDB_CH0_MODE_MASK;
+		ch_val = reg & ch_mask;
+
+		if (bits_per_pixel(setting->if_fmt) == 24) {
+			if (lvds_channel == 0)
+				reg |= LDB_DATA_WIDTH_CH0_24;
+			else
+				reg |= LDB_DATA_WIDTH_CH1_24;
+		} else {
+			if (lvds_channel == 0)
+				reg |= LDB_DATA_WIDTH_CH0_18;
+			else
+				reg |= LDB_DATA_WIDTH_CH1_18;
+		}
+		writel(reg, ldb->control_reg);
+
+		/* clock setting */
+		if (is_imx6_ldb(plat_data))
+			ldb_clk[6] += lvds_channel;
+		else
+			ldb_clk[6] += setting->disp_id;
+		ldb->setting[setting_idx].ldb_di_clk = clk_get(&ldb->pdev->dev,
+								ldb_clk);
+		if (IS_ERR(ldb->setting[setting_idx].ldb_di_clk)) {
+			dev_err(&ldb->pdev->dev, "get ldb clk1 failed\n");
+			return PTR_ERR(ldb->setting[setting_idx].ldb_di_clk);
+		}
+		di_clk[3] += setting->dev_id;
+		di_clk[7] += setting->disp_id;
+		ldb->setting[setting_idx].di_clk = clk_get(&ldb->pdev->dev,
+								di_clk);
+		if (IS_ERR(ldb->setting[setting_idx].di_clk)) {
+			dev_err(&ldb->pdev->dev, "get di clk1 failed\n");
+			return PTR_ERR(ldb->setting[setting_idx].di_clk);
+		}
+
+		dev_dbg(&ldb->pdev->dev, "ldb_clk to di clk: %s -> %s\n", ldb_clk, di_clk);
+	}
+
+	ldb->setting[setting_idx].ch_mask = ch_mask;
+	ldb->setting[setting_idx].ch_val = ch_val;
+
+	if (is_imx6_ldb(plat_data))
+		ldb_ipu_ldb_route(setting->dev_id, setting->disp_id, ldb);
+
+	/*
+	 * ldb_di0_clk -> ipux_di0_clk
+	 * ldb_di1_clk -> ipux_di1_clk
+	 */
+	ret = clk_set_parent(ldb->setting[setting_idx].di_clk,
+			ldb->setting[setting_idx].ldb_di_clk);
+	if (ret < 0) {
+		dev_err(&ldb->pdev->dev, "fail: set ldb_di clk as"
+			"the parent of ipu_di clk ret:%d!\n", ret);
+		return ret;
+	}
+
+	/* must use spec video mode defined by driver */
+	ret = fb_find_mode(&setting->fbi->var, setting->fbi, setting->dft_mode_str,
+				ldb_modedb, ldb_modedb_sz, NULL, setting->default_bpp);
+	if (ret != 1)
+		fb_videomode_to_var(&setting->fbi->var, &ldb_modedb[0]);
+
+	INIT_LIST_HEAD(&setting->fbi->modelist);
+	for (i = 0; i < ldb_modedb_sz; i++) {
+		struct fb_videomode m;
+		fb_var_to_videomode(&m, &setting->fbi->var);
+		if (fb_mode_is_equal(&m, &ldb_modedb[i])) {
+			fb_add_videomode(&ldb_modedb[i],
+					&setting->fbi->modelist);
+			break;
+		}
+	}
+
+	/* save current ldb setting for fb notifier */
+	ldb->setting[setting_idx].active = true;
+	ldb->setting[setting_idx].ipu = setting->dev_id;
+	ldb->setting[setting_idx].di = setting->disp_id;
+
+	return ret;
+}
+
+static void ldb_disp_deinit(struct mxc_dispdrv_handle *disp)
+{
+	struct ldb_data *ldb = mxc_dispdrv_getdata(disp);
+	int i;
+
+	writel(0, ldb->control_reg);
+
+	for (i = 0; i < 2; i++) {
+		clk_disable(ldb->setting[i].ldb_di_clk);
+		clk_put(ldb->setting[i].ldb_di_clk);
+	}
+
+	fb_unregister_client(&ldb->nb);
+
+	iounmap(ldb->reg);
+}
+
+static struct mxc_dispdrv_driver ldb_drv = {
+	.name 	= DISPDRV_LDB,
+	.init 	= ldb_disp_init,
+	.deinit	= ldb_disp_deinit,
+	.setup = ldb_disp_setup,
+};
+
+static int ldb_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct ldb_data *ldb = dev_get_drvdata(&pdev->dev);
+	uint32_t	data;
+
+	if (!ldb->inited)
+		return 0;
+	data = readl(ldb->control_reg);
+	ldb->control_reg_data = data;
+	data &= ~(LDB_CH0_MODE_MASK | LDB_CH1_MODE_MASK);
+	writel(data, ldb->control_reg);
+
+	return 0;
+}
+
+static int ldb_resume(struct platform_device *pdev)
+{
+	struct ldb_data *ldb = dev_get_drvdata(&pdev->dev);
+
+	if (!ldb->inited)
+		return 0;
+	writel(ldb->control_reg_data, ldb->control_reg);
+
+	return 0;
+}
+
+static struct platform_device_id imx_ldb_devtype[] = {
+	{
+		.name = "ldb-imx6",
+		.driver_data = LDB_IMX6,
+	}, {
+		/* sentinel */
+	}
+};
+
+static const struct of_device_id imx_ldb_dt_ids[] = {
+	{ .compatible = "fsl,imx6q-ldb", .data = &imx_ldb_devtype[IMX6_LDB],},
+	{ /* sentinel */ }
+};
+
+/*!
+ * This function is called by the driver framework to initialize the LDB
+ * device.
+ *
+ * @param	dev	The device structure for the LDB passed in by the
+ *			driver framework.
+ *
+ * @return      Returns 0 on success or negative error code on error
+ */
+static int ldb_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct ldb_data *ldb;
+	struct fsl_mxc_ldb_platform_data *plat_data;
+	const struct of_device_id *of_id =
+			of_match_device(imx_ldb_dt_ids, &pdev->dev);
+
+	dev_dbg(&pdev->dev, "%s enter\n", __func__);
+	ldb = devm_kzalloc(&pdev->dev, sizeof(struct ldb_data), GFP_KERNEL);
+	if (!ldb)
+		return -ENOMEM;
+
+	plat_data = devm_kzalloc(&pdev->dev,
+				sizeof(struct fsl_mxc_ldb_platform_data),
+				GFP_KERNEL);
+	if (!plat_data)
+		return -ENOMEM;
+	pdev->dev.platform_data = plat_data;
+	if (of_id)
+		pdev->id_entry = of_id->data;
+	plat_data->devtype = pdev->id_entry->driver_data;
+
+	ret = ldb_get_of_property(pdev, plat_data);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "get ldb of property fail\n");
+		return ret;
+	}
+
+	ldb->pdev = pdev;
+	ldb->disp_ldb = mxc_dispdrv_register(&ldb_drv);
+	mxc_dispdrv_setdata(ldb->disp_ldb, ldb);
+
+	dev_set_drvdata(&pdev->dev, ldb);
+
+	dev_dbg(&pdev->dev, "%s exit\n", __func__);
+	return ret;
+}
+
+static int ldb_remove(struct platform_device *pdev)
+{
+	struct ldb_data *ldb = dev_get_drvdata(&pdev->dev);
+
+	if (!ldb->inited)
+		return 0;
+	mxc_dispdrv_puthandle(ldb->disp_ldb);
+	mxc_dispdrv_unregister(ldb->disp_ldb);
+	return 0;
+}
+
+static struct platform_driver mxcldb_driver = {
+	.driver = {
+		.name = "mxc_ldb",
+		.of_match_table	= imx_ldb_dt_ids,
+	},
+	.probe = ldb_probe,
+	.remove = ldb_remove,
+	.suspend = ldb_suspend,
+	.resume = ldb_resume,
+};
+
+static int __init ldb_init(void)
+{
+	return platform_driver_register(&mxcldb_driver);
+}
+
+static void __exit ldb_uninit(void)
+{
+	platform_driver_unregister(&mxcldb_driver);
+}
+
+module_init(ldb_init);
+module_exit(ldb_uninit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC LDB driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/mxc/mxc_dispdrv.c b/drivers/video/mxc/mxc_dispdrv.c
new file mode 100644
index 000000000000..2e6da99c4f68
--- /dev/null
+++ b/drivers/video/mxc/mxc_dispdrv.c
@@ -0,0 +1,148 @@
+/*
+ * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mxc_dispdrv.c
+ * @brief mxc display driver framework.
+ *
+ * A display device driver could call mxc_dispdrv_register(drv) in its dev_probe() function.
+ * Move all dev_probe() things into mxc_dispdrv_driver->init(), init() function should init
+ * and feedback setting;
+ * Move all dev_remove() things into mxc_dispdrv_driver->deinit();
+ * Move all dev_suspend() things into fb_notifier for SUSPEND, if there is;
+ * Move all dev_resume() things into fb_notifier for RESUME, if there is;
+ *
+ * ipuv3 fb driver could call mxc_dispdrv_gethandle(name, setting) before a fb
+ * need be added, with fbi param passing by setting, after
+ * mxc_dispdrv_gethandle() return, FB driver should get the basic setting
+ * about fbi info and ipuv3-hw (ipu_id and disp_id).
+ *
+ * @ingroup Framebuffer
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/string.h>
+#include "mxc_dispdrv.h"
+
+static LIST_HEAD(dispdrv_list);
+static DEFINE_MUTEX(dispdrv_lock);
+
+struct mxc_dispdrv_entry {
+	/* Note: drv always the first element */
+	struct mxc_dispdrv_driver *drv;
+	bool active;
+	void *priv;
+	struct list_head list;
+};
+
+struct mxc_dispdrv_handle *mxc_dispdrv_register(struct mxc_dispdrv_driver *drv)
+{
+	struct mxc_dispdrv_entry *new;
+
+	mutex_lock(&dispdrv_lock);
+
+	new = kzalloc(sizeof(struct mxc_dispdrv_entry), GFP_KERNEL);
+	if (!new) {
+		mutex_unlock(&dispdrv_lock);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	new->drv = drv;
+	list_add_tail(&new->list, &dispdrv_list);
+
+	mutex_unlock(&dispdrv_lock);
+
+	return (struct mxc_dispdrv_handle *)new;
+}
+EXPORT_SYMBOL_GPL(mxc_dispdrv_register);
+
+int mxc_dispdrv_unregister(struct mxc_dispdrv_handle *handle)
+{
+	struct mxc_dispdrv_entry *entry = (struct mxc_dispdrv_entry *)handle;
+
+	if (entry) {
+		mutex_lock(&dispdrv_lock);
+		list_del(&entry->list);
+		mutex_unlock(&dispdrv_lock);
+		kfree(entry);
+		return 0;
+	} else
+		return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(mxc_dispdrv_unregister);
+
+struct mxc_dispdrv_handle *mxc_dispdrv_gethandle(char *name,
+	struct mxc_dispdrv_setting *setting)
+{
+	int ret, found = 0;
+	struct mxc_dispdrv_entry *entry;
+
+	mutex_lock(&dispdrv_lock);
+	list_for_each_entry(entry, &dispdrv_list, list) {
+		if (!strcmp(entry->drv->name, name) && (entry->drv->init)) {
+			ret = entry->drv->init((struct mxc_dispdrv_handle *)
+				entry, setting);
+			if (ret >= 0) {
+				entry->active = true;
+				found = 1;
+				break;
+			}
+		}
+	}
+	mutex_unlock(&dispdrv_lock);
+
+	return found ? (struct mxc_dispdrv_handle *)entry:ERR_PTR(-ENODEV);
+}
+EXPORT_SYMBOL_GPL(mxc_dispdrv_gethandle);
+
+void mxc_dispdrv_puthandle(struct mxc_dispdrv_handle *handle)
+{
+	struct mxc_dispdrv_entry *entry = (struct mxc_dispdrv_entry *)handle;
+
+	mutex_lock(&dispdrv_lock);
+	if (entry && entry->active && entry->drv->deinit) {
+		entry->drv->deinit(handle);
+		entry->active = false;
+	}
+	mutex_unlock(&dispdrv_lock);
+
+}
+EXPORT_SYMBOL_GPL(mxc_dispdrv_puthandle);
+
+int mxc_dispdrv_setdata(struct mxc_dispdrv_handle *handle, void *data)
+{
+	struct mxc_dispdrv_entry *entry = (struct mxc_dispdrv_entry *)handle;
+
+	if (entry) {
+		entry->priv = data;
+		return 0;
+	} else
+		return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(mxc_dispdrv_setdata);
+
+void *mxc_dispdrv_getdata(struct mxc_dispdrv_handle *handle)
+{
+	struct mxc_dispdrv_entry *entry = (struct mxc_dispdrv_entry *)handle;
+
+	if (entry) {
+		return entry->priv;
+	} else
+		return ERR_PTR(-EINVAL);
+}
+EXPORT_SYMBOL_GPL(mxc_dispdrv_getdata);
diff --git a/drivers/video/mxc/mxc_dispdrv.h b/drivers/video/mxc/mxc_dispdrv.h
new file mode 100644
index 000000000000..d6dcf19dc404
--- /dev/null
+++ b/drivers/video/mxc/mxc_dispdrv.h
@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef __MXC_DISPDRV_H__
+#define __MXC_DISPDRV_H__
+#include <linux/fb.h>
+
+struct mxc_dispdrv_handle {
+	struct mxc_dispdrv_driver *drv;
+};
+
+struct mxc_dispdrv_setting {
+	/*input-feedback parameter*/
+	struct fb_info *fbi;
+	int if_fmt;
+	int default_bpp;
+	char *dft_mode_str;
+
+	/*feedback parameter*/
+	int dev_id;
+	int disp_id;
+};
+
+struct mxc_dispdrv_driver {
+	const char *name;
+	int (*init) (struct mxc_dispdrv_handle *, struct mxc_dispdrv_setting *);
+	void (*deinit) (struct mxc_dispdrv_handle *);
+	/* display driver enable function for extension */
+	int (*enable) (struct mxc_dispdrv_handle *);
+	/* display driver disable function, called at early part of fb_blank */
+	void (*disable) (struct mxc_dispdrv_handle *);
+	/* display driver setup function, called at early part of fb_set_par */
+	int (*setup) (struct mxc_dispdrv_handle *, struct fb_info *fbi);
+};
+
+struct mxc_dispdrv_handle *mxc_dispdrv_register(struct mxc_dispdrv_driver *drv);
+int mxc_dispdrv_unregister(struct mxc_dispdrv_handle *handle);
+struct mxc_dispdrv_handle *mxc_dispdrv_gethandle(char *name,
+	struct mxc_dispdrv_setting *setting);
+void mxc_dispdrv_puthandle(struct mxc_dispdrv_handle *handle);
+int mxc_dispdrv_setdata(struct mxc_dispdrv_handle *handle, void *data);
+void *mxc_dispdrv_getdata(struct mxc_dispdrv_handle *handle);
+#endif
diff --git a/drivers/video/mxc/mxc_ipuv3_fb.c b/drivers/video/mxc/mxc_ipuv3_fb.c
new file mode 100644
index 000000000000..99bc724fd140
--- /dev/null
+++ b/drivers/video/mxc/mxc_ipuv3_fb.c
@@ -0,0 +1,2549 @@
+/*
+ * Copyright 2004-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup Framebuffer Framebuffer Driver for SDC and ADC.
+ */
+
+/*!
+ * @file mxcfb.c
+ *
+ * @brief MXC Frame buffer driver for SDC
+ *
+ * @ingroup Framebuffer
+ */
+
+/*!
+ * Include files
+ */
+#include <linux/clk.h>
+#include <linux/console.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/errno.h>
+#include <linux/fb.h>
+#include <linux/fsl_devices.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/ipu.h>
+#include <linux/ipu-v3.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mxcfb.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/uaccess.h>
+
+#include "mxc_dispdrv.h"
+
+/*
+ * Driver name
+ */
+#define MXCFB_NAME      "mxc_sdc_fb"
+
+/* Display port number */
+#define MXCFB_PORT_NUM	2
+/*!
+ * Structure containing the MXC specific framebuffer information.
+ */
+struct mxcfb_info {
+	int default_bpp;
+	int cur_blank;
+	int next_blank;
+	ipu_channel_t ipu_ch;
+	int ipu_id;
+	int ipu_di;
+	u32 ipu_di_pix_fmt;
+	bool ipu_int_clk;
+	bool overlay;
+	bool alpha_chan_en;
+	bool late_init;
+	bool first_set_par;
+	dma_addr_t alpha_phy_addr0;
+	dma_addr_t alpha_phy_addr1;
+	void *alpha_virt_addr0;
+	void *alpha_virt_addr1;
+	uint32_t alpha_mem_len;
+	uint32_t ipu_ch_irq;
+	uint32_t ipu_ch_nf_irq;
+	uint32_t ipu_alp_ch_irq;
+	uint32_t cur_ipu_buf;
+	uint32_t cur_ipu_alpha_buf;
+
+	u32 pseudo_palette[16];
+
+	bool mode_found;
+	struct completion flip_complete;
+	struct completion alpha_flip_complete;
+	struct completion vsync_complete;
+
+	void *ipu;
+	struct fb_info *ovfbi;
+
+	struct mxc_dispdrv_handle *dispdrv;
+
+	struct fb_var_screeninfo cur_var;
+};
+
+struct mxcfb_pfmt {
+	u32 fb_pix_fmt;
+	int bpp;
+	struct fb_bitfield red;
+	struct fb_bitfield green;
+	struct fb_bitfield blue;
+	struct fb_bitfield transp;
+};
+
+static const struct mxcfb_pfmt mxcfb_pfmts[] = {
+	/*     pixel         bpp    red         green        blue      transp */
+	{IPU_PIX_FMT_RGB565, 16, {11, 5, 0}, { 5, 6, 0}, { 0, 5, 0}, { 0, 0, 0} },
+	{IPU_PIX_FMT_RGB24,  24, { 0, 8, 0}, { 8, 8, 0}, {16, 8, 0}, { 0, 0, 0} },
+	{IPU_PIX_FMT_BGR24,  24, {16, 8, 0}, { 8, 8, 0}, { 0, 8, 0}, { 0, 0, 0} },
+	{IPU_PIX_FMT_RGB32,  32, { 0, 8, 0}, { 8, 8, 0}, {16, 8, 0}, {24, 8, 0} },
+	{IPU_PIX_FMT_BGR32,  32, {16, 8, 0}, { 8, 8, 0}, { 0, 8, 0}, {24, 8, 0} },
+	{IPU_PIX_FMT_ABGR32, 32, {24, 8, 0}, {16, 8, 0}, { 8, 8, 0}, { 0, 8, 0} },
+};
+
+struct mxcfb_alloc_list {
+	struct list_head list;
+	dma_addr_t phy_addr;
+	void *cpu_addr;
+	u32 size;
+};
+
+enum {
+	BOTH_ON,
+	SRC_ON,
+	TGT_ON,
+	BOTH_OFF
+};
+
+static bool g_dp_in_use[2];
+LIST_HEAD(fb_alloc_list);
+
+/* Return default standard(RGB) pixel format */
+static uint32_t bpp_to_pixfmt(int bpp)
+{
+	uint32_t pixfmt = 0;
+
+	switch (bpp) {
+	case 24:
+		pixfmt = IPU_PIX_FMT_BGR24;
+		break;
+	case 32:
+		pixfmt = IPU_PIX_FMT_BGR32;
+		break;
+	case 16:
+		pixfmt = IPU_PIX_FMT_RGB565;
+		break;
+	}
+	return pixfmt;
+}
+
+static inline int bitfield_is_equal(struct fb_bitfield f1,
+				    struct fb_bitfield f2)
+{
+	return !memcmp(&f1, &f2, sizeof(f1));
+}
+
+static int pixfmt_to_var(uint32_t pixfmt, struct fb_var_screeninfo *var)
+{
+	int i, ret = -1;
+
+	for (i = 0; i < ARRAY_SIZE(mxcfb_pfmts); i++) {
+		if (pixfmt == mxcfb_pfmts[i].fb_pix_fmt) {
+			var->red    = mxcfb_pfmts[i].red;
+			var->green  = mxcfb_pfmts[i].green;
+			var->blue   = mxcfb_pfmts[i].blue;
+			var->transp = mxcfb_pfmts[i].transp;
+			var->bits_per_pixel = mxcfb_pfmts[i].bpp;
+			ret = 0;
+			break;
+		}
+	}
+	return ret;
+}
+
+static int bpp_to_var(int bpp, struct fb_var_screeninfo *var)
+{
+	uint32_t pixfmt = 0;
+
+	pixfmt = bpp_to_pixfmt(bpp);
+	if (pixfmt)
+		return pixfmt_to_var(pixfmt, var);
+	else
+		return -1;
+}
+
+static int check_var_pixfmt(struct fb_var_screeninfo *var)
+{
+	int i, ret = -1;
+
+	for (i = 0; i < ARRAY_SIZE(mxcfb_pfmts); i++) {
+		if (bitfield_is_equal(var->red, mxcfb_pfmts[i].red) &&
+		    bitfield_is_equal(var->green, mxcfb_pfmts[i].green) &&
+		    bitfield_is_equal(var->blue, mxcfb_pfmts[i].blue) &&
+		    bitfield_is_equal(var->transp, mxcfb_pfmts[i].transp) &&
+		    var->bits_per_pixel == mxcfb_pfmts[i].bpp) {
+			ret = 0;
+			break;
+		}
+	}
+	return ret;
+}
+
+static uint32_t fbi_to_pixfmt(struct fb_info *fbi)
+{
+	int i;
+	uint32_t pixfmt = 0;
+
+	if (fbi->var.nonstd)
+		return fbi->var.nonstd;
+
+	for (i = 0; i < ARRAY_SIZE(mxcfb_pfmts); i++) {
+		if (bitfield_is_equal(fbi->var.red, mxcfb_pfmts[i].red) &&
+		    bitfield_is_equal(fbi->var.green, mxcfb_pfmts[i].green) &&
+		    bitfield_is_equal(fbi->var.blue, mxcfb_pfmts[i].blue) &&
+		    bitfield_is_equal(fbi->var.transp, mxcfb_pfmts[i].transp)) {
+			pixfmt = mxcfb_pfmts[i].fb_pix_fmt;
+			break;
+		}
+	}
+
+	if (pixfmt == 0)
+		dev_err(fbi->device, "cannot get pixel format\n");
+
+	return pixfmt;
+}
+
+static struct fb_info *found_registered_fb(ipu_channel_t ipu_ch, int ipu_id)
+{
+	int i;
+	struct mxcfb_info *mxc_fbi;
+	struct fb_info *fbi = NULL;
+
+	for (i = 0; i < num_registered_fb; i++) {
+		mxc_fbi =
+			((struct mxcfb_info *)(registered_fb[i]->par));
+
+		if ((mxc_fbi->ipu_ch == ipu_ch) &&
+			(mxc_fbi->ipu_id == ipu_id)) {
+			fbi = registered_fb[i];
+			break;
+		}
+	}
+	return fbi;
+}
+
+static irqreturn_t mxcfb_irq_handler(int irq, void *dev_id);
+static irqreturn_t mxcfb_nf_irq_handler(int irq, void *dev_id);
+static int mxcfb_blank(int blank, struct fb_info *info);
+static int mxcfb_map_video_memory(struct fb_info *fbi);
+static int mxcfb_unmap_video_memory(struct fb_info *fbi);
+
+/*
+ * Set fixed framebuffer parameters based on variable settings.
+ *
+ * @param       info     framebuffer information pointer
+ */
+static int mxcfb_set_fix(struct fb_info *info)
+{
+	struct fb_fix_screeninfo *fix = &info->fix;
+	struct fb_var_screeninfo *var = &info->var;
+
+	fix->line_length = var->xres_virtual * var->bits_per_pixel / 8;
+
+	fix->type = FB_TYPE_PACKED_PIXELS;
+	fix->accel = FB_ACCEL_NONE;
+	fix->visual = FB_VISUAL_TRUECOLOR;
+	fix->xpanstep = 1;
+	fix->ywrapstep = 1;
+	fix->ypanstep = 1;
+
+	return 0;
+}
+
+static int _setup_disp_channel1(struct fb_info *fbi)
+{
+	ipu_channel_params_t params;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+
+	memset(&params, 0, sizeof(params));
+
+	if (mxc_fbi->ipu_ch == MEM_DC_SYNC) {
+		params.mem_dc_sync.di = mxc_fbi->ipu_di;
+		if (fbi->var.vmode & FB_VMODE_INTERLACED)
+			params.mem_dc_sync.interlaced = true;
+		params.mem_dc_sync.out_pixel_fmt = mxc_fbi->ipu_di_pix_fmt;
+		params.mem_dc_sync.in_pixel_fmt = fbi_to_pixfmt(fbi);
+	} else {
+		params.mem_dp_bg_sync.di = mxc_fbi->ipu_di;
+		if (fbi->var.vmode & FB_VMODE_INTERLACED)
+			params.mem_dp_bg_sync.interlaced = true;
+		params.mem_dp_bg_sync.out_pixel_fmt = mxc_fbi->ipu_di_pix_fmt;
+		params.mem_dp_bg_sync.in_pixel_fmt = fbi_to_pixfmt(fbi);
+		if (mxc_fbi->alpha_chan_en)
+			params.mem_dp_bg_sync.alpha_chan_en = true;
+	}
+	ipu_init_channel(mxc_fbi->ipu, mxc_fbi->ipu_ch, &params);
+
+	return 0;
+}
+
+static int _setup_disp_channel2(struct fb_info *fbi)
+{
+	int retval = 0;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+	int fb_stride;
+	unsigned long base;
+	unsigned int fr_xoff, fr_yoff, fr_w, fr_h;
+
+	switch (fbi_to_pixfmt(fbi)) {
+	case IPU_PIX_FMT_YUV420P2:
+	case IPU_PIX_FMT_YVU420P:
+	case IPU_PIX_FMT_NV12:
+	case IPU_PIX_FMT_YUV422P:
+	case IPU_PIX_FMT_YVU422P:
+	case IPU_PIX_FMT_YUV420P:
+	case IPU_PIX_FMT_YUV444P:
+		fb_stride = fbi->var.xres_virtual;
+		break;
+	default:
+		fb_stride = fbi->fix.line_length;
+	}
+
+	base = fbi->fix.smem_start;
+	fr_xoff = fbi->var.xoffset;
+	fr_w = fbi->var.xres_virtual;
+	if (!(fbi->var.vmode & FB_VMODE_YWRAP)) {
+		dev_dbg(fbi->device, "Y wrap disabled\n");
+		fr_yoff = fbi->var.yoffset % fbi->var.yres;
+		fr_h = fbi->var.yres;
+		base += fbi->fix.line_length * fbi->var.yres *
+			(fbi->var.yoffset / fbi->var.yres);
+	} else {
+		dev_dbg(fbi->device, "Y wrap enabled\n");
+		fr_yoff = fbi->var.yoffset;
+		fr_h = fbi->var.yres_virtual;
+	}
+	base += fr_yoff * fb_stride + fr_xoff;
+
+	mxc_fbi->cur_ipu_buf = 2;
+	init_completion(&mxc_fbi->flip_complete);
+	/*
+	 * We don't need to wait for vsync at the first time
+	 * we do pan display after fb is initialized, as IPU will
+	 * switch to the newly selected buffer automatically,
+	 * so we call complete() for both mxc_fbi->flip_complete
+	 * and mxc_fbi->alpha_flip_complete.
+	 */
+	complete(&mxc_fbi->flip_complete);
+	if (mxc_fbi->alpha_chan_en) {
+		mxc_fbi->cur_ipu_alpha_buf = 1;
+		init_completion(&mxc_fbi->alpha_flip_complete);
+		complete(&mxc_fbi->alpha_flip_complete);
+	}
+
+	retval = ipu_init_channel_buffer(mxc_fbi->ipu,
+					 mxc_fbi->ipu_ch, IPU_INPUT_BUFFER,
+					 fbi_to_pixfmt(fbi),
+					 fbi->var.xres, fbi->var.yres,
+					 fb_stride,
+					 fbi->var.rotate,
+					 base,
+					 base,
+					 fbi->var.accel_flags &
+						FB_ACCEL_DOUBLE_FLAG ? 0 : base,
+					 0, 0);
+	if (retval) {
+		dev_err(fbi->device,
+			"ipu_init_channel_buffer error %d\n", retval);
+		return retval;
+	}
+
+	/* update u/v offset */
+	ipu_update_channel_offset(mxc_fbi->ipu, mxc_fbi->ipu_ch,
+			IPU_INPUT_BUFFER,
+			fbi_to_pixfmt(fbi),
+			fr_w,
+			fr_h,
+			fr_w,
+			0, 0,
+			fr_yoff,
+			fr_xoff);
+
+	if (mxc_fbi->alpha_chan_en) {
+		retval = ipu_init_channel_buffer(mxc_fbi->ipu,
+						 mxc_fbi->ipu_ch,
+						 IPU_ALPHA_IN_BUFFER,
+						 IPU_PIX_FMT_GENERIC,
+						 fbi->var.xres, fbi->var.yres,
+						 fbi->var.xres,
+						 fbi->var.rotate,
+						 mxc_fbi->alpha_phy_addr1,
+						 mxc_fbi->alpha_phy_addr0,
+						 0,
+						 0, 0);
+		if (retval) {
+			dev_err(fbi->device,
+				"ipu_init_channel_buffer error %d\n", retval);
+			return retval;
+		}
+	}
+
+	return retval;
+}
+
+static bool mxcfb_need_to_set_par(struct fb_info *fbi)
+{
+	struct mxcfb_info *mxc_fbi = fbi->par;
+
+	if ((fbi->var.activate & FB_ACTIVATE_FORCE) &&
+	    (fbi->var.activate & FB_ACTIVATE_MASK) == FB_ACTIVATE_NOW)
+		return true;
+
+	/*
+	 * Ignore xoffset and yoffset update,
+	 * because pan display handles this case.
+	 */
+	mxc_fbi->cur_var.xoffset = fbi->var.xoffset;
+	mxc_fbi->cur_var.yoffset = fbi->var.yoffset;
+
+	return !!memcmp(&mxc_fbi->cur_var, &fbi->var,
+			sizeof(struct fb_var_screeninfo));
+}
+
+/*
+ * Set framebuffer parameters and change the operating mode.
+ *
+ * @param       info     framebuffer information pointer
+ */
+static int mxcfb_set_par(struct fb_info *fbi)
+{
+	int retval = 0;
+	u32 mem_len, alpha_mem_len;
+	ipu_di_signal_cfg_t sig_cfg;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+
+	int16_t ov_pos_x = 0, ov_pos_y = 0;
+	int ov_pos_ret = 0;
+	struct mxcfb_info *mxc_fbi_fg = NULL;
+	bool ovfbi_enable = false;
+
+	if (mxc_fbi->ovfbi)
+		mxc_fbi_fg = (struct mxcfb_info *)mxc_fbi->ovfbi->par;
+
+	if (mxc_fbi->ovfbi && mxc_fbi_fg)
+		if (mxc_fbi_fg->next_blank == FB_BLANK_UNBLANK)
+			ovfbi_enable = true;
+
+	if (!mxcfb_need_to_set_par(fbi))
+		return 0;
+
+	dev_dbg(fbi->device, "Reconfiguring framebuffer\n");
+
+	if (fbi->var.xres == 0 || fbi->var.yres == 0)
+		return 0;
+
+	if (ovfbi_enable) {
+		ov_pos_ret = ipu_disp_get_window_pos(
+						mxc_fbi_fg->ipu, mxc_fbi_fg->ipu_ch,
+						&ov_pos_x, &ov_pos_y);
+		if (ov_pos_ret < 0)
+			dev_err(fbi->device, "Get overlay pos failed, dispdrv:%s.\n",
+					mxc_fbi->dispdrv->drv->name);
+
+		ipu_clear_irq(mxc_fbi_fg->ipu, mxc_fbi_fg->ipu_ch_irq);
+		ipu_disable_irq(mxc_fbi_fg->ipu, mxc_fbi_fg->ipu_ch_irq);
+		ipu_clear_irq(mxc_fbi_fg->ipu, mxc_fbi_fg->ipu_ch_nf_irq);
+		ipu_disable_irq(mxc_fbi_fg->ipu, mxc_fbi_fg->ipu_ch_nf_irq);
+		ipu_disable_channel(mxc_fbi_fg->ipu, mxc_fbi_fg->ipu_ch, true);
+		ipu_uninit_channel(mxc_fbi_fg->ipu, mxc_fbi_fg->ipu_ch);
+	}
+
+	ipu_clear_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_irq);
+	ipu_disable_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_irq);
+	ipu_clear_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_nf_irq);
+	ipu_disable_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_nf_irq);
+	ipu_disable_channel(mxc_fbi->ipu, mxc_fbi->ipu_ch, true);
+	ipu_uninit_channel(mxc_fbi->ipu, mxc_fbi->ipu_ch);
+
+	/*
+	 * Disable IPU hsp clock if it is enabled for an
+	 * additional time in ipu common driver.
+	 */
+	if (mxc_fbi->first_set_par && mxc_fbi->late_init)
+		ipu_disable_hsp_clk(mxc_fbi->ipu);
+
+	mxcfb_set_fix(fbi);
+
+	mem_len = fbi->var.yres_virtual * fbi->fix.line_length;
+	if (!fbi->fix.smem_start || (mem_len > fbi->fix.smem_len)) {
+		if (fbi->fix.smem_start)
+			mxcfb_unmap_video_memory(fbi);
+
+		if (mxcfb_map_video_memory(fbi) < 0)
+			return -ENOMEM;
+	}
+
+	if (mxc_fbi->first_set_par) {
+		/*
+		 * Clear the screen in case uboot fb pixel format is not
+		 * the same to kernel fb pixel format.
+		 */
+		if (mxc_fbi->late_init)
+			memset((char *)fbi->screen_base, 0, fbi->fix.smem_len);
+
+		mxc_fbi->first_set_par = false;
+	}
+
+	if (mxc_fbi->alpha_chan_en) {
+		alpha_mem_len = fbi->var.xres * fbi->var.yres;
+		if ((!mxc_fbi->alpha_phy_addr0 && !mxc_fbi->alpha_phy_addr1) ||
+		    (alpha_mem_len > mxc_fbi->alpha_mem_len)) {
+			if (mxc_fbi->alpha_phy_addr0)
+				dma_free_coherent(fbi->device,
+						  mxc_fbi->alpha_mem_len,
+						  mxc_fbi->alpha_virt_addr0,
+						  mxc_fbi->alpha_phy_addr0);
+			if (mxc_fbi->alpha_phy_addr1)
+				dma_free_coherent(fbi->device,
+						  mxc_fbi->alpha_mem_len,
+						  mxc_fbi->alpha_virt_addr1,
+						  mxc_fbi->alpha_phy_addr1);
+
+			mxc_fbi->alpha_virt_addr0 =
+					dma_alloc_coherent(fbi->device,
+						  alpha_mem_len,
+						  &mxc_fbi->alpha_phy_addr0,
+						  GFP_DMA | GFP_KERNEL);
+
+			mxc_fbi->alpha_virt_addr1 =
+					dma_alloc_coherent(fbi->device,
+						  alpha_mem_len,
+						  &mxc_fbi->alpha_phy_addr1,
+						  GFP_DMA | GFP_KERNEL);
+			if (mxc_fbi->alpha_virt_addr0 == NULL ||
+			    mxc_fbi->alpha_virt_addr1 == NULL) {
+				dev_err(fbi->device, "mxcfb: dma alloc for"
+					" alpha buffer failed.\n");
+				if (mxc_fbi->alpha_virt_addr0)
+					dma_free_coherent(fbi->device,
+						  mxc_fbi->alpha_mem_len,
+						  mxc_fbi->alpha_virt_addr0,
+						  mxc_fbi->alpha_phy_addr0);
+				if (mxc_fbi->alpha_virt_addr1)
+					dma_free_coherent(fbi->device,
+						  mxc_fbi->alpha_mem_len,
+						  mxc_fbi->alpha_virt_addr1,
+						  mxc_fbi->alpha_phy_addr1);
+				return -ENOMEM;
+			}
+			mxc_fbi->alpha_mem_len = alpha_mem_len;
+		}
+	}
+
+	if (mxc_fbi->next_blank != FB_BLANK_UNBLANK)
+		return retval;
+
+	if (mxc_fbi->dispdrv && mxc_fbi->dispdrv->drv->setup) {
+		retval = mxc_fbi->dispdrv->drv->setup(mxc_fbi->dispdrv, fbi);
+		if (retval < 0) {
+			dev_err(fbi->device, "setup error, dispdrv:%s.\n",
+					mxc_fbi->dispdrv->drv->name);
+			return -EINVAL;
+		}
+	}
+
+	_setup_disp_channel1(fbi);
+	if (ovfbi_enable)
+		_setup_disp_channel1(mxc_fbi->ovfbi);
+
+	if (!mxc_fbi->overlay) {
+		uint32_t out_pixel_fmt;
+
+		memset(&sig_cfg, 0, sizeof(sig_cfg));
+		if (fbi->var.vmode & FB_VMODE_INTERLACED)
+			sig_cfg.interlaced = true;
+		out_pixel_fmt = mxc_fbi->ipu_di_pix_fmt;
+		if (fbi->var.vmode & FB_VMODE_ODD_FLD_FIRST) /* PAL */
+			sig_cfg.odd_field_first = true;
+		if (mxc_fbi->ipu_int_clk)
+			sig_cfg.int_clk = true;
+		if (fbi->var.sync & FB_SYNC_HOR_HIGH_ACT)
+			sig_cfg.Hsync_pol = true;
+		if (fbi->var.sync & FB_SYNC_VERT_HIGH_ACT)
+			sig_cfg.Vsync_pol = true;
+		if (!(fbi->var.sync & FB_SYNC_CLK_LAT_FALL))
+			sig_cfg.clk_pol = true;
+		if (fbi->var.sync & FB_SYNC_DATA_INVERT)
+			sig_cfg.data_pol = true;
+		if (!(fbi->var.sync & FB_SYNC_OE_LOW_ACT))
+			sig_cfg.enable_pol = true;
+		if (fbi->var.sync & FB_SYNC_CLK_IDLE_EN)
+			sig_cfg.clkidle_en = true;
+
+		dev_dbg(fbi->device, "pixclock = %ul Hz\n",
+			(u32) (PICOS2KHZ(fbi->var.pixclock) * 1000UL));
+
+		if (ipu_init_sync_panel(mxc_fbi->ipu, mxc_fbi->ipu_di,
+					(PICOS2KHZ(fbi->var.pixclock)) * 1000UL,
+					fbi->var.xres, fbi->var.yres,
+					out_pixel_fmt,
+					fbi->var.left_margin,
+					fbi->var.hsync_len,
+					fbi->var.right_margin,
+					fbi->var.upper_margin,
+					fbi->var.vsync_len,
+					fbi->var.lower_margin,
+					0, sig_cfg) != 0) {
+			dev_err(fbi->device,
+				"mxcfb: Error initializing panel.\n");
+			return -EINVAL;
+		}
+
+		fbi->mode =
+		    (struct fb_videomode *)fb_match_mode(&fbi->var,
+							 &fbi->modelist);
+
+		ipu_disp_set_window_pos(mxc_fbi->ipu, mxc_fbi->ipu_ch, 0, 0);
+	}
+
+	retval = _setup_disp_channel2(fbi);
+	if (retval) {
+		ipu_uninit_channel(mxc_fbi->ipu, mxc_fbi->ipu_ch);
+		return retval;
+	}
+
+	if (ovfbi_enable) {
+		if (ov_pos_ret >= 0)
+			ipu_disp_set_window_pos(
+					mxc_fbi_fg->ipu, mxc_fbi_fg->ipu_ch,
+					ov_pos_x, ov_pos_y);
+		retval = _setup_disp_channel2(mxc_fbi->ovfbi);
+		if (retval) {
+			ipu_uninit_channel(mxc_fbi_fg->ipu, mxc_fbi_fg->ipu_ch);
+			ipu_uninit_channel(mxc_fbi->ipu, mxc_fbi->ipu_ch);
+			return retval;
+		}
+	}
+
+	ipu_enable_channel(mxc_fbi->ipu, mxc_fbi->ipu_ch);
+	if (ovfbi_enable)
+		ipu_enable_channel(mxc_fbi_fg->ipu, mxc_fbi_fg->ipu_ch);
+
+	if (mxc_fbi->dispdrv && mxc_fbi->dispdrv->drv->enable) {
+		retval = mxc_fbi->dispdrv->drv->enable(mxc_fbi->dispdrv);
+		if (retval < 0) {
+			dev_err(fbi->device, "enable error, dispdrv:%s.\n",
+					mxc_fbi->dispdrv->drv->name);
+			return -EINVAL;
+		}
+	}
+
+	mxc_fbi->cur_var = fbi->var;
+
+	return retval;
+}
+
+static int _swap_channels(struct fb_info *fbi_from,
+			  struct fb_info *fbi_to, bool both_on)
+{
+	int retval, tmp;
+	ipu_channel_t old_ch;
+	struct fb_info *ovfbi;
+	struct mxcfb_info *mxc_fbi_from = (struct mxcfb_info *)fbi_from->par;
+	struct mxcfb_info *mxc_fbi_to = (struct mxcfb_info *)fbi_to->par;
+
+	if (both_on) {
+		ipu_disable_channel(mxc_fbi_to->ipu, mxc_fbi_to->ipu_ch, true);
+		ipu_uninit_channel(mxc_fbi_to->ipu, mxc_fbi_to->ipu_ch);
+	}
+
+	/* switch the mxc fbi parameters */
+	old_ch = mxc_fbi_from->ipu_ch;
+	mxc_fbi_from->ipu_ch = mxc_fbi_to->ipu_ch;
+	mxc_fbi_to->ipu_ch = old_ch;
+	tmp = mxc_fbi_from->ipu_ch_irq;
+	mxc_fbi_from->ipu_ch_irq = mxc_fbi_to->ipu_ch_irq;
+	mxc_fbi_to->ipu_ch_irq = tmp;
+	tmp = mxc_fbi_from->ipu_ch_nf_irq;
+	mxc_fbi_from->ipu_ch_nf_irq = mxc_fbi_to->ipu_ch_nf_irq;
+	mxc_fbi_to->ipu_ch_nf_irq = tmp;
+	ovfbi = mxc_fbi_from->ovfbi;
+	mxc_fbi_from->ovfbi = mxc_fbi_to->ovfbi;
+	mxc_fbi_to->ovfbi = ovfbi;
+
+	_setup_disp_channel1(fbi_from);
+	retval = _setup_disp_channel2(fbi_from);
+	if (retval)
+		return retval;
+
+	/* switch between dp and dc, disable old idmac, enable new idmac */
+	retval = ipu_swap_channel(mxc_fbi_from->ipu, old_ch, mxc_fbi_from->ipu_ch);
+	ipu_uninit_channel(mxc_fbi_from->ipu, old_ch);
+
+	if (both_on) {
+		_setup_disp_channel1(fbi_to);
+		retval = _setup_disp_channel2(fbi_to);
+		if (retval)
+			return retval;
+		ipu_enable_channel(mxc_fbi_to->ipu, mxc_fbi_to->ipu_ch);
+	}
+
+	return retval;
+}
+
+static int swap_channels(struct fb_info *fbi_from)
+{
+	int i;
+	int swap_mode;
+	ipu_channel_t ch_to;
+	struct mxcfb_info *mxc_fbi_from = (struct mxcfb_info *)fbi_from->par;
+	struct fb_info *fbi_to = NULL;
+	struct mxcfb_info *mxc_fbi_to;
+
+	/* what's the target channel? */
+	if (mxc_fbi_from->ipu_ch == MEM_BG_SYNC)
+		ch_to = MEM_DC_SYNC;
+	else
+		ch_to = MEM_BG_SYNC;
+
+	fbi_to = found_registered_fb(ch_to, mxc_fbi_from->ipu_id);
+	if (!fbi_to)
+		return -1;
+	mxc_fbi_to = (struct mxcfb_info *)fbi_to->par;
+
+	ipu_clear_irq(mxc_fbi_from->ipu, mxc_fbi_from->ipu_ch_irq);
+	ipu_clear_irq(mxc_fbi_to->ipu, mxc_fbi_to->ipu_ch_irq);
+	ipu_free_irq(mxc_fbi_from->ipu, mxc_fbi_from->ipu_ch_irq, fbi_from);
+	ipu_free_irq(mxc_fbi_to->ipu, mxc_fbi_to->ipu_ch_irq, fbi_to);
+	ipu_clear_irq(mxc_fbi_from->ipu, mxc_fbi_from->ipu_ch_nf_irq);
+	ipu_clear_irq(mxc_fbi_to->ipu, mxc_fbi_to->ipu_ch_nf_irq);
+	ipu_free_irq(mxc_fbi_from->ipu, mxc_fbi_from->ipu_ch_nf_irq, fbi_from);
+	ipu_free_irq(mxc_fbi_to->ipu, mxc_fbi_to->ipu_ch_nf_irq, fbi_to);
+
+	if (mxc_fbi_from->cur_blank == FB_BLANK_UNBLANK) {
+		if (mxc_fbi_to->cur_blank == FB_BLANK_UNBLANK)
+			swap_mode = BOTH_ON;
+		else
+			swap_mode = SRC_ON;
+	} else {
+		if (mxc_fbi_to->cur_blank == FB_BLANK_UNBLANK)
+			swap_mode = TGT_ON;
+		else
+			swap_mode = BOTH_OFF;
+	}
+
+	switch (swap_mode) {
+	case BOTH_ON:
+		/* disable target->switch src->enable target */
+		_swap_channels(fbi_from, fbi_to, true);
+		break;
+	case SRC_ON:
+		/* just switch src */
+		_swap_channels(fbi_from, fbi_to, false);
+		break;
+	case TGT_ON:
+		/* just switch target */
+		_swap_channels(fbi_to, fbi_from, false);
+		break;
+	case BOTH_OFF:
+		/* switch directly, no more need to do */
+		mxc_fbi_to->ipu_ch = mxc_fbi_from->ipu_ch;
+		mxc_fbi_from->ipu_ch = ch_to;
+		i = mxc_fbi_from->ipu_ch_irq;
+		mxc_fbi_from->ipu_ch_irq = mxc_fbi_to->ipu_ch_irq;
+		mxc_fbi_to->ipu_ch_irq = i;
+		i = mxc_fbi_from->ipu_ch_nf_irq;
+		mxc_fbi_from->ipu_ch_nf_irq = mxc_fbi_to->ipu_ch_nf_irq;
+		mxc_fbi_to->ipu_ch_nf_irq = i;
+		break;
+	default:
+		break;
+	}
+
+	if (ipu_request_irq(mxc_fbi_from->ipu, mxc_fbi_from->ipu_ch_irq,
+		mxcfb_irq_handler, IPU_IRQF_ONESHOT,
+		MXCFB_NAME, fbi_from) != 0) {
+		dev_err(fbi_from->device, "Error registering irq %d\n",
+			mxc_fbi_from->ipu_ch_irq);
+		return -EBUSY;
+	}
+	ipu_disable_irq(mxc_fbi_from->ipu, mxc_fbi_from->ipu_ch_irq);
+	if (ipu_request_irq(mxc_fbi_to->ipu, mxc_fbi_to->ipu_ch_irq,
+		mxcfb_irq_handler, IPU_IRQF_ONESHOT,
+		MXCFB_NAME, fbi_to) != 0) {
+		dev_err(fbi_to->device, "Error registering irq %d\n",
+			mxc_fbi_to->ipu_ch_irq);
+		return -EBUSY;
+	}
+	ipu_disable_irq(mxc_fbi_to->ipu, mxc_fbi_to->ipu_ch_irq);
+	if (ipu_request_irq(mxc_fbi_from->ipu, mxc_fbi_from->ipu_ch_nf_irq,
+		mxcfb_nf_irq_handler, IPU_IRQF_ONESHOT,
+		MXCFB_NAME, fbi_from) != 0) {
+		dev_err(fbi_from->device, "Error registering irq %d\n",
+			mxc_fbi_from->ipu_ch_nf_irq);
+		return -EBUSY;
+	}
+	ipu_disable_irq(mxc_fbi_from->ipu, mxc_fbi_from->ipu_ch_nf_irq);
+	if (ipu_request_irq(mxc_fbi_to->ipu, mxc_fbi_to->ipu_ch_nf_irq,
+		mxcfb_nf_irq_handler, IPU_IRQF_ONESHOT,
+		MXCFB_NAME, fbi_to) != 0) {
+		dev_err(fbi_to->device, "Error registering irq %d\n",
+			mxc_fbi_to->ipu_ch_nf_irq);
+		return -EBUSY;
+	}
+	ipu_disable_irq(mxc_fbi_to->ipu, mxc_fbi_to->ipu_ch_nf_irq);
+
+	return 0;
+}
+
+/*
+ * Check framebuffer variable parameters and adjust to valid values.
+ *
+ * @param       var      framebuffer variable parameters
+ *
+ * @param       info     framebuffer information pointer
+ */
+static int mxcfb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	u32 vtotal;
+	u32 htotal;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)info->par;
+
+
+	if (var->xres == 0 || var->yres == 0)
+		return 0;
+
+	/* fg should not bigger than bg */
+	if (mxc_fbi->ipu_ch == MEM_FG_SYNC) {
+		struct fb_info *fbi_tmp;
+		int bg_xres = 0, bg_yres = 0;
+		int16_t pos_x, pos_y;
+
+		bg_xres = var->xres;
+		bg_yres = var->yres;
+
+		fbi_tmp = found_registered_fb(MEM_BG_SYNC, mxc_fbi->ipu_id);
+		if (fbi_tmp) {
+			bg_xres = fbi_tmp->var.xres;
+			bg_yres = fbi_tmp->var.yres;
+		}
+
+		ipu_disp_get_window_pos(mxc_fbi->ipu, mxc_fbi->ipu_ch, &pos_x, &pos_y);
+
+		if ((var->xres + pos_x) > bg_xres)
+			var->xres = bg_xres - pos_x;
+		if ((var->yres + pos_y) > bg_yres)
+			var->yres = bg_yres - pos_y;
+	}
+
+	if (var->rotate > IPU_ROTATE_VERT_FLIP)
+		var->rotate = IPU_ROTATE_NONE;
+
+	if (var->xres_virtual < var->xres)
+		var->xres_virtual = var->xres;
+
+	if (var->yres_virtual < var->yres)
+		var->yres_virtual = var->yres * 3;
+
+	if ((var->bits_per_pixel != 32) && (var->bits_per_pixel != 24) &&
+	    (var->bits_per_pixel != 16) && (var->bits_per_pixel != 12) &&
+	    (var->bits_per_pixel != 8))
+		var->bits_per_pixel = 16;
+
+	if (check_var_pixfmt(var))
+		/* Fall back to default */
+		bpp_to_var(var->bits_per_pixel, var);
+
+	if (var->pixclock < 1000) {
+		htotal = var->xres + var->right_margin + var->hsync_len +
+		    var->left_margin;
+		vtotal = var->yres + var->lower_margin + var->vsync_len +
+		    var->upper_margin;
+		var->pixclock = (vtotal * htotal * 6UL) / 100UL;
+		var->pixclock = KHZ2PICOS(var->pixclock);
+		dev_dbg(info->device,
+			"pixclock set for 60Hz refresh = %u ps\n",
+			var->pixclock);
+	}
+
+	var->height = -1;
+	var->width = -1;
+	var->grayscale = 0;
+
+	return 0;
+}
+
+static inline u_int _chan_to_field(u_int chan, struct fb_bitfield *bf)
+{
+	chan &= 0xffff;
+	chan >>= 16 - bf->length;
+	return chan << bf->offset;
+}
+
+static int mxcfb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
+			   u_int trans, struct fb_info *fbi)
+{
+	unsigned int val;
+	int ret = 1;
+
+	/*
+	 * If greyscale is true, then we convert the RGB value
+	 * to greyscale no matter what visual we are using.
+	 */
+	if (fbi->var.grayscale)
+		red = green = blue = (19595 * red + 38470 * green +
+				      7471 * blue) >> 16;
+	switch (fbi->fix.visual) {
+	case FB_VISUAL_TRUECOLOR:
+		/*
+		 * 16-bit True Colour.  We encode the RGB value
+		 * according to the RGB bitfield information.
+		 */
+		if (regno < 16) {
+			u32 *pal = fbi->pseudo_palette;
+
+			val = _chan_to_field(red, &fbi->var.red);
+			val |= _chan_to_field(green, &fbi->var.green);
+			val |= _chan_to_field(blue, &fbi->var.blue);
+
+			pal[regno] = val;
+			ret = 0;
+		}
+		break;
+
+	case FB_VISUAL_STATIC_PSEUDOCOLOR:
+	case FB_VISUAL_PSEUDOCOLOR:
+		break;
+	}
+
+	return ret;
+}
+
+/*
+ * Function to handle custom ioctls for MXC framebuffer.
+ *
+ * @param       inode   inode struct
+ *
+ * @param       file    file struct
+ *
+ * @param       cmd     Ioctl command to handle
+ *
+ * @param       arg     User pointer to command arguments
+ *
+ * @param       fbi     framebuffer information pointer
+ */
+static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	int __user *argp = (void __user *)arg;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+
+	switch (cmd) {
+	case MXCFB_SET_GBL_ALPHA:
+		{
+			struct mxcfb_gbl_alpha ga;
+
+			if (copy_from_user(&ga, (void *)arg, sizeof(ga))) {
+				retval = -EFAULT;
+				break;
+			}
+
+			if (ipu_disp_set_global_alpha(mxc_fbi->ipu,
+						      mxc_fbi->ipu_ch,
+						      (bool)ga.enable,
+						      ga.alpha)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			if (ga.enable)
+				mxc_fbi->alpha_chan_en = false;
+
+			if (ga.enable)
+				dev_dbg(fbi->device,
+					"Set global alpha of %s to %d\n",
+					fbi->fix.id, ga.alpha);
+			break;
+		}
+	case MXCFB_SET_LOC_ALPHA:
+		{
+			struct mxcfb_loc_alpha la;
+
+			if (copy_from_user(&la, (void *)arg, sizeof(la))) {
+				retval = -EFAULT;
+				break;
+			}
+
+			if (ipu_disp_set_global_alpha(mxc_fbi->ipu, mxc_fbi->ipu_ch,
+						      !(bool)la.enable, 0)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			if (la.enable && !la.alpha_in_pixel) {
+				struct fb_info *fbi_tmp;
+				ipu_channel_t ipu_ch;
+
+				mxc_fbi->alpha_chan_en = true;
+
+				if (mxc_fbi->ipu_ch == MEM_FG_SYNC)
+					ipu_ch = MEM_BG_SYNC;
+				else if (mxc_fbi->ipu_ch == MEM_BG_SYNC)
+					ipu_ch = MEM_FG_SYNC;
+				else {
+					retval = -EINVAL;
+					break;
+				}
+
+				fbi_tmp = found_registered_fb(ipu_ch, mxc_fbi->ipu_id);
+				if (fbi_tmp)
+					((struct mxcfb_info *)(fbi_tmp->par))->alpha_chan_en = false;
+			} else
+				mxc_fbi->alpha_chan_en = false;
+
+			fbi->var.activate = (fbi->var.activate & ~FB_ACTIVATE_MASK) |
+						FB_ACTIVATE_NOW | FB_ACTIVATE_FORCE;
+			mxcfb_set_par(fbi);
+
+			la.alpha_phy_addr0 = mxc_fbi->alpha_phy_addr0;
+			la.alpha_phy_addr1 = mxc_fbi->alpha_phy_addr1;
+			if (copy_to_user((void *)arg, &la, sizeof(la))) {
+				retval = -EFAULT;
+				break;
+			}
+
+			if (la.enable)
+				dev_dbg(fbi->device,
+					"Enable DP local alpha for %s\n",
+					fbi->fix.id);
+			break;
+		}
+	case MXCFB_SET_LOC_ALP_BUF:
+		{
+			unsigned long base;
+			uint32_t ipu_alp_ch_irq;
+
+			if (!(((mxc_fbi->ipu_ch == MEM_FG_SYNC) ||
+			     (mxc_fbi->ipu_ch == MEM_BG_SYNC)) &&
+			     (mxc_fbi->alpha_chan_en))) {
+				dev_err(fbi->device,
+					"Should use background or overlay "
+					"framebuffer to set the alpha buffer "
+					"number\n");
+				return -EINVAL;
+			}
+
+			if (get_user(base, argp))
+				return -EFAULT;
+
+			if (base != mxc_fbi->alpha_phy_addr0 &&
+			    base != mxc_fbi->alpha_phy_addr1) {
+				dev_err(fbi->device,
+					"Wrong alpha buffer physical address "
+					"%lu\n", base);
+				return -EINVAL;
+			}
+
+			if (mxc_fbi->ipu_ch == MEM_FG_SYNC)
+				ipu_alp_ch_irq = IPU_IRQ_FG_ALPHA_SYNC_EOF;
+			else
+				ipu_alp_ch_irq = IPU_IRQ_BG_ALPHA_SYNC_EOF;
+
+			retval = wait_for_completion_timeout(
+				&mxc_fbi->alpha_flip_complete, HZ/2);
+			if (retval == 0) {
+				dev_err(fbi->device, "timeout when waiting for alpha flip irq\n");
+				retval = -ETIMEDOUT;
+				break;
+			}
+
+			mxc_fbi->cur_ipu_alpha_buf =
+						!mxc_fbi->cur_ipu_alpha_buf;
+			if (ipu_update_channel_buffer(mxc_fbi->ipu, mxc_fbi->ipu_ch,
+						      IPU_ALPHA_IN_BUFFER,
+						      mxc_fbi->
+							cur_ipu_alpha_buf,
+						      base) == 0) {
+				ipu_select_buffer(mxc_fbi->ipu, mxc_fbi->ipu_ch,
+						  IPU_ALPHA_IN_BUFFER,
+						  mxc_fbi->cur_ipu_alpha_buf);
+				ipu_clear_irq(mxc_fbi->ipu, ipu_alp_ch_irq);
+				ipu_enable_irq(mxc_fbi->ipu, ipu_alp_ch_irq);
+			} else {
+				dev_err(fbi->device,
+					"Error updating %s SDC alpha buf %d "
+					"to address=0x%08lX\n",
+					fbi->fix.id,
+					mxc_fbi->cur_ipu_alpha_buf, base);
+			}
+			break;
+		}
+	case MXCFB_SET_CLR_KEY:
+		{
+			struct mxcfb_color_key key;
+			if (copy_from_user(&key, (void *)arg, sizeof(key))) {
+				retval = -EFAULT;
+				break;
+			}
+			retval = ipu_disp_set_color_key(mxc_fbi->ipu, mxc_fbi->ipu_ch,
+							key.enable,
+							key.color_key);
+			dev_dbg(fbi->device, "Set color key to 0x%08X\n",
+				key.color_key);
+			break;
+		}
+	case MXCFB_SET_GAMMA:
+		{
+			struct mxcfb_gamma gamma;
+			if (copy_from_user(&gamma, (void *)arg, sizeof(gamma))) {
+				retval = -EFAULT;
+				break;
+			}
+			retval = ipu_disp_set_gamma_correction(mxc_fbi->ipu,
+							mxc_fbi->ipu_ch,
+							gamma.enable,
+							gamma.constk,
+							gamma.slopek);
+			break;
+		}
+	case MXCFB_WAIT_FOR_VSYNC:
+		{
+			if (mxc_fbi->ipu_ch == MEM_FG_SYNC) {
+				/* BG should poweron */
+				struct mxcfb_info *bg_mxcfbi = NULL;
+				struct fb_info *fbi_tmp;
+
+				fbi_tmp = found_registered_fb(MEM_BG_SYNC, mxc_fbi->ipu_id);
+				if (fbi_tmp)
+					bg_mxcfbi = ((struct mxcfb_info *)(fbi_tmp->par));
+
+				if (!bg_mxcfbi) {
+					retval = -EINVAL;
+					break;
+				}
+				if (bg_mxcfbi->cur_blank != FB_BLANK_UNBLANK) {
+					retval = -EINVAL;
+					break;
+				}
+			}
+			if (mxc_fbi->cur_blank != FB_BLANK_UNBLANK) {
+				retval = -EINVAL;
+				break;
+			}
+
+			init_completion(&mxc_fbi->vsync_complete);
+			ipu_clear_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_nf_irq);
+			ipu_enable_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_nf_irq);
+			retval = wait_for_completion_interruptible_timeout(
+				&mxc_fbi->vsync_complete, 1 * HZ);
+			if (retval == 0) {
+				dev_err(fbi->device,
+					"MXCFB_WAIT_FOR_VSYNC: timeout %d\n",
+					retval);
+				retval = -ETIME;
+			} else if (retval > 0) {
+				retval = 0;
+			}
+			break;
+		}
+	case FBIO_ALLOC:
+		{
+			int size;
+			struct mxcfb_alloc_list *mem;
+
+			mem = kzalloc(sizeof(*mem), GFP_KERNEL);
+			if (mem == NULL)
+				return -ENOMEM;
+
+			if (get_user(size, argp))
+				return -EFAULT;
+
+			mem->size = PAGE_ALIGN(size);
+
+			mem->cpu_addr = dma_alloc_coherent(fbi->device, size,
+							   &mem->phy_addr,
+							   GFP_KERNEL);
+			if (mem->cpu_addr == NULL) {
+				kfree(mem);
+				return -ENOMEM;
+			}
+
+			list_add(&mem->list, &fb_alloc_list);
+
+			dev_dbg(fbi->device, "allocated %d bytes @ 0x%08X\n",
+				mem->size, mem->phy_addr);
+
+			if (put_user(mem->phy_addr, argp))
+				return -EFAULT;
+
+			break;
+		}
+	case FBIO_FREE:
+		{
+			unsigned long offset;
+			struct mxcfb_alloc_list *mem;
+
+			if (get_user(offset, argp))
+				return -EFAULT;
+
+			retval = -EINVAL;
+			list_for_each_entry(mem, &fb_alloc_list, list) {
+				if (mem->phy_addr == offset) {
+					list_del(&mem->list);
+					dma_free_coherent(fbi->device,
+							  mem->size,
+							  mem->cpu_addr,
+							  mem->phy_addr);
+					kfree(mem);
+					retval = 0;
+					break;
+				}
+			}
+
+			break;
+		}
+	case MXCFB_SET_OVERLAY_POS:
+		{
+			struct mxcfb_pos pos;
+			struct fb_info *bg_fbi = NULL;
+			struct mxcfb_info *bg_mxcfbi = NULL;
+
+			if (mxc_fbi->ipu_ch != MEM_FG_SYNC) {
+				dev_err(fbi->device, "Should use the overlay "
+					"framebuffer to set the position of "
+					"the overlay window\n");
+				retval = -EINVAL;
+				break;
+			}
+
+			if (copy_from_user(&pos, (void *)arg, sizeof(pos))) {
+				retval = -EFAULT;
+				break;
+			}
+
+			bg_fbi = found_registered_fb(MEM_BG_SYNC, mxc_fbi->ipu_id);
+			if (bg_fbi)
+				bg_mxcfbi = ((struct mxcfb_info *)(bg_fbi->par));
+
+			if (bg_fbi == NULL) {
+				dev_err(fbi->device, "Cannot find the "
+					"background framebuffer\n");
+				retval = -ENOENT;
+				break;
+			}
+
+			/* if fb is unblank, check if the pos fit the display */
+			if (mxc_fbi->cur_blank == FB_BLANK_UNBLANK) {
+				if (fbi->var.xres + pos.x > bg_fbi->var.xres) {
+					if (bg_fbi->var.xres < fbi->var.xres)
+						pos.x = 0;
+					else
+						pos.x = bg_fbi->var.xres - fbi->var.xres;
+				}
+				if (fbi->var.yres + pos.y > bg_fbi->var.yres) {
+					if (bg_fbi->var.yres < fbi->var.yres)
+						pos.y = 0;
+					else
+						pos.y = bg_fbi->var.yres - fbi->var.yres;
+				}
+			}
+
+			retval = ipu_disp_set_window_pos(mxc_fbi->ipu, mxc_fbi->ipu_ch,
+							 pos.x, pos.y);
+
+			if (copy_to_user((void *)arg, &pos, sizeof(pos))) {
+				retval = -EFAULT;
+				break;
+			}
+			break;
+		}
+	case MXCFB_GET_FB_IPU_CHAN:
+		{
+			struct mxcfb_info *mxc_fbi =
+				(struct mxcfb_info *)fbi->par;
+
+			if (put_user(mxc_fbi->ipu_ch, argp))
+				return -EFAULT;
+			break;
+		}
+	case MXCFB_GET_DIFMT:
+		{
+			struct mxcfb_info *mxc_fbi =
+				(struct mxcfb_info *)fbi->par;
+
+			if (put_user(mxc_fbi->ipu_di_pix_fmt, argp))
+				return -EFAULT;
+			break;
+		}
+	case MXCFB_GET_FB_IPU_DI:
+		{
+			struct mxcfb_info *mxc_fbi =
+				(struct mxcfb_info *)fbi->par;
+
+			if (put_user(mxc_fbi->ipu_di, argp))
+				return -EFAULT;
+			break;
+		}
+	case MXCFB_GET_FB_BLANK:
+		{
+			struct mxcfb_info *mxc_fbi =
+				(struct mxcfb_info *)fbi->par;
+
+			if (put_user(mxc_fbi->cur_blank, argp))
+				return -EFAULT;
+			break;
+		}
+	case MXCFB_SET_DIFMT:
+		{
+			struct mxcfb_info *mxc_fbi =
+				(struct mxcfb_info *)fbi->par;
+
+			if (get_user(mxc_fbi->ipu_di_pix_fmt, argp))
+				return -EFAULT;
+
+			break;
+		}
+	case MXCFB_CSC_UPDATE:
+		{
+			struct mxcfb_csc_matrix csc;
+
+			if (copy_from_user(&csc, (void *) arg, sizeof(csc)))
+				return -EFAULT;
+
+			if ((mxc_fbi->ipu_ch != MEM_FG_SYNC) &&
+				(mxc_fbi->ipu_ch != MEM_BG_SYNC) &&
+				(mxc_fbi->ipu_ch != MEM_BG_ASYNC0))
+				return -EFAULT;
+			ipu_set_csc_coefficients(mxc_fbi->ipu, mxc_fbi->ipu_ch,
+						csc.param);
+		}
+	default:
+		retval = -EINVAL;
+	}
+	return retval;
+}
+
+/*
+ * mxcfb_blank():
+ *      Blank the display.
+ */
+static int mxcfb_blank(int blank, struct fb_info *info)
+{
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)info->par;
+	int ret = 0;
+
+	dev_dbg(info->device, "blank = %d\n", blank);
+
+	if (mxc_fbi->cur_blank == blank)
+		return 0;
+
+	mxc_fbi->next_blank = blank;
+
+	switch (blank) {
+	case FB_BLANK_POWERDOWN:
+	case FB_BLANK_VSYNC_SUSPEND:
+	case FB_BLANK_HSYNC_SUSPEND:
+	case FB_BLANK_NORMAL:
+		if (mxc_fbi->dispdrv && mxc_fbi->dispdrv->drv->disable)
+			mxc_fbi->dispdrv->drv->disable(mxc_fbi->dispdrv);
+		ipu_disable_channel(mxc_fbi->ipu, mxc_fbi->ipu_ch, true);
+		if (mxc_fbi->ipu_di >= 0)
+			ipu_uninit_sync_panel(mxc_fbi->ipu, mxc_fbi->ipu_di);
+		ipu_uninit_channel(mxc_fbi->ipu, mxc_fbi->ipu_ch);
+		break;
+	case FB_BLANK_UNBLANK:
+		info->var.activate = (info->var.activate & ~FB_ACTIVATE_MASK) |
+				FB_ACTIVATE_NOW | FB_ACTIVATE_FORCE;
+		ret = mxcfb_set_par(info);
+		break;
+	}
+	if (!ret)
+		mxc_fbi->cur_blank = blank;
+	return ret;
+}
+
+/*
+ * Pan or Wrap the Display
+ *
+ * This call looks only at xoffset, yoffset and the FB_VMODE_YWRAP flag
+ *
+ * @param               var     Variable screen buffer information
+ * @param               info    Framebuffer information pointer
+ */
+static int
+mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)info->par,
+			  *mxc_graphic_fbi = NULL;
+	u_int y_bottom;
+	unsigned int fr_xoff, fr_yoff, fr_w, fr_h;
+	unsigned long base, active_alpha_phy_addr = 0;
+	bool loc_alpha_en = false;
+	int fb_stride;
+	int i;
+	int ret;
+
+	/* no pan display during fb blank */
+	if (mxc_fbi->ipu_ch == MEM_FG_SYNC) {
+		struct mxcfb_info *bg_mxcfbi = NULL;
+		struct fb_info *fbi_tmp;
+
+		fbi_tmp = found_registered_fb(MEM_BG_SYNC, mxc_fbi->ipu_id);
+		if (fbi_tmp)
+			bg_mxcfbi = ((struct mxcfb_info *)(fbi_tmp->par));
+		if (!bg_mxcfbi)
+			return -EINVAL;
+		if (bg_mxcfbi->cur_blank != FB_BLANK_UNBLANK)
+			return -EINVAL;
+	}
+	if (mxc_fbi->cur_blank != FB_BLANK_UNBLANK)
+		return -EINVAL;
+
+	y_bottom = var->yoffset;
+
+	if (y_bottom > info->var.yres_virtual)
+		return -EINVAL;
+
+	switch (fbi_to_pixfmt(info)) {
+	case IPU_PIX_FMT_YUV420P2:
+	case IPU_PIX_FMT_YVU420P:
+	case IPU_PIX_FMT_NV12:
+	case IPU_PIX_FMT_YUV422P:
+	case IPU_PIX_FMT_YVU422P:
+	case IPU_PIX_FMT_YUV420P:
+	case IPU_PIX_FMT_YUV444P:
+		fb_stride = info->var.xres_virtual;
+		break;
+	default:
+		fb_stride = info->fix.line_length;
+	}
+
+	base = info->fix.smem_start;
+	fr_xoff = var->xoffset;
+	fr_w = info->var.xres_virtual;
+	if (!(var->vmode & FB_VMODE_YWRAP)) {
+		dev_dbg(info->device, "Y wrap disabled\n");
+		fr_yoff = var->yoffset % info->var.yres;
+		fr_h = info->var.yres;
+		base += info->fix.line_length * info->var.yres *
+			(var->yoffset / info->var.yres);
+	} else {
+		dev_dbg(info->device, "Y wrap enabled\n");
+		fr_yoff = var->yoffset;
+		fr_h = info->var.yres_virtual;
+	}
+	base += fr_yoff * fb_stride + fr_xoff;
+
+	/* Check if DP local alpha is enabled and find the graphic fb */
+	if (mxc_fbi->ipu_ch == MEM_BG_SYNC || mxc_fbi->ipu_ch == MEM_FG_SYNC) {
+		for (i = 0; i < num_registered_fb; i++) {
+			char bg_id[] = "DISP3 BG";
+			char fg_id[] = "DISP3 FG";
+			char *idstr = registered_fb[i]->fix.id;
+			bg_id[4] += mxc_fbi->ipu_id;
+			fg_id[4] += mxc_fbi->ipu_id;
+			if ((strcmp(idstr, bg_id) == 0 ||
+			     strcmp(idstr, fg_id) == 0) &&
+			    ((struct mxcfb_info *)
+			      (registered_fb[i]->par))->alpha_chan_en) {
+				loc_alpha_en = true;
+				mxc_graphic_fbi = (struct mxcfb_info *)
+						(registered_fb[i]->par);
+				active_alpha_phy_addr =
+					mxc_fbi->cur_ipu_alpha_buf ?
+					mxc_graphic_fbi->alpha_phy_addr1 :
+					mxc_graphic_fbi->alpha_phy_addr0;
+				dev_dbg(info->device, "Updating SDC alpha "
+					"buf %d address=0x%08lX\n",
+					!mxc_fbi->cur_ipu_alpha_buf,
+					active_alpha_phy_addr);
+				break;
+			}
+		}
+	}
+
+	ret = wait_for_completion_timeout(&mxc_fbi->flip_complete, HZ/2);
+	if (ret == 0) {
+		dev_err(info->device, "timeout when waiting for flip irq\n");
+		return -ETIMEDOUT;
+	}
+
+	++mxc_fbi->cur_ipu_buf;
+	mxc_fbi->cur_ipu_buf %= 3;
+	mxc_fbi->cur_ipu_alpha_buf = !mxc_fbi->cur_ipu_alpha_buf;
+
+	dev_dbg(info->device, "Updating SDC %s buf %d address=0x%08lX\n",
+		info->fix.id, mxc_fbi->cur_ipu_buf, base);
+
+	if (ipu_update_channel_buffer(mxc_fbi->ipu, mxc_fbi->ipu_ch, IPU_INPUT_BUFFER,
+				      mxc_fbi->cur_ipu_buf, base) == 0) {
+		/* Update the DP local alpha buffer only for graphic plane */
+		if (loc_alpha_en && mxc_graphic_fbi == mxc_fbi &&
+		    ipu_update_channel_buffer(mxc_graphic_fbi->ipu, mxc_graphic_fbi->ipu_ch,
+					      IPU_ALPHA_IN_BUFFER,
+					      mxc_fbi->cur_ipu_alpha_buf,
+					      active_alpha_phy_addr) == 0) {
+			ipu_select_buffer(mxc_graphic_fbi->ipu, mxc_graphic_fbi->ipu_ch,
+					  IPU_ALPHA_IN_BUFFER,
+					  mxc_fbi->cur_ipu_alpha_buf);
+		}
+
+		/* update u/v offset */
+		ipu_update_channel_offset(mxc_fbi->ipu, mxc_fbi->ipu_ch,
+				IPU_INPUT_BUFFER,
+				fbi_to_pixfmt(info),
+				fr_w,
+				fr_h,
+				fr_w,
+				0, 0,
+				fr_yoff,
+				fr_xoff);
+
+		ipu_select_buffer(mxc_fbi->ipu, mxc_fbi->ipu_ch, IPU_INPUT_BUFFER,
+				  mxc_fbi->cur_ipu_buf);
+		ipu_clear_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_irq);
+		ipu_enable_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_irq);
+	} else {
+		dev_err(info->device,
+			"Error updating SDC buf %d to address=0x%08lX, "
+			"current buf %d, buf0 ready %d, buf1 ready %d, "
+			"buf2 ready %d\n", mxc_fbi->cur_ipu_buf, base,
+			ipu_get_cur_buffer_idx(mxc_fbi->ipu, mxc_fbi->ipu_ch,
+					       IPU_INPUT_BUFFER),
+			ipu_check_buffer_ready(mxc_fbi->ipu, mxc_fbi->ipu_ch,
+					       IPU_INPUT_BUFFER, 0),
+			ipu_check_buffer_ready(mxc_fbi->ipu, mxc_fbi->ipu_ch,
+					       IPU_INPUT_BUFFER, 1),
+			ipu_check_buffer_ready(mxc_fbi->ipu, mxc_fbi->ipu_ch,
+					       IPU_INPUT_BUFFER, 2));
+		++mxc_fbi->cur_ipu_buf;
+		mxc_fbi->cur_ipu_buf %= 3;
+		++mxc_fbi->cur_ipu_buf;
+		mxc_fbi->cur_ipu_buf %= 3;
+		mxc_fbi->cur_ipu_alpha_buf = !mxc_fbi->cur_ipu_alpha_buf;
+		ipu_clear_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_irq);
+		ipu_enable_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_irq);
+		return -EBUSY;
+	}
+
+	dev_dbg(info->device, "Update complete\n");
+
+	info->var.yoffset = var->yoffset;
+
+	return 0;
+}
+
+/*
+ * Function to handle custom mmap for MXC framebuffer.
+ *
+ * @param       fbi     framebuffer information pointer
+ *
+ * @param       vma     Pointer to vm_area_struct
+ */
+static int mxcfb_mmap(struct fb_info *fbi, struct vm_area_struct *vma)
+{
+	bool found = false;
+	u32 len;
+	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
+	struct mxcfb_alloc_list *mem;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+
+	if (offset < fbi->fix.smem_len) {
+		/* mapping framebuffer memory */
+		len = fbi->fix.smem_len - offset;
+		vma->vm_pgoff = (fbi->fix.smem_start + offset) >> PAGE_SHIFT;
+	} else if ((vma->vm_pgoff ==
+			(mxc_fbi->alpha_phy_addr0 >> PAGE_SHIFT)) ||
+		   (vma->vm_pgoff ==
+			(mxc_fbi->alpha_phy_addr1 >> PAGE_SHIFT))) {
+		len = mxc_fbi->alpha_mem_len;
+	} else {
+		list_for_each_entry(mem, &fb_alloc_list, list) {
+			if (offset == mem->phy_addr) {
+				found = true;
+				len = mem->size;
+				break;
+			}
+		}
+		if (!found)
+			return -EINVAL;
+	}
+
+	len = PAGE_ALIGN(len);
+	if (vma->vm_end - vma->vm_start > len)
+		return -EINVAL;
+
+	/* make buffers bufferable */
+	vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
+
+	vma->vm_flags |= VM_IO;
+
+	if (remap_pfn_range(vma, vma->vm_start, vma->vm_pgoff,
+			    vma->vm_end - vma->vm_start, vma->vm_page_prot)) {
+		dev_dbg(fbi->device, "mmap remap_pfn_range failed\n");
+		return -ENOBUFS;
+	}
+
+	return 0;
+}
+
+/*!
+ * This structure contains the pointers to the control functions that are
+ * invoked by the core framebuffer driver to perform operations like
+ * blitting, rectangle filling, copy regions and cursor definition.
+ */
+static struct fb_ops mxcfb_ops = {
+	.owner = THIS_MODULE,
+	.fb_set_par = mxcfb_set_par,
+	.fb_check_var = mxcfb_check_var,
+	.fb_setcolreg = mxcfb_setcolreg,
+	.fb_pan_display = mxcfb_pan_display,
+	.fb_ioctl = mxcfb_ioctl,
+	.fb_mmap = mxcfb_mmap,
+	.fb_fillrect = cfb_fillrect,
+	.fb_copyarea = cfb_copyarea,
+	.fb_imageblit = cfb_imageblit,
+	.fb_blank = mxcfb_blank,
+};
+
+static irqreturn_t mxcfb_irq_handler(int irq, void *dev_id)
+{
+	struct fb_info *fbi = dev_id;
+	struct mxcfb_info *mxc_fbi = fbi->par;
+
+	complete(&mxc_fbi->flip_complete);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t mxcfb_nf_irq_handler(int irq, void *dev_id)
+{
+	struct fb_info *fbi = dev_id;
+	struct mxcfb_info *mxc_fbi = fbi->par;
+
+	complete(&mxc_fbi->vsync_complete);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t mxcfb_alpha_irq_handler(int irq, void *dev_id)
+{
+	struct fb_info *fbi = dev_id;
+	struct mxcfb_info *mxc_fbi = fbi->par;
+
+	complete(&mxc_fbi->alpha_flip_complete);
+	return IRQ_HANDLED;
+}
+
+/*
+ * Suspends the framebuffer and blanks the screen. Power management support
+ */
+static int mxcfb_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct fb_info *fbi = platform_get_drvdata(pdev);
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+	int saved_blank;
+#ifdef CONFIG_FB_MXC_LOW_PWR_DISPLAY
+	void *fbmem;
+#endif
+
+	if (mxc_fbi->ovfbi) {
+		struct mxcfb_info *mxc_fbi_fg =
+			(struct mxcfb_info *)mxc_fbi->ovfbi->par;
+
+		console_lock();
+		fb_set_suspend(mxc_fbi->ovfbi, 1);
+		saved_blank = mxc_fbi_fg->cur_blank;
+		mxcfb_blank(FB_BLANK_POWERDOWN, mxc_fbi->ovfbi);
+		mxc_fbi_fg->next_blank = saved_blank;
+		console_unlock();
+	}
+
+	console_lock();
+	fb_set_suspend(fbi, 1);
+	saved_blank = mxc_fbi->cur_blank;
+	mxcfb_blank(FB_BLANK_POWERDOWN, fbi);
+	mxc_fbi->next_blank = saved_blank;
+	console_unlock();
+
+	return 0;
+}
+
+/*
+ * Resumes the framebuffer and unblanks the screen. Power management support
+ */
+static int mxcfb_resume(struct platform_device *pdev)
+{
+	struct fb_info *fbi = platform_get_drvdata(pdev);
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+
+	console_lock();
+	mxcfb_blank(mxc_fbi->next_blank, fbi);
+	fb_set_suspend(fbi, 0);
+	console_unlock();
+
+	if (mxc_fbi->ovfbi) {
+		struct mxcfb_info *mxc_fbi_fg =
+			(struct mxcfb_info *)mxc_fbi->ovfbi->par;
+		console_lock();
+		mxcfb_blank(mxc_fbi_fg->next_blank, mxc_fbi->ovfbi);
+		fb_set_suspend(mxc_fbi->ovfbi, 0);
+		console_unlock();
+	}
+
+	return 0;
+}
+
+/*
+ * Main framebuffer functions
+ */
+
+/*!
+ * Allocates the DRAM memory for the frame buffer.      This buffer is remapped
+ * into a non-cached, non-buffered, memory region to allow palette and pixel
+ * writes to occur without flushing the cache.  Once this area is remapped,
+ * all virtual memory access to the video memory should occur at the new region.
+ *
+ * @param       fbi     framebuffer information pointer
+ *
+ * @return      Error code indicating success or failure
+ */
+static int mxcfb_map_video_memory(struct fb_info *fbi)
+{
+	if (fbi->fix.smem_len < fbi->var.yres_virtual * fbi->fix.line_length)
+		fbi->fix.smem_len = fbi->var.yres_virtual *
+				    fbi->fix.line_length;
+
+	fbi->screen_base = dma_alloc_writecombine(fbi->device,
+				fbi->fix.smem_len,
+				(dma_addr_t *)&fbi->fix.smem_start,
+				GFP_DMA | GFP_KERNEL);
+	if (fbi->screen_base == 0) {
+		dev_err(fbi->device, "Unable to allocate framebuffer memory\n");
+		fbi->fix.smem_len = 0;
+		fbi->fix.smem_start = 0;
+		return -EBUSY;
+	}
+
+	dev_dbg(fbi->device, "allocated fb @ paddr=0x%08X, size=%d.\n",
+		(uint32_t) fbi->fix.smem_start, fbi->fix.smem_len);
+
+	fbi->screen_size = fbi->fix.smem_len;
+
+	/* Clear the screen */
+	memset((char *)fbi->screen_base, 0, fbi->fix.smem_len);
+
+	return 0;
+}
+
+/*!
+ * De-allocates the DRAM memory for the frame buffer.
+ *
+ * @param       fbi     framebuffer information pointer
+ *
+ * @return      Error code indicating success or failure
+ */
+static int mxcfb_unmap_video_memory(struct fb_info *fbi)
+{
+	dma_free_writecombine(fbi->device, fbi->fix.smem_len,
+			      fbi->screen_base, fbi->fix.smem_start);
+	fbi->screen_base = 0;
+	fbi->fix.smem_start = 0;
+	fbi->fix.smem_len = 0;
+	return 0;
+}
+
+/*!
+ * Initializes the framebuffer information pointer. After allocating
+ * sufficient memory for the framebuffer structure, the fields are
+ * filled with custom information passed in from the configurable
+ * structures.  This includes information such as bits per pixel,
+ * color maps, screen width/height and RGBA offsets.
+ *
+ * @return      Framebuffer structure initialized with our information
+ */
+static struct fb_info *mxcfb_init_fbinfo(struct device *dev, struct fb_ops *ops)
+{
+	struct fb_info *fbi;
+	struct mxcfb_info *mxcfbi;
+
+	/*
+	 * Allocate sufficient memory for the fb structure
+	 */
+	fbi = framebuffer_alloc(sizeof(struct mxcfb_info), dev);
+	if (!fbi)
+		return NULL;
+
+	mxcfbi = (struct mxcfb_info *)fbi->par;
+
+	fbi->var.activate = FB_ACTIVATE_NOW;
+
+	fbi->fbops = ops;
+	fbi->flags = FBINFO_FLAG_DEFAULT;
+	fbi->pseudo_palette = mxcfbi->pseudo_palette;
+
+	/*
+	 * Allocate colormap
+	 */
+	fb_alloc_cmap(&fbi->cmap, 16, 0);
+
+	return fbi;
+}
+
+static ssize_t show_disp_chan(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+	struct fb_info *info = dev_get_drvdata(dev);
+	struct mxcfb_info *mxcfbi = (struct mxcfb_info *)info->par;
+
+	if (mxcfbi->ipu_ch == MEM_BG_SYNC)
+		return sprintf(buf, "2-layer-fb-bg\n");
+	else if (mxcfbi->ipu_ch == MEM_FG_SYNC)
+		return sprintf(buf, "2-layer-fb-fg\n");
+	else if (mxcfbi->ipu_ch == MEM_DC_SYNC)
+		return sprintf(buf, "1-layer-fb\n");
+	else
+		return sprintf(buf, "err: no display chan\n");
+}
+
+static ssize_t swap_disp_chan(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t count)
+{
+	struct fb_info *info = dev_get_drvdata(dev);
+	struct mxcfb_info *mxcfbi = (struct mxcfb_info *)info->par;
+	struct mxcfb_info *fg_mxcfbi = NULL;
+
+	console_lock();
+	/* swap only happen between DP-BG and DC, while DP-FG disable */
+	if (((mxcfbi->ipu_ch == MEM_BG_SYNC) &&
+	     (strstr(buf, "1-layer-fb") != NULL)) ||
+	    ((mxcfbi->ipu_ch == MEM_DC_SYNC) &&
+	     (strstr(buf, "2-layer-fb-bg") != NULL))) {
+		struct fb_info *fbi_fg;
+
+		fbi_fg = found_registered_fb(MEM_FG_SYNC, mxcfbi->ipu_id);
+		if (fbi_fg)
+			fg_mxcfbi = (struct mxcfb_info *)fbi_fg->par;
+
+		if (!fg_mxcfbi ||
+			fg_mxcfbi->cur_blank == FB_BLANK_UNBLANK) {
+			dev_err(dev,
+				"Can not switch while fb2(fb-fg) is on.\n");
+			console_unlock();
+			return count;
+		}
+
+		if (swap_channels(info) < 0)
+			dev_err(dev, "Swap display channel failed.\n");
+	}
+
+	console_unlock();
+	return count;
+}
+static DEVICE_ATTR(fsl_disp_property, 644, show_disp_chan, swap_disp_chan);
+
+static ssize_t show_disp_dev(struct device *dev,
+			     struct device_attribute *attr, char *buf)
+{
+	struct fb_info *info = dev_get_drvdata(dev);
+	struct mxcfb_info *mxcfbi = (struct mxcfb_info *)info->par;
+
+	if (mxcfbi->ipu_ch == MEM_FG_SYNC)
+		return sprintf(buf, "overlay\n");
+	else
+		return sprintf(buf, "%s\n", mxcfbi->dispdrv->drv->name);
+}
+static DEVICE_ATTR(fsl_disp_dev_property, S_IRUGO, show_disp_dev, NULL);
+
+static int mxcfb_dispdrv_init(struct platform_device *pdev,
+		struct fb_info *fbi)
+{
+	struct ipuv3_fb_platform_data *plat_data = pdev->dev.platform_data;
+	struct mxcfb_info *mxcfbi = (struct mxcfb_info *)fbi->par;
+	struct mxc_dispdrv_setting setting;
+	char disp_dev[32], *default_dev = "lcd";
+	int ret = 0;
+
+	setting.if_fmt = plat_data->interface_pix_fmt;
+	setting.dft_mode_str = plat_data->mode_str;
+	setting.default_bpp = plat_data->default_bpp;
+	if (!setting.default_bpp)
+		setting.default_bpp = 16;
+	setting.fbi = fbi;
+	if (!strlen(plat_data->disp_dev)) {
+		memcpy(disp_dev, default_dev, strlen(default_dev));
+		disp_dev[strlen(default_dev)] = '\0';
+	} else {
+		memcpy(disp_dev, plat_data->disp_dev,
+				strlen(plat_data->disp_dev));
+		disp_dev[strlen(plat_data->disp_dev)] = '\0';
+	}
+
+	dev_info(&pdev->dev, "register mxc display driver %s\n", disp_dev);
+
+	mxcfbi->dispdrv = mxc_dispdrv_gethandle(disp_dev, &setting);
+	if (IS_ERR(mxcfbi->dispdrv)) {
+		ret = PTR_ERR(mxcfbi->dispdrv);
+		dev_err(&pdev->dev, "NO mxc display driver found!\n");
+		return ret;
+	} else {
+		/* fix-up  */
+		mxcfbi->ipu_di_pix_fmt = setting.if_fmt;
+		mxcfbi->default_bpp = setting.default_bpp;
+
+		/* setting */
+		mxcfbi->ipu_id = setting.dev_id;
+		mxcfbi->ipu_di = setting.disp_id;
+		dev_dbg(&pdev->dev, "di_pixfmt:0x%x, bpp:0x%x, di:%d, ipu:%d\n",
+				setting.if_fmt, setting.default_bpp,
+				setting.disp_id, setting.dev_id);
+	}
+
+	return ret;
+}
+
+/*
+ * Parse user specified options (`video=trident:')
+ * example:
+ * 	video=mxcfb0:dev=lcd,800x480M-16@55,if=RGB565,bpp=16,noaccel
+ *	video=mxcfb0:dev=lcd,800x480M-16@55,if=RGB565,fbpix=RGB565
+ */
+static int mxcfb_option_setup(struct platform_device *pdev, struct fb_info *fbi)
+{
+	struct ipuv3_fb_platform_data *pdata = pdev->dev.platform_data;
+	char *options, *opt, *fb_mode_str = NULL;
+	char name[] = "mxcfb0";
+	uint32_t fb_pix_fmt = 0;
+
+	name[5] += pdev->id;
+	if (fb_get_options(name, &options)) {
+		dev_err(&pdev->dev, "Can't get fb option for %s!\n", name);
+		return -ENODEV;
+	}
+
+	if (!options || !*options)
+		return 0;
+
+	while ((opt = strsep(&options, ",")) != NULL) {
+		if (!*opt)
+			continue;
+
+		if (!strncmp(opt, "dev=", 4)) {
+			memcpy(pdata->disp_dev, opt + 4, strlen(opt) - 4);
+			pdata->disp_dev[strlen(opt) - 4] = '\0';
+		} else if (!strncmp(opt, "if=", 3)) {
+			if (!strncmp(opt+3, "RGB24", 5))
+				pdata->interface_pix_fmt = IPU_PIX_FMT_RGB24;
+			else if (!strncmp(opt+3, "BGR24", 5))
+				pdata->interface_pix_fmt = IPU_PIX_FMT_BGR24;
+			else if (!strncmp(opt+3, "GBR24", 5))
+				pdata->interface_pix_fmt = IPU_PIX_FMT_GBR24;
+			else if (!strncmp(opt+3, "RGB565", 6))
+				pdata->interface_pix_fmt = IPU_PIX_FMT_RGB565;
+			else if (!strncmp(opt+3, "RGB666", 6))
+				pdata->interface_pix_fmt = IPU_PIX_FMT_RGB666;
+			else if (!strncmp(opt+3, "YUV444", 6))
+				pdata->interface_pix_fmt = IPU_PIX_FMT_YUV444;
+			else if (!strncmp(opt+3, "LVDS666", 7))
+				pdata->interface_pix_fmt = IPU_PIX_FMT_LVDS666;
+			else if (!strncmp(opt+3, "YUYV16", 6))
+				pdata->interface_pix_fmt = IPU_PIX_FMT_YUYV;
+			else if (!strncmp(opt+3, "UYVY16", 6))
+				pdata->interface_pix_fmt = IPU_PIX_FMT_UYVY;
+			else if (!strncmp(opt+3, "YVYU16", 6))
+				pdata->interface_pix_fmt = IPU_PIX_FMT_YVYU;
+			else if (!strncmp(opt+3, "VYUY16", 6))
+				pdata->interface_pix_fmt = IPU_PIX_FMT_VYUY;
+		} else if (!strncmp(opt, "fbpix=", 6)) {
+			if (!strncmp(opt+6, "RGB24", 5))
+				fb_pix_fmt = IPU_PIX_FMT_RGB24;
+			else if (!strncmp(opt+6, "BGR24", 5))
+				fb_pix_fmt = IPU_PIX_FMT_BGR24;
+			else if (!strncmp(opt+6, "RGB32", 5))
+				fb_pix_fmt = IPU_PIX_FMT_RGB32;
+			else if (!strncmp(opt+6, "BGR32", 5))
+				fb_pix_fmt = IPU_PIX_FMT_BGR32;
+			else if (!strncmp(opt+6, "ABGR32", 6))
+				fb_pix_fmt = IPU_PIX_FMT_ABGR32;
+			else if (!strncmp(opt+6, "RGB565", 6))
+				fb_pix_fmt = IPU_PIX_FMT_RGB565;
+
+			if (fb_pix_fmt) {
+				pixfmt_to_var(fb_pix_fmt, &fbi->var);
+				pdata->default_bpp =
+					fbi->var.bits_per_pixel;
+			}
+		} else if (!strncmp(opt, "int_clk", 7)) {
+			pdata->int_clk = true;
+			continue;
+		} else if (!strncmp(opt, "bpp=", 4)) {
+			/* bpp setting cannot overwirte fbpix setting */
+			if (fb_pix_fmt)
+				continue;
+
+			pdata->default_bpp =
+				simple_strtoul(opt + 4, NULL, 0);
+
+			fb_pix_fmt = bpp_to_pixfmt(pdata->default_bpp);
+			if (fb_pix_fmt)
+				pixfmt_to_var(fb_pix_fmt, &fbi->var);
+		} else
+			fb_mode_str = opt;
+	}
+
+	if (fb_mode_str)
+		pdata->mode_str = fb_mode_str;
+
+	return 0;
+}
+
+static int mxcfb_register(struct fb_info *fbi)
+{
+	struct mxcfb_info *mxcfbi = (struct mxcfb_info *)fbi->par;
+	struct fb_videomode m;
+	int ret = 0;
+	char bg0_id[] = "DISP3 BG";
+	char bg1_id[] = "DISP3 BG - DI1";
+	char fg_id[] = "DISP3 FG";
+
+	if (mxcfbi->ipu_di == 0) {
+		bg0_id[4] += mxcfbi->ipu_id;
+		strcpy(fbi->fix.id, bg0_id);
+	} else if (mxcfbi->ipu_di == 1) {
+		bg1_id[4] += mxcfbi->ipu_id;
+		strcpy(fbi->fix.id, bg1_id);
+	} else { /* Overlay */
+		fg_id[4] += mxcfbi->ipu_id;
+		strcpy(fbi->fix.id, fg_id);
+	}
+
+	mxcfb_check_var(&fbi->var, fbi);
+
+	mxcfb_set_fix(fbi);
+
+	/* Added first mode to fbi modelist. */
+	if (!fbi->modelist.next || !fbi->modelist.prev)
+		INIT_LIST_HEAD(&fbi->modelist);
+	fb_var_to_videomode(&m, &fbi->var);
+	fb_add_videomode(&m, &fbi->modelist);
+
+	if (ipu_request_irq(mxcfbi->ipu, mxcfbi->ipu_ch_irq,
+		mxcfb_irq_handler, IPU_IRQF_ONESHOT, MXCFB_NAME, fbi) != 0) {
+		dev_err(fbi->device, "Error registering EOF irq handler.\n");
+		ret = -EBUSY;
+		goto err0;
+	}
+	ipu_disable_irq(mxcfbi->ipu, mxcfbi->ipu_ch_irq);
+	if (ipu_request_irq(mxcfbi->ipu, mxcfbi->ipu_ch_nf_irq,
+		mxcfb_nf_irq_handler, IPU_IRQF_ONESHOT, MXCFB_NAME, fbi) != 0) {
+		dev_err(fbi->device, "Error registering NFACK irq handler.\n");
+		ret = -EBUSY;
+		goto err1;
+	}
+	ipu_disable_irq(mxcfbi->ipu, mxcfbi->ipu_ch_nf_irq);
+
+	if (mxcfbi->ipu_alp_ch_irq != -1)
+		if (ipu_request_irq(mxcfbi->ipu, mxcfbi->ipu_alp_ch_irq,
+				mxcfb_alpha_irq_handler, IPU_IRQF_ONESHOT,
+					MXCFB_NAME, fbi) != 0) {
+			dev_err(fbi->device, "Error registering alpha irq "
+					"handler.\n");
+			ret = -EBUSY;
+			goto err2;
+		}
+
+	if (!mxcfbi->late_init) {
+		fbi->var.activate |= FB_ACTIVATE_FORCE;
+		console_lock();
+		fbi->flags |= FBINFO_MISC_USEREVENT;
+		ret = fb_set_var(fbi, &fbi->var);
+		fbi->flags &= ~FBINFO_MISC_USEREVENT;
+		console_unlock();
+		if (ret < 0) {
+			dev_err(fbi->device, "Error fb_set_var ret:%d\n", ret);
+			goto err3;
+		}
+
+		if (mxcfbi->next_blank == FB_BLANK_UNBLANK) {
+			console_lock();
+			ret = fb_blank(fbi, FB_BLANK_UNBLANK);
+			console_unlock();
+			if (ret < 0) {
+				dev_err(fbi->device,
+					"Error fb_blank ret:%d\n", ret);
+				goto err4;
+			}
+		}
+	} else {
+		/*
+		 * Setup the channel again though bootloader
+		 * has done this, then set_par() can stop the
+		 * channel neatly and re-initialize it .
+		 */
+		if (mxcfbi->next_blank == FB_BLANK_UNBLANK) {
+			console_lock();
+			_setup_disp_channel1(fbi);
+			ipu_enable_channel(mxcfbi->ipu, mxcfbi->ipu_ch);
+			console_unlock();
+		}
+	}
+
+
+	ret = register_framebuffer(fbi);
+	if (ret < 0)
+		goto err5;
+
+	return ret;
+err5:
+	if (mxcfbi->next_blank == FB_BLANK_UNBLANK) {
+		console_lock();
+		if (!mxcfbi->late_init)
+			fb_blank(fbi, FB_BLANK_POWERDOWN);
+		else {
+			ipu_disable_channel(mxcfbi->ipu, mxcfbi->ipu_ch,
+					    true);
+			ipu_uninit_channel(mxcfbi->ipu, mxcfbi->ipu_ch);
+		}
+		console_unlock();
+	}
+err4:
+err3:
+	if (mxcfbi->ipu_alp_ch_irq != -1)
+		ipu_free_irq(mxcfbi->ipu, mxcfbi->ipu_alp_ch_irq, fbi);
+err2:
+	ipu_free_irq(mxcfbi->ipu, mxcfbi->ipu_ch_nf_irq, fbi);
+err1:
+	ipu_free_irq(mxcfbi->ipu, mxcfbi->ipu_ch_irq, fbi);
+err0:
+	return ret;
+}
+
+static void mxcfb_unregister(struct fb_info *fbi)
+{
+	struct mxcfb_info *mxcfbi = (struct mxcfb_info *)fbi->par;
+
+	if (mxcfbi->ipu_alp_ch_irq != -1)
+		ipu_free_irq(mxcfbi->ipu, mxcfbi->ipu_alp_ch_irq, fbi);
+	if (mxcfbi->ipu_ch_irq)
+		ipu_free_irq(mxcfbi->ipu, mxcfbi->ipu_ch_irq, fbi);
+	if (mxcfbi->ipu_ch_nf_irq)
+		ipu_free_irq(mxcfbi->ipu, mxcfbi->ipu_ch_nf_irq, fbi);
+
+	unregister_framebuffer(fbi);
+}
+
+static int mxcfb_setup_overlay(struct platform_device *pdev,
+		struct fb_info *fbi_bg, struct resource *res)
+{
+	struct fb_info *ovfbi;
+	struct mxcfb_info *mxcfbi_bg = (struct mxcfb_info *)fbi_bg->par;
+	struct mxcfb_info *mxcfbi_fg;
+	int ret = 0;
+
+	ovfbi = mxcfb_init_fbinfo(&pdev->dev, &mxcfb_ops);
+	if (!ovfbi) {
+		ret = -ENOMEM;
+		goto init_ovfbinfo_failed;
+	}
+	mxcfbi_fg = (struct mxcfb_info *)ovfbi->par;
+
+	mxcfbi_fg->ipu = ipu_get_soc(mxcfbi_bg->ipu_id);
+	if (IS_ERR(mxcfbi_fg->ipu)) {
+		ret = -ENODEV;
+		goto get_ipu_failed;
+	}
+	mxcfbi_fg->ipu_id = mxcfbi_bg->ipu_id;
+	mxcfbi_fg->ipu_ch_irq = IPU_IRQ_FG_SYNC_EOF;
+	mxcfbi_fg->ipu_ch_nf_irq = IPU_IRQ_FG_SYNC_NFACK;
+	mxcfbi_fg->ipu_alp_ch_irq = IPU_IRQ_FG_ALPHA_SYNC_EOF;
+	mxcfbi_fg->ipu_ch = MEM_FG_SYNC;
+	mxcfbi_fg->ipu_di = -1;
+	mxcfbi_fg->ipu_di_pix_fmt = mxcfbi_bg->ipu_di_pix_fmt;
+	mxcfbi_fg->overlay = true;
+	mxcfbi_fg->cur_blank = mxcfbi_fg->next_blank = FB_BLANK_POWERDOWN;
+
+	/* Need dummy values until real panel is configured */
+	ovfbi->var.xres = 240;
+	ovfbi->var.yres = 320;
+
+	if (res && res->start && res->end) {
+		ovfbi->fix.smem_len = res->end - res->start + 1;
+		ovfbi->fix.smem_start = res->start;
+		ovfbi->screen_base = ioremap(
+					ovfbi->fix.smem_start,
+					ovfbi->fix.smem_len);
+	}
+
+	ret = mxcfb_register(ovfbi);
+	if (ret < 0)
+		goto register_ov_failed;
+
+	mxcfbi_bg->ovfbi = ovfbi;
+
+	return ret;
+
+register_ov_failed:
+get_ipu_failed:
+	fb_dealloc_cmap(&ovfbi->cmap);
+	framebuffer_release(ovfbi);
+init_ovfbinfo_failed:
+	return ret;
+}
+
+static void mxcfb_unsetup_overlay(struct fb_info *fbi_bg)
+{
+	struct mxcfb_info *mxcfbi_bg = (struct mxcfb_info *)fbi_bg->par;
+	struct fb_info *ovfbi = mxcfbi_bg->ovfbi;
+
+	mxcfb_unregister(ovfbi);
+
+	if (&ovfbi->cmap)
+		fb_dealloc_cmap(&ovfbi->cmap);
+	framebuffer_release(ovfbi);
+}
+
+static bool ipu_usage[2][2];
+static int ipu_test_set_usage(int ipu, int di)
+{
+	if (ipu_usage[ipu][di])
+		return -EBUSY;
+	else
+		ipu_usage[ipu][di] = true;
+	return 0;
+}
+
+static void ipu_clear_usage(int ipu, int di)
+{
+	ipu_usage[ipu][di] = false;
+}
+
+static int mxcfb_get_of_property(struct platform_device *pdev,
+				struct ipuv3_fb_platform_data *plat_data)
+{
+	struct device_node *np = pdev->dev.of_node;
+	const char *disp_dev;
+	const char *mode_str;
+	const char *pixfmt;
+	int err;
+	int len;
+	u32 bpp, int_clk;
+	u32 late_init;
+
+	err = of_property_read_string(np, "disp_dev", &disp_dev);
+	if (err < 0) {
+		dev_dbg(&pdev->dev, "get of property disp_dev fail\n");
+		return err;
+	}
+	err = of_property_read_string(np, "mode_str", &mode_str);
+	if (err < 0) {
+		dev_dbg(&pdev->dev, "get of property mode_str fail\n");
+		return err;
+	}
+	err = of_property_read_string(np, "interface_pix_fmt", &pixfmt);
+	if (err) {
+		dev_dbg(&pdev->dev, "get of property pix fmt fail\n");
+		return err;
+	}
+	err = of_property_read_u32(np, "default_bpp", &bpp);
+	if (err) {
+		dev_dbg(&pdev->dev, "get of property bpp fail\n");
+		return err;
+	}
+	err = of_property_read_u32(np, "int_clk", &int_clk);
+	if (err) {
+		dev_dbg(&pdev->dev, "get of property int_clk fail\n");
+		return err;
+	}
+	err = of_property_read_u32(np, "late_init", &late_init);
+	if (err) {
+		dev_dbg(&pdev->dev, "get of property late_init fail\n");
+		return err;
+	}
+
+	if (!strncmp(pixfmt, "RGB24", 5))
+		plat_data->interface_pix_fmt = IPU_PIX_FMT_RGB24;
+	else if (!strncmp(pixfmt, "BGR24", 5))
+		plat_data->interface_pix_fmt = IPU_PIX_FMT_BGR24;
+	else if (!strncmp(pixfmt, "GBR24", 5))
+		plat_data->interface_pix_fmt = IPU_PIX_FMT_GBR24;
+	else if (!strncmp(pixfmt, "RGB565", 6))
+		plat_data->interface_pix_fmt = IPU_PIX_FMT_RGB565;
+	else if (!strncmp(pixfmt, "RGB666", 6))
+		plat_data->interface_pix_fmt = IPU_PIX_FMT_RGB666;
+	else if (!strncmp(pixfmt, "YUV444", 6))
+		plat_data->interface_pix_fmt = IPU_PIX_FMT_YUV444;
+	else if (!strncmp(pixfmt, "LVDS666", 7))
+		plat_data->interface_pix_fmt = IPU_PIX_FMT_LVDS666;
+	else if (!strncmp(pixfmt, "YUYV16", 6))
+		plat_data->interface_pix_fmt = IPU_PIX_FMT_YUYV;
+	else if (!strncmp(pixfmt, "UYVY16", 6))
+		plat_data->interface_pix_fmt = IPU_PIX_FMT_UYVY;
+	else if (!strncmp(pixfmt, "YVYU16", 6))
+		plat_data->interface_pix_fmt = IPU_PIX_FMT_YVYU;
+	else if (!strncmp(pixfmt, "VYUY16", 6))
+				plat_data->interface_pix_fmt = IPU_PIX_FMT_VYUY;
+	else {
+		dev_err(&pdev->dev, "err interface_pix_fmt!\n");
+		return -ENOENT;
+	}
+
+	len = min(sizeof(plat_data->disp_dev) - 1, strlen(disp_dev));
+	memcpy(plat_data->disp_dev, disp_dev, len);
+	plat_data->disp_dev[len] = '\0';
+	plat_data->mode_str = (char *)mode_str;
+	plat_data->default_bpp = bpp;
+	plat_data->int_clk = (bool)int_clk;
+	plat_data->late_init = (bool)late_init;
+	return err;
+}
+
+/*!
+ * Probe routine for the framebuffer driver. It is called during the
+ * driver binding process.      The following functions are performed in
+ * this routine: Framebuffer initialization, Memory allocation and
+ * mapping, Framebuffer registration, IPU initialization.
+ *
+ * @return      Appropriate error code to the kernel common code
+ */
+static int mxcfb_probe(struct platform_device *pdev)
+{
+	struct ipuv3_fb_platform_data *plat_data;
+	struct fb_info *fbi;
+	struct mxcfb_info *mxcfbi;
+	struct resource *res;
+	int ret = 0;
+
+	dev_dbg(&pdev->dev, "%s enter\n", __func__);
+	pdev->id = of_alias_get_id(pdev->dev.of_node, "mxcfb");
+	if (pdev->id < 0) {
+		dev_err(&pdev->dev, "can not get alias id\n");
+		return pdev->id;
+	}
+
+	plat_data = devm_kzalloc(&pdev->dev, sizeof(struct
+					ipuv3_fb_platform_data), GFP_KERNEL);
+	if (!plat_data)
+		return -ENOMEM;
+	pdev->dev.platform_data = plat_data;
+
+	ret = mxcfb_get_of_property(pdev, plat_data);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "get mxcfb of property fail\n");
+		return ret;
+	}
+
+	/* Initialize FB structures */
+	fbi = mxcfb_init_fbinfo(&pdev->dev, &mxcfb_ops);
+	if (!fbi) {
+		ret = -ENOMEM;
+		goto init_fbinfo_failed;
+	}
+
+	ret = mxcfb_option_setup(pdev, fbi);
+	if (ret)
+		goto get_fb_option_failed;
+
+	mxcfbi = (struct mxcfb_info *)fbi->par;
+	mxcfbi->ipu_int_clk = plat_data->int_clk;
+	mxcfbi->late_init = plat_data->late_init;
+	mxcfbi->first_set_par = true;
+	ret = mxcfb_dispdrv_init(pdev, fbi);
+	if (ret < 0)
+		goto init_dispdrv_failed;
+
+	ret = ipu_test_set_usage(mxcfbi->ipu_id, mxcfbi->ipu_di);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "ipu%d-di%d already in use\n",
+				mxcfbi->ipu_id, mxcfbi->ipu_di);
+		goto ipu_in_busy;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res && res->start && res->end) {
+		fbi->fix.smem_len = res->end - res->start + 1;
+		fbi->fix.smem_start = res->start;
+		fbi->screen_base = ioremap(fbi->fix.smem_start, fbi->fix.smem_len);
+		/* Do not clear the fb content drawn in bootloader. */
+		if (!mxcfbi->late_init)
+			memset(fbi->screen_base, 0, fbi->fix.smem_len);
+	}
+
+	mxcfbi->ipu = ipu_get_soc(mxcfbi->ipu_id);
+	if (IS_ERR(mxcfbi->ipu)) {
+		ret = -ENODEV;
+		goto get_ipu_failed;
+	}
+
+	/* first user uses DP with alpha feature */
+	if (!g_dp_in_use[mxcfbi->ipu_id]) {
+		mxcfbi->ipu_ch_irq = IPU_IRQ_BG_SYNC_EOF;
+		mxcfbi->ipu_ch_nf_irq = IPU_IRQ_BG_SYNC_NFACK;
+		mxcfbi->ipu_alp_ch_irq = IPU_IRQ_BG_ALPHA_SYNC_EOF;
+		mxcfbi->ipu_ch = MEM_BG_SYNC;
+		/* Unblank the primary fb only by default */
+		if (pdev->id == 0)
+			mxcfbi->cur_blank = mxcfbi->next_blank = FB_BLANK_UNBLANK;
+		else
+			mxcfbi->cur_blank = mxcfbi->next_blank = FB_BLANK_POWERDOWN;
+
+		ret = mxcfb_register(fbi);
+		if (ret < 0)
+			goto mxcfb_register_failed;
+
+		ipu_disp_set_global_alpha(mxcfbi->ipu, mxcfbi->ipu_ch,
+					  true, 0x80);
+		ipu_disp_set_color_key(mxcfbi->ipu, mxcfbi->ipu_ch, false, 0);
+
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+		ret = mxcfb_setup_overlay(pdev, fbi, res);
+
+		if (ret < 0) {
+			mxcfb_unregister(fbi);
+			goto mxcfb_setupoverlay_failed;
+		}
+
+		g_dp_in_use[mxcfbi->ipu_id] = true;
+
+		ret = device_create_file(mxcfbi->ovfbi->dev,
+					 &dev_attr_fsl_disp_property);
+		if (ret)
+			dev_err(mxcfbi->ovfbi->dev, "Error %d on creating "
+						    "file for disp property\n",
+						    ret);
+
+		ret = device_create_file(mxcfbi->ovfbi->dev,
+					 &dev_attr_fsl_disp_dev_property);
+		if (ret)
+			dev_err(mxcfbi->ovfbi->dev, "Error %d on creating "
+						    "file for disp device "
+						    "propety\n", ret);
+	} else {
+		mxcfbi->ipu_ch_irq = IPU_IRQ_DC_SYNC_EOF;
+		mxcfbi->ipu_ch_nf_irq = IPU_IRQ_DC_SYNC_NFACK;
+		mxcfbi->ipu_alp_ch_irq = -1;
+		mxcfbi->ipu_ch = MEM_DC_SYNC;
+		mxcfbi->cur_blank = mxcfbi->next_blank = FB_BLANK_POWERDOWN;
+
+		ret = mxcfb_register(fbi);
+		if (ret < 0)
+			goto mxcfb_register_failed;
+	}
+
+	platform_set_drvdata(pdev, fbi);
+
+	ret = device_create_file(fbi->dev, &dev_attr_fsl_disp_property);
+	if (ret)
+		dev_err(&pdev->dev, "Error %d on creating file for disp "
+				    "property\n", ret);
+
+	ret = device_create_file(fbi->dev, &dev_attr_fsl_disp_dev_property);
+	if (ret)
+		dev_err(&pdev->dev, "Error %d on creating file for disp "
+				    " device propety\n", ret);
+
+	return 0;
+
+mxcfb_setupoverlay_failed:
+mxcfb_register_failed:
+get_ipu_failed:
+	ipu_clear_usage(mxcfbi->ipu_id, mxcfbi->ipu_di);
+ipu_in_busy:
+init_dispdrv_failed:
+	fb_dealloc_cmap(&fbi->cmap);
+	framebuffer_release(fbi);
+get_fb_option_failed:
+init_fbinfo_failed:
+	return ret;
+}
+
+static int mxcfb_remove(struct platform_device *pdev)
+{
+	struct fb_info *fbi = platform_get_drvdata(pdev);
+	struct mxcfb_info *mxc_fbi = fbi->par;
+
+	if (!fbi)
+		return 0;
+
+	device_remove_file(fbi->dev, &dev_attr_fsl_disp_dev_property);
+	device_remove_file(fbi->dev, &dev_attr_fsl_disp_property);
+	mxcfb_blank(FB_BLANK_POWERDOWN, fbi);
+	mxcfb_unregister(fbi);
+	mxcfb_unmap_video_memory(fbi);
+
+	if (mxc_fbi->ovfbi) {
+		device_remove_file(mxc_fbi->ovfbi->dev,
+				   &dev_attr_fsl_disp_dev_property);
+		device_remove_file(mxc_fbi->ovfbi->dev,
+				   &dev_attr_fsl_disp_property);
+		mxcfb_blank(FB_BLANK_POWERDOWN, mxc_fbi->ovfbi);
+		mxcfb_unsetup_overlay(fbi);
+		mxcfb_unmap_video_memory(mxc_fbi->ovfbi);
+	}
+
+	ipu_clear_usage(mxc_fbi->ipu_id, mxc_fbi->ipu_di);
+	if (&fbi->cmap)
+		fb_dealloc_cmap(&fbi->cmap);
+	framebuffer_release(fbi);
+	return 0;
+}
+
+static const struct of_device_id imx_mxcfb_dt_ids[] = {
+	{ .compatible = "fsl,mxc_sdc_fb"},
+	{ /* sentinel */ }
+};
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxcfb_driver = {
+	.driver = {
+		.name = MXCFB_NAME,
+		.of_match_table	= imx_mxcfb_dt_ids,
+	},
+	.probe = mxcfb_probe,
+	.remove = mxcfb_remove,
+	.suspend = mxcfb_suspend,
+	.resume = mxcfb_resume,
+};
+
+/*!
+ * Main entry function for the framebuffer. The function registers the power
+ * management callback functions with the kernel and also registers the MXCFB
+ * callback functions with the core Linux framebuffer driver \b fbmem.c
+ *
+ * @return      Error code indicating success or failure
+ */
+int __init mxcfb_init(void)
+{
+	return platform_driver_register(&mxcfb_driver);
+}
+
+void mxcfb_exit(void)
+{
+	platform_driver_unregister(&mxcfb_driver);
+}
+
+module_init(mxcfb_init);
+module_exit(mxcfb_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC framebuffer driver");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("fb");
diff --git a/drivers/video/mxc/mxc_lcdif.c b/drivers/video/mxc/mxc_lcdif.c
new file mode 100644
index 000000000000..a0cca1c347fb
--- /dev/null
+++ b/drivers/video/mxc/mxc_lcdif.c
@@ -0,0 +1,235 @@
+/*
+ * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/init.h>
+#include <linux/ipu.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mxcfb.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+
+#include "mxc_dispdrv.h"
+
+struct mxc_lcd_platform_data {
+	u32 default_ifmt;
+	u32 ipu_id;
+	u32 disp_id;
+};
+
+struct mxc_lcdif_data {
+	struct platform_device *pdev;
+	struct mxc_dispdrv_handle *disp_lcdif;
+};
+
+#define DISPDRV_LCD	"lcd"
+
+static struct fb_videomode lcdif_modedb[] = {
+	{
+	/* 800x480 @ 57 Hz , pixel clk @ 27MHz */
+	"CLAA-WVGA", 57, 800, 480, 37037, 40, 60, 10, 10, 20, 10,
+	FB_SYNC_CLK_LAT_FALL,
+	FB_VMODE_NONINTERLACED,
+	0,},
+	{
+	/* 800x480 @ 60 Hz , pixel clk @ 32MHz */
+	"SEIKO-WVGA", 60, 800, 480, 29850, 89, 164, 23, 10, 10, 10,
+	FB_SYNC_CLK_LAT_FALL,
+	FB_VMODE_NONINTERLACED,
+	0,},
+};
+static int lcdif_modedb_sz = ARRAY_SIZE(lcdif_modedb);
+
+static int lcdif_init(struct mxc_dispdrv_handle *disp,
+	struct mxc_dispdrv_setting *setting)
+{
+	int ret, i;
+	struct mxc_lcdif_data *lcdif = mxc_dispdrv_getdata(disp);
+	struct mxc_lcd_platform_data *plat_data
+			= lcdif->pdev->dev.platform_data;
+	struct fb_videomode *modedb = lcdif_modedb;
+	int modedb_sz = lcdif_modedb_sz;
+
+	/* use platform defined ipu/di */
+	setting->dev_id = plat_data->ipu_id;
+	setting->disp_id = plat_data->disp_id;
+
+	ret = fb_find_mode(&setting->fbi->var, setting->fbi, setting->dft_mode_str,
+				modedb, modedb_sz, NULL, setting->default_bpp);
+	if (!ret) {
+		fb_videomode_to_var(&setting->fbi->var, &modedb[0]);
+		setting->if_fmt = plat_data->default_ifmt;
+	}
+
+	INIT_LIST_HEAD(&setting->fbi->modelist);
+	for (i = 0; i < modedb_sz; i++) {
+		struct fb_videomode m;
+		fb_var_to_videomode(&m, &setting->fbi->var);
+		if (fb_mode_is_equal(&m, &modedb[i])) {
+			fb_add_videomode(&modedb[i],
+					&setting->fbi->modelist);
+			break;
+		}
+	}
+
+	return ret;
+}
+
+void lcdif_deinit(struct mxc_dispdrv_handle *disp)
+{
+	/*TODO*/
+}
+
+static struct mxc_dispdrv_driver lcdif_drv = {
+	.name 	= DISPDRV_LCD,
+	.init 	= lcdif_init,
+	.deinit	= lcdif_deinit,
+};
+
+static int lcd_get_of_property(struct platform_device *pdev,
+				struct mxc_lcd_platform_data *plat_data)
+{
+	struct device_node *np = pdev->dev.of_node;
+	int err;
+	u32 ipu_id, disp_id;
+	const char *default_ifmt;
+
+	err = of_property_read_string(np, "default_ifmt", &default_ifmt);
+	if (err) {
+		dev_dbg(&pdev->dev, "get of property default_ifmt fail\n");
+		return err;
+	}
+	err = of_property_read_u32(np, "ipu_id", &ipu_id);
+	if (err) {
+		dev_dbg(&pdev->dev, "get of property ipu_id fail\n");
+		return err;
+	}
+	err = of_property_read_u32(np, "disp_id", &disp_id);
+	if (err) {
+		dev_dbg(&pdev->dev, "get of property disp_id fail\n");
+		return err;
+	}
+
+	plat_data->ipu_id = ipu_id;
+	plat_data->disp_id = disp_id;
+	if (!strncmp(default_ifmt, "RGB24", 5))
+		plat_data->default_ifmt = IPU_PIX_FMT_RGB24;
+	else if (!strncmp(default_ifmt, "BGR24", 5))
+		plat_data->default_ifmt = IPU_PIX_FMT_BGR24;
+	else if (!strncmp(default_ifmt, "GBR24", 5))
+		plat_data->default_ifmt = IPU_PIX_FMT_GBR24;
+	else if (!strncmp(default_ifmt, "RGB565", 6))
+		plat_data->default_ifmt = IPU_PIX_FMT_RGB565;
+	else if (!strncmp(default_ifmt, "RGB666", 6))
+		plat_data->default_ifmt = IPU_PIX_FMT_RGB666;
+	else if (!strncmp(default_ifmt, "YUV444", 6))
+		plat_data->default_ifmt = IPU_PIX_FMT_YUV444;
+	else if (!strncmp(default_ifmt, "LVDS666", 7))
+		plat_data->default_ifmt = IPU_PIX_FMT_LVDS666;
+	else if (!strncmp(default_ifmt, "YUYV16", 6))
+		plat_data->default_ifmt = IPU_PIX_FMT_YUYV;
+	else if (!strncmp(default_ifmt, "UYVY16", 6))
+		plat_data->default_ifmt = IPU_PIX_FMT_UYVY;
+	else if (!strncmp(default_ifmt, "YVYU16", 6))
+		plat_data->default_ifmt = IPU_PIX_FMT_YVYU;
+	else if (!strncmp(default_ifmt, "VYUY16", 6))
+				plat_data->default_ifmt = IPU_PIX_FMT_VYUY;
+	else {
+		dev_err(&pdev->dev, "err default_ifmt!\n");
+		return -ENOENT;
+	}
+
+	return err;
+}
+
+static int mxc_lcdif_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct pinctrl *pinctrl;
+	struct mxc_lcdif_data *lcdif;
+	struct mxc_lcd_platform_data *plat_data;
+
+	dev_dbg(&pdev->dev, "%s enter\n", __func__);
+	lcdif = devm_kzalloc(&pdev->dev, sizeof(struct mxc_lcdif_data),
+				GFP_KERNEL);
+	if (!lcdif)
+		return -ENOMEM;
+	plat_data = devm_kzalloc(&pdev->dev,
+				sizeof(struct mxc_lcd_platform_data),
+				GFP_KERNEL);
+	if (!plat_data)
+		return -ENOMEM;
+	pdev->dev.platform_data = plat_data;
+
+	ret = lcd_get_of_property(pdev, plat_data);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "get lcd of property fail\n");
+		return ret;
+	}
+
+	pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
+	if (IS_ERR(pinctrl)) {
+		dev_err(&pdev->dev, "can't get/select pinctrl\n");
+		return PTR_ERR(pinctrl);
+	}
+
+	lcdif->pdev = pdev;
+	lcdif->disp_lcdif = mxc_dispdrv_register(&lcdif_drv);
+	mxc_dispdrv_setdata(lcdif->disp_lcdif, lcdif);
+
+	dev_set_drvdata(&pdev->dev, lcdif);
+	dev_dbg(&pdev->dev, "%s exit\n", __func__);
+
+	return ret;
+}
+
+static int mxc_lcdif_remove(struct platform_device *pdev)
+{
+	struct mxc_lcdif_data *lcdif = dev_get_drvdata(&pdev->dev);
+
+	mxc_dispdrv_puthandle(lcdif->disp_lcdif);
+	mxc_dispdrv_unregister(lcdif->disp_lcdif);
+	kfree(lcdif);
+	return 0;
+}
+
+static const struct of_device_id imx_lcd_dt_ids[] = {
+	{ .compatible = "fsl,lcd"},
+	{ /* sentinel */ }
+};
+static struct platform_driver mxc_lcdif_driver = {
+	.driver = {
+		.name = "mxc_lcdif",
+		.of_match_table	= imx_lcd_dt_ids,
+	},
+	.probe = mxc_lcdif_probe,
+	.remove = mxc_lcdif_remove,
+};
+
+static int __init mxc_lcdif_init(void)
+{
+	return platform_driver_register(&mxc_lcdif_driver);
+}
+
+static void __exit mxc_lcdif_exit(void)
+{
+	platform_driver_unregister(&mxc_lcdif_driver);
+}
+
+module_init(mxc_lcdif_init);
+module_exit(mxc_lcdif_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("i.MX ipuv3 LCD extern port driver");
+MODULE_LICENSE("GPL");