Merge remote branch 'fsl-linux-sdk/imx_3.0.35_12.11.01' into imx_3.0.35_android_r13.4.y

Conflicts:
	arch/arm/configs/imx6s_updater_defconfig
	arch/arm/include/asm/hardware/coresight.h
	arch/arm/kernel/etm.c
	arch/arm/mach-mx6/board-mx6q_sabresd.c
	arch/arm/mach-mx6/cpu_op-mx6.c
	arch/arm/mach-mx6/mx6_suspend.S
	arch/arm/mach-mx6/mx6sl_wfi.S
	arch/arm/mach-mx6/pm.c
	arch/arm/mach-mx6/system.c
	arch/arm/plat-mxc/cpufreq.c
	drivers/media/video/mxc/capture/ov5640_mipi.c
	drivers/mfd/mxc-hdmi-core.c
	drivers/mmc/host/sdhci.c
	drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_driver.c
	drivers/power/sabresd_battery.c
	drivers/video/mxc/mxc_ipuv3_fb.c
	include/linux/mfd/mxc-hdmi-core.h

72 files changed, 3674 insertions, 957 deletions

diff --git a/drivers/char/fsl_otp.c b/drivers/char/fsl_otp.c
index 05ad55e9d6a9..7021c42c4bfd 100755
--- a/drivers/char/fsl_otp.c
+++ b/drivers/char/fsl_otp.c
@@ -1,7 +1,7 @@
 /*
  * Freescale On-Chip OTP driver
  *
- * Copyright (C) 2010-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2010-2012 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
@@ -153,6 +153,7 @@ static int __init alloc_otp_attr(struct mxc_otp_platform_data *pdata)
 		goto error_out;
 
 	for (i = 0; i < otp_data->fuse_num; i++) {
+		sysfs_attr_init(&kattr[i].attr);
 		kattr[i].attr.name = pdata->fuse_name[i];
 		kattr[i].attr.mode = 0600;
 		kattr[i].show  = otp_show;
diff --git a/drivers/dma/imx-sdma.c b/drivers/dma/imx-sdma.c
index e93c4ea1a1a8..9de94344a97f 100644
--- a/drivers/dma/imx-sdma.c
+++ b/drivers/dma/imx-sdma.c
@@ -32,6 +32,7 @@
 #include <linux/slab.h>
 #include <linux/platform_device.h>
 #include <linux/dmaengine.h>
+#include <linux/delay.h>
 
 #include <asm/irq.h>
 #include <mach/sdma.h>
@@ -278,6 +279,7 @@ struct sdma_channel {
 	enum dma_status			status;
 	unsigned int			chn_count;
 	unsigned int			chn_real_count;
+	unsigned int			irq_handling;
 };
 
 #define MAX_DMA_CHANNELS 32
@@ -324,6 +326,7 @@ struct sdma_engine {
 	struct dma_device		dma_device;
 	struct clk			*clk;
 	struct sdma_script_start_addrs	*script_addrs;
+	spinlock_t			irq_reg_lock;
 };
 
 #define SDMA_H_CONFIG_DSPDMA	(1 << 12) /* indicates if the DSPDMA is used */
@@ -537,18 +540,33 @@ static void mxc_sdma_handle_channel(struct sdma_channel *sdmac)
 static irqreturn_t sdma_int_handler(int irq, void *dev_id)
 {
 	struct sdma_engine *sdma = dev_id;
-	u32 stat;
+	struct sdma_channel *sdmac;
+	unsigned long flag;
+	int channel;
+	u32 stat, stat_bak;
 
+	spin_lock_irqsave(&sdma->irq_reg_lock, flag);
 	stat = readl_relaxed(sdma->regs + SDMA_H_INTR);
 	writel_relaxed(stat, sdma->regs + SDMA_H_INTR);
+	spin_unlock_irqrestore(&sdma->irq_reg_lock, flag);
+
+	stat_bak = stat;
+	while (stat_bak) {
+		channel = fls(stat_bak) - 1;
+		sdmac = &sdma->channel[channel];
+		sdmac->irq_handling = 1;
+		stat_bak &= ~(1 << channel);
+	}
 
 	while (stat) {
-		int channel = fls(stat) - 1;
-		struct sdma_channel *sdmac = &sdma->channel[channel];
+		channel = fls(stat) - 1;
+		sdmac = &sdma->channel[channel];
 
-		mxc_sdma_handle_channel(sdmac);
+		if (sdmac->irq_handling)
+			mxc_sdma_handle_channel(sdmac);
 
 		stat &= ~(1 << channel);
+		sdmac->irq_handling = 0;
 	}
 
 	return IRQ_HANDLED;
@@ -878,6 +896,8 @@ static int sdma_request_channel(struct sdma_channel *sdmac)
 
 	sdmac->buf_tail = 0;
 
+	sdmac->irq_handling = 0;
+
 	return 0;
 out:
 
@@ -973,11 +993,41 @@ static int sdma_alloc_chan_resources(struct dma_chan *chan)
 	return 0;
 }
 
+static void sdma_irq_pending_check(struct sdma_channel *sdmac)
+{
+	struct sdma_engine *sdma = sdmac->sdma;
+	unsigned long flag;
+	u32 stat;
+
+	spin_lock_irqsave(&sdma->irq_reg_lock, flag);
+	stat = readl_relaxed(sdma->regs + SDMA_H_INTR);
+
+	/*Check if the current channel's IRQ hasn't been responded*/
+	if (stat & (1 << sdmac->channel)) {
+		/*Handle the irq manually*/
+		writel_relaxed(1 << sdmac->channel, sdma->regs + SDMA_H_INTR);
+		spin_unlock_irqrestore(&sdma->irq_reg_lock, flag);
+
+		/*Prevent irq_handler from doing handle_channel() again*/
+		sdmac->irq_handling = 0;
+		mxc_sdma_handle_channel(sdmac);
+	} else {
+		spin_unlock_irqrestore(&sdma->irq_reg_lock, flag);
+	}
+
+	/*Wait here until irq_handler's finished*/
+	while (sdmac->irq_handling)
+		udelay(100);
+}
+
 static void sdma_free_chan_resources(struct dma_chan *chan)
 {
 	struct sdma_channel *sdmac = to_sdma_chan(chan);
 	struct sdma_engine *sdma = sdmac->sdma;
 
+	/*Check if irq to the channel is still pending*/
+	sdma_irq_pending_check(sdmac);
+
 	sdma_disable_channel(sdmac);
 
 	if (sdmac->event_id0)
@@ -1449,6 +1499,8 @@ static int __init sdma_probe(struct platform_device *pdev)
 	dma_cap_set(DMA_SLAVE, sdma->dma_device.cap_mask);
 	dma_cap_set(DMA_CYCLIC, sdma->dma_device.cap_mask);
 
+	spin_lock_init(&sdma->irq_reg_lock);
+
 	INIT_LIST_HEAD(&sdma->dma_device.channels);
 	/* Initialize channel parameters */
 	for (i = 0; i < MAX_DMA_CHANNELS; i++) {
diff --git a/drivers/dma/mxs-dma.c b/drivers/dma/mxs-dma.c
index 162e6c3eb64d..e604cd7ae384 100644
--- a/drivers/dma/mxs-dma.c
+++ b/drivers/dma/mxs-dma.c
@@ -323,6 +323,7 @@ static int mxs_dma_alloc_chan_resources(struct dma_chan *chan)
 	/* the descriptor is ready */
 	async_tx_ack(&mxs_chan->desc);
 
+	clk_disable_unprepare(mxs_dma->clk);
 	return 0;
 
 err_clk:
@@ -339,6 +340,7 @@ static void mxs_dma_free_chan_resources(struct dma_chan *chan)
 	struct mxs_dma_chan *mxs_chan = to_mxs_dma_chan(chan);
 	struct mxs_dma_engine *mxs_dma = mxs_chan->mxs_dma;
 
+	clk_prepare_enable(mxs_dma->clk);
 	mxs_dma_disable_chan(mxs_chan);
 
 	free_irq(mxs_chan->chan_irq, mxs_dma);
diff --git a/drivers/dma/pxp/pxp_dma_v2.c b/drivers/dma/pxp/pxp_dma_v2.c
index 87b8f558ae8c..b74f62cac032 100644
--- a/drivers/dma/pxp/pxp_dma_v2.c
+++ b/drivers/dma/pxp/pxp_dma_v2.c
@@ -264,6 +264,9 @@ static void pxp_set_ctrl(struct pxps *pxp)
 	case PXP_PIX_FMT_YUV422P:
 		fmt_ctrl = BV_PXP_PS_CTRL_FORMAT__YUV422;
 		break;
+	case PXP_PIX_FMT_UYVY:
+		fmt_ctrl = BV_PXP_PS_CTRL_FORMAT__UYVY1P422;
+		break;
 	default:
 		fmt_ctrl = 0;
 	}
@@ -654,7 +657,7 @@ static void pxp_set_csc(struct pxps *pxp)
 
 		/* CSC1 - YUV->RGB */
 		__raw_writel(0x84ab01f0, pxp->base + HW_PXP_CSC1_COEF0);
-		__raw_writel(0x01230204, pxp->base + HW_PXP_CSC1_COEF1);
+		__raw_writel(0x01980204, pxp->base + HW_PXP_CSC1_COEF1);
 		__raw_writel(0x0730079c, pxp->base + HW_PXP_CSC1_COEF2);
 
 		/* CSC2 - Bypass */
diff --git a/drivers/media/video/mxc/capture/Kconfig b/drivers/media/video/mxc/capture/Kconfig
index a7b14cb23b9b..243b888c0f4f 100644
--- a/drivers/media/video/mxc/capture/Kconfig
+++ b/drivers/media/video/mxc/capture/Kconfig
@@ -13,7 +13,7 @@ config VIDEO_MXC_EMMA_CAMERA
 	default y
 
 config VIDEO_MXC_CSI_CAMERA
-	tristate "MX25 CSI camera support"
+	tristate "CSI camera support"
 	depends on !VIDEO_MXC_EMMA_CAMERA
 
 config VIDEO_MXC_CSI_DMA
@@ -116,6 +116,16 @@ config MXC_CAMERA_OV5640_MIPI
 	---help---
 	  If you plan to use the ov5640 Camera with mipi interface in your MXC system, say Y here.
 
+config MXC_MIPI_CSI2_TVIN_ADV7280
+	tristate "Analog Devices adv7280 TV Decoder Input support using mipi-csi2"
+	depends on (!VIDEO_MXC_EMMA_CAMERA)
+	depends on ARCH_MX6Q
+	select MXC_MIPI_CSI2 if ARCH_MX6Q
+	select MXC_CAMERA_SENSOR_CLK
+	---help---
+	  If you plan to use the adv7280 device with mipi-csi2 interface
+	  in your iMX6 system, say Y here.
+
 config MXC_CAMERA_SENSOR_CLK
 	tristate "camera clock"
 	depends on !VIDEO_MXC_EMMA_CAMERA
diff --git a/drivers/media/video/mxc/capture/Makefile b/drivers/media/video/mxc/capture/Makefile
index bdef5b401485..7f8b168f6f5e 100644
--- a/drivers/media/video/mxc/capture/Makefile
+++ b/drivers/media/video/mxc/capture/Makefile
@@ -52,3 +52,6 @@ obj-$(CONFIG_MXC_CAMERA_OV8820_MIPI) += ov8820_camera_mipi.o
 
 adv7180_tvin-objs := adv7180.o
 obj-$(CONFIG_MXC_TVIN_ADV7180) += adv7180_tvin.o
+
+adv7280_tvin-objs := adv7280_mipi_tvin.o
+obj-$(CONFIG_MXC_MIPI_CSI2_TVIN_ADV7280) += adv7280_mipi_tvin.o
diff --git a/drivers/media/video/mxc/capture/adv7280_mipi_tvin.c b/drivers/media/video/mxc/capture/adv7280_mipi_tvin.c
new file mode 100644
index 000000000000..d4e1d2ddc16b
--- /dev/null
+++ b/drivers/media/video/mxc/capture/adv7280_mipi_tvin.c
@@ -0,0 +1,281 @@
+/*
+ * Copyright (C) 2012 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 adv7280_mipi_csi2_tvin.c
+ *
+ * @brief Analog Device adv7280 video decoder functions
+ *
+ * @ingroup Camera
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/fsl_devices.h>
+
+#define ADV7280_SAMPLE_SILICON         0x40
+#define ADV7280_PROD_SILICON           0x41
+/** ADV7280 register definitions */
+#define ADV7280_INPUT_CTL              0x00	/* Input Control */
+#define ADV7280_STATUS_1               0x10	/* Status #1 */
+#define ADV7280_BRIGHTNESS             0x0a	/* Brightness */
+#define ADV7280_IDENT                  0x11	/* IDENT */
+#define ADV7280_VSYNC_FIELD_CTL_1      0x31	/* VSYNC Field Control #1 */
+#define ADV7280_MANUAL_WIN_CTL         0x3d	/* Manual Window Control */
+#define ADV7280_SD_SATURATION_CB       0xe3	/* SD Saturation Cb */
+#define ADV7280_SD_SATURATION_CR       0xe4	/* SD Saturation Cr */
+#define ADV7280_PM_REG                 0x0f /* Power Management */
+#define ADV7280_ADI_CONTROL1           0x0E /* ADI Control 1 */
+/* adv7280 power management reset bit */
+#define ADV7280_PM_RESET_BIT           0x80
+/** adv7280 input video masks */
+#define ADV7280_INPUT_VIDEO_MASK       0x70 /* input video mask */
+#define ADV7280_PAL_MODE_BIT           0x40 /* PAL mode bit     */
+#define ADV7280_PAL_M_MODE_BIT         0x20 /* PAL M mode bit   */
+#define ADV7280_NTSC_MODE_BIT          0x00 /* NTSC mode bit    */
+#define ADV7280_NTSC_4_43_MODE_BIT     0x10 /* NTSC 4.43 mode bit*/
+/** adv7280 voltages */
+#define ADV7280_VOLTAGE_ANALOG         2800000
+#define ADV7280_VOLTAGE_DIGITAL_CORE   1500000
+#define ADV7280_VOLTAGE_DIGITAL_IO     1800000
+
+struct reg_value {
+	u8 reg;
+	u8 value;
+	u8 mask;
+	u32 delay_ms;
+};
+
+struct adv7280_priv {
+	struct fsl_mxc_tvin_platform_data *pdata;
+	struct i2c_client *client;
+};
+
+/*!
+ *  adv7280 initialization register structure
+ *  registers are configured based on ADI RM
+ *  1st value = Register Address
+ *  2nd value = default value
+ *  3rd value = Register Mask
+ *  4th value = delay time to wait until next command
+ */
+
+static struct reg_value adv7280_init_params[] = {
+	{0x0F, 0x04, 0x00, 0}, /* exit power down mode, no pwrdwn line available
+						   in imx6-AI use i2c powrdwn */
+	{0x00, 0x0E, 0x00, 0}, /* input control */
+	{0x03, 0x0C, 0x00, 0}, /* enable pixel and sync output drivers */
+	{0x04, 0x37, 0x00, 0}, /* enable SFL */
+	{0x13, 0x00, 0x00, 0}, /* enable INTRQ output driver */
+	{0x17, 0x41, 0x00, 0}, /* select SH1 */
+	{0x1D, 0x40, 0x00, 0}, /* enable LCC output driver */
+	{0x52, 0xC0, 0x00, 0}, /* ADI recommended*/
+	{0xFE, 0xA0, 0x00, 0}, /* set CSI-Tx slave address to 0xA0 */
+	{0x59, 0x15, 0x00, 0}, /* GPO control */
+};
+
+/*! Read one register from a ADV7280 i2c slave device.
+ *  @param *reg     register in the device we wish to access.
+ *  @return         0 if success, an error code otherwise.
+ */
+static inline int adv7280_read_reg(struct adv7280_priv *adv7280, u8 reg)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(adv7280->client, reg);
+	if (ret < 0) {
+		dev_dbg(&adv7280->client->dev, "%s:read reg error: reg=%2x\n",
+				__func__, reg);
+	}
+
+	return ret;
+}
+
+/*! Write one register from a ADV7280 i2c slave device.
+ *  @param *reg     register in the device we wish to access.
+ *  @return         0 if success, an error code otherwise.
+ */
+static inline int adv7280_write_reg(struct adv7280_priv *adv7280,
+		u8 reg, u8 val)
+{
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(adv7280->client, reg, val);
+	if (ret < 0) {
+		dev_dbg(&adv7280->client->dev, "%s:write reg error: reg=%2x\n",
+				__func__, reg);
+	}
+
+	return ret;
+}
+
+/*! Write ADV7280 config paramater array
+ */
+static int adv7280_config(struct adv7280_priv *adv7280,
+		struct reg_value *config, int size) {
+	int i, ret;
+
+	for (i = 0; i < size; i++) {
+		pr_debug("%s[%d]: reg = 0x%02x, value = 0x%02x\n", __func__,
+				i, config[i].reg,  config[i].value);
+		ret = adv7280_write_reg(adv7280, config[i].reg,
+				config[i].value | config[i].mask);
+		if (ret < 0) {
+			pr_err("%s: write error %x\n", __func__, ret);
+			return ret;
+		}
+
+		if (config[i].delay_ms)
+			msleep(config[i].delay_ms);
+	}
+
+	return 0;
+}
+
+/*!
+ * ADV7280 I2C probe function.
+ * Function set in i2c_driver struct.
+ * Called by insmod.
+ *
+ *  @param *adapter	I2C adapter descriptor.
+ *  @return		Error code indicating success or failure.
+ */
+static int adv7280_i2c_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	int ret;
+	struct adv7280_priv *adv7280;
+
+	adv7280 = kzalloc(sizeof(struct adv7280_priv), GFP_KERNEL);
+	if (!adv7280)
+		return -ENOMEM;
+
+	adv7280->client = client;
+	adv7280->pdata = client->dev.platform_data;
+	i2c_set_clientdata(client, adv7280);
+
+	ret = adv7280_read_reg(adv7280, ADV7280_IDENT);
+	if (ret < 0) {
+		pr_err("%s: read id error %x\n", __func__, ret);
+		goto err;
+	}
+
+	if ((ret & ADV7280_SAMPLE_SILICON) != 0x40) {
+		pr_err("%s: device ADV7280 not found, ret = 0x%02x\n",
+				__func__, ret);
+		goto err;
+	}
+
+	pr_info("%s: device found, rev_id 0x%02x\n", __func__, ret);
+	/* select main register map */
+	ret = adv7280_write_reg(adv7280, ADV7280_ADI_CONTROL1, 0x00);
+	if (ret < 0) {
+		pr_err("%s: write error, select memory map %x\n",
+				__func__, ret);
+		goto err;
+	}
+
+	/* perform a device reset */
+	ret = adv7280_write_reg(adv7280, ADV7280_PM_REG,
+			ADV7280_PM_RESET_BIT);
+	if (ret < 0) {
+		pr_err("%s: write error, reset %x\n", __func__, ret);
+		goto err;
+	}
+	/* Wait 5ms reset time spec */
+	msleep(5);
+
+	/* Initial device configuration */
+	ret = adv7280_config(adv7280, adv7280_init_params,
+			ARRAY_SIZE(adv7280_init_params));
+	if (ret < 0) {
+		pr_err("%s: config device error %x\n", __func__, ret);
+		goto err;
+	}
+
+	return 0;
+err:
+	kfree(adv7280);
+	return ret;
+}
+
+static int adv7280_i2c_remove(struct i2c_client *i2c_client)
+{
+	struct adv7280_priv *adv7280 = i2c_get_clientdata(i2c_client);
+
+	kfree(adv7280);
+	return 0;
+}
+
+/*!
+ * adv7280_i2c_driver - i2c device identification
+ * This structure tells the I2C subsystem how to identify and support a
+ * given I2C device type.
+ */
+
+static const struct i2c_device_id adv7280_i2c_id[] = {
+	{"adv7280", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, adv7280_i2c_id);
+
+static struct i2c_driver adv7280_i2c_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "adv7280",
+		   },
+	.probe = adv7280_i2c_probe,
+	.remove = adv7280_i2c_remove,
+	.id_table = adv7280_i2c_id,
+};
+
+/*!
+ * ADV7280 init function.:
+ * Called on insmod.
+ * @return    Error code indicating success or failure.
+ */
+static __init int adv7280_init(void)
+{
+	int err;
+
+	err = i2c_add_driver(&adv7280_i2c_driver);
+
+	if (err < 0)
+		pr_err("%s: driver registration failed, error=%d\n",
+			__func__, err);
+
+	return err;
+}
+
+/*!
+ * ADV7280 cleanup function.
+ * Called on rmmod.
+ * @return   Error code indicating success or failure.
+ */
+static void __exit adv7280_clean(void)
+{
+	i2c_del_driver(&adv7280_i2c_driver);
+}
+
+module_init(adv7280_init);
+module_exit(adv7280_clean);
+
+MODULE_AUTHOR("Freescale Semiconductor");
+MODULE_DESCRIPTION("adv7280 video decoder driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/csi_v4l2_capture.c b/drivers/media/video/mxc/capture/csi_v4l2_capture.c
index eb824ddf5eb3..a0887b930ea4 100644
--- a/drivers/media/video/mxc/capture/csi_v4l2_capture.c
+++ b/drivers/media/video/mxc/capture/csi_v4l2_capture.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2009-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2009-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -15,7 +15,7 @@
  * @file drivers/media/video/mxc/capture/csi_v4l2_capture.c
  * This file is derived from mxc_v4l2_capture.c
  *
- * @brief MX25 Video For Linux 2 driver
+ * @brief Video For Linux 2 capture driver
  *
  * @ingroup MXC_V4L2_CAPTURE
  */
@@ -61,6 +61,198 @@ static struct v4l2_int_device csi_v4l2_int_device = {
 	      },
 };
 
+/* Callback function triggered after PxP receives an EOF interrupt */
+static void pxp_dma_done(void *arg)
+{
+	struct pxp_tx_desc *tx_desc = to_tx_desc(arg);
+	struct dma_chan *chan = tx_desc->txd.chan;
+	struct pxp_channel *pxp_chan = to_pxp_channel(chan);
+	cam_data *cam = pxp_chan->client;
+
+	/* This call will signal wait_for_completion_timeout() */
+	complete(&cam->pxp_tx_cmpl);
+}
+
+static bool chan_filter(struct dma_chan *chan, void *arg)
+{
+	if (imx_dma_is_pxp(chan))
+		return true;
+	else
+		return false;
+}
+
+/* Function to request PXP DMA channel */
+static int pxp_chan_init(cam_data *cam)
+{
+	dma_cap_mask_t mask;
+	struct dma_chan *chan;
+
+	/* Request a free channel */
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+	dma_cap_set(DMA_PRIVATE, mask);
+	chan = dma_request_channel(mask, chan_filter, NULL);
+	if (!chan) {
+		pr_err("Unsuccessfully request channel!\n");
+		return -EBUSY;
+	}
+
+	cam->pxp_chan = to_pxp_channel(chan);
+	cam->pxp_chan->client = cam;
+
+	init_completion(&cam->pxp_tx_cmpl);
+
+	return 0;
+}
+
+/*
+ * Function to call PxP DMA driver and send our new V4L2 buffer
+ * through the PxP and PxP will process this buffer in place.
+ * Note: This is a blocking call, so upon return the PxP tx should be complete.
+ */
+static int pxp_process_update(cam_data *cam)
+{
+	dma_cookie_t cookie;
+	struct scatterlist *sg = cam->sg;
+	struct dma_chan *dma_chan;
+	struct pxp_tx_desc *desc;
+	struct dma_async_tx_descriptor *txd;
+	struct pxp_config_data *pxp_conf = &cam->pxp_conf;
+	struct pxp_proc_data *proc_data = &cam->pxp_conf.proc_data;
+	int i, ret;
+	int length;
+
+	pr_debug("Starting PxP Send Buffer\n");
+
+	/* First, check to see that we have acquired a PxP Channel object */
+	if (cam->pxp_chan == NULL) {
+		/*
+		 * PxP Channel has not yet been created and initialized,
+		 * so let's go ahead and try
+		 */
+		ret = pxp_chan_init(cam);
+		if (ret) {
+			/*
+			 * PxP channel init failed, and we can't use the
+			 * PxP until the PxP DMA driver has loaded, so we abort
+			 */
+			pr_err("PxP chan init failed\n");
+			return -ENODEV;
+		}
+	}
+
+	/*
+	 * Init completion, so that we can be properly informed of
+	 * the completion of the PxP task when it is done.
+	 */
+	init_completion(&cam->pxp_tx_cmpl);
+
+	dma_chan = &cam->pxp_chan->dma_chan;
+
+	txd = dma_chan->device->device_prep_slave_sg(dma_chan, sg, 2,
+						     DMA_TO_DEVICE,
+						     DMA_PREP_INTERRUPT);
+	if (!txd) {
+		pr_err("Error preparing a DMA transaction descriptor.\n");
+		return -EIO;
+	}
+
+	txd->callback_param = txd;
+	txd->callback = pxp_dma_done;
+
+	/*
+	 * Configure PxP for processing of new v4l2 buf
+	 */
+	pxp_conf->s0_param.pixel_fmt = PXP_PIX_FMT_UYVY;
+	pxp_conf->s0_param.color_key = -1;
+	pxp_conf->s0_param.color_key_enable = false;
+	pxp_conf->s0_param.width = cam->v2f.fmt.pix.width;
+	pxp_conf->s0_param.height = cam->v2f.fmt.pix.height;
+
+	pxp_conf->ol_param[0].combine_enable = false;
+
+	proc_data->srect.top = 0;
+	proc_data->srect.left = 0;
+	proc_data->srect.width = pxp_conf->s0_param.width;
+	proc_data->srect.height = pxp_conf->s0_param.height;
+
+	proc_data->drect.top = 0;
+	proc_data->drect.left = 0;
+	proc_data->drect.width = proc_data->srect.width;
+	proc_data->drect.height = proc_data->srect.height;
+	proc_data->scaling = 0;
+	proc_data->hflip = 0;
+	proc_data->vflip = 0;
+	proc_data->rotate = 0;
+	proc_data->bgcolor = 0;
+
+	pxp_conf->out_param.pixel_fmt = PXP_PIX_FMT_RGB565;
+	pxp_conf->out_param.width = proc_data->drect.width;
+	pxp_conf->out_param.height = proc_data->drect.height;
+
+	if (cam->rotation >= IPU_ROTATE_90_RIGHT)
+		pxp_conf->out_param.stride = pxp_conf->out_param.height;
+	else
+		pxp_conf->out_param.stride = pxp_conf->out_param.width;
+
+	desc = to_tx_desc(txd);
+	length = desc->len;
+	for (i = 0; i < length; i++) {
+		if (i == 0) {/* S0 */
+			memcpy(&desc->proc_data, proc_data,
+				sizeof(struct pxp_proc_data));
+			pxp_conf->s0_param.paddr = sg_dma_address(&sg[0]);
+			memcpy(&desc->layer_param.s0_param, &pxp_conf->s0_param,
+				sizeof(struct pxp_layer_param));
+		} else if (i == 1) {
+			pxp_conf->out_param.paddr = sg_dma_address(&sg[1]);
+			memcpy(&desc->layer_param.out_param,
+				&pxp_conf->out_param,
+				sizeof(struct pxp_layer_param));
+		}
+
+		desc = desc->next;
+	}
+
+	/* Submitting our TX starts the PxP processing task */
+	cookie = txd->tx_submit(txd);
+	if (cookie < 0) {
+		pr_err("Error sending FB through PxP\n");
+		return -EIO;
+	}
+
+	cam->txd = txd;
+
+	/* trigger PxP */
+	dma_async_issue_pending(dma_chan);
+
+	return 0;
+}
+
+static int pxp_complete_update(cam_data *cam)
+{
+	int ret;
+	/*
+	 * Wait for completion event, which will be set
+	 * through our TX callback function.
+	 */
+	ret = wait_for_completion_timeout(&cam->pxp_tx_cmpl, HZ / 10);
+	if (ret <= 0) {
+		pr_warning("PxP operation failed due to %s\n",
+			 ret < 0 ? "user interrupt" : "timeout");
+		dma_release_channel(&cam->pxp_chan->dma_chan);
+		cam->pxp_chan = NULL;
+		return ret ? : -ETIMEDOUT;
+	}
+
+	dma_release_channel(&cam->pxp_chan->dma_chan);
+	cam->pxp_chan = NULL;
+
+	pr_debug("TX completed\n");
+
+	return 0;
+}
+
 /*!
  * Camera V4l2 callback function.
  *
@@ -666,6 +858,23 @@ static int csi_v4l_dqueue(cam_data *cam, struct v4l2_buffer *buf)
 	buf->flags = frame->buffer.flags;
 	buf->m = cam->frame[frame->index].buffer.m;
 
+	/*
+	 * Note:
+	 * If want to do preview on LCD, use PxP CSC to convert from UYVY
+	 * to RGB565; but for encoding, usually we don't use RGB format.
+	 */
+	if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB565) {
+		/* PxP processes it in place */
+		sg_dma_address(&cam->sg[0]) = buf->m.offset;
+		sg_dma_address(&cam->sg[1]) = buf->m.offset;
+		retval = pxp_process_update(cam);
+		if (retval) {
+			pr_err("Unable to submit PxP update task.\n");
+			return retval;
+		}
+		pxp_complete_update(cam);
+	}
+
 	return retval;
 }
 
@@ -1157,22 +1366,6 @@ static struct video_device csi_v4l_template = {
 };
 
 /*!
- * This function can be used to release any platform data on closing.
- */
-static void camera_platform_release(struct device *device)
-{
-}
-
-/*! Device Definition for csi v4l2 device */
-static struct platform_device csi_v4l2_devices = {
-	.name = "csi_v4l2",
-	.dev = {
-		.release = camera_platform_release,
-		},
-	.id = 0,
-};
-
-/*!
  * initialize cam_data structure
  *
  * @param cam      structure cam_data *
@@ -1186,8 +1379,8 @@ static void init_camera_struct(cam_data *cam)
 	/* Default everything to 0 */
 	memset(cam, 0, sizeof(cam_data));
 
-	init_MUTEX(&cam->param_lock);
-	init_MUTEX(&cam->busy_lock);
+	sema_init(&cam->param_lock, 1);
+	sema_init(&cam->busy_lock, 1);
 
 	cam->video_dev = video_device_alloc();
 	if (cam->video_dev == NULL)
@@ -1196,7 +1389,6 @@ static void init_camera_struct(cam_data *cam)
 	*(cam->video_dev) = csi_v4l_template;
 
 	video_set_drvdata(cam->video_dev, cam);
-	dev_set_drvdata(&csi_v4l2_devices.dev, (void *)cam);
 	cam->video_dev->minor = -1;
 
 	init_waitqueue_head(&cam->enc_queue);
@@ -1259,6 +1451,66 @@ static u8 camera_power(cam_data *cam, bool cameraOn)
 	return 0;
 }
 
+static int __devinit csi_v4l2_probe(struct platform_device *pdev)
+{
+	struct scatterlist *sg;
+	u8 err = 0;
+
+	/* Create g_cam and initialize it. */
+	g_cam = kmalloc(sizeof(cam_data), GFP_KERNEL);
+	if (g_cam == NULL) {
+		pr_err("ERROR: v4l2 capture: failed to register camera\n");
+		err = -ENOMEM;
+		goto out;
+	}
+	init_camera_struct(g_cam);
+	platform_set_drvdata(pdev, (void *)g_cam);
+
+	/* Set up the v4l2 device and register it */
+	csi_v4l2_int_device.priv = g_cam;
+	/* This function contains a bug that won't let this be rmmod'd. */
+	v4l2_int_device_register(&csi_v4l2_int_device);
+
+	/* register v4l video device */
+	if (video_register_device(g_cam->video_dev, VFL_TYPE_GRABBER, video_nr)
+	    == -1) {
+		kfree(g_cam);
+		g_cam = NULL;
+		pr_err("ERROR: v4l2 capture: video_register_device failed\n");
+		err = -ENODEV;
+		goto out;
+	}
+	pr_debug("   Video device registered: %s #%d\n",
+		 g_cam->video_dev->name, g_cam->video_dev->minor);
+
+	g_cam->pxp_chan = NULL;
+	/* Initialize Scatter-gather list containing 2 buffer addresses. */
+	sg = g_cam->sg;
+	sg_init_table(sg, 2);
+
+out:
+	return err;
+}
+
+static int __devexit csi_v4l2_remove(struct platform_device *pdev)
+{
+	if (g_cam->open_count) {
+		pr_err("ERROR: v4l2 capture:camera open "
+		       "-- setting ops to NULL\n");
+	} else {
+		pr_info("V4L2 freeing image input device\n");
+		v4l2_int_device_unregister(&csi_v4l2_int_device);
+		csi_stop_callback(g_cam);
+		video_unregister_device(g_cam->video_dev);
+		platform_set_drvdata(pdev, NULL);
+
+		kfree(g_cam);
+		g_cam = NULL;
+	}
+
+	return 0;
+}
+
 /*!
  * This function is called to put the sensor in a low power state.
  * Refer to the document driver-model/driver.txt in the kernel source tree
@@ -1324,8 +1576,8 @@ static struct platform_driver csi_v4l2_driver = {
 	.driver = {
 		   .name = "csi_v4l2",
 		   },
-	.probe = NULL,
-	.remove = NULL,
+	.probe = csi_v4l2_probe,
+	.remove = __devexit_p(csi_v4l2_remove),
 #ifdef CONFIG_PM
 	.suspend = csi_v4l2_suspend,
 	.resume = csi_v4l2_resume,
@@ -1382,55 +1634,8 @@ static void csi_v4l2_master_detach(struct v4l2_int_device *slave)
  */
 static __init int camera_init(void)
 {
-	u8 err = 0;
-
 	/* Register the device driver structure. */
-	err = platform_driver_register(&csi_v4l2_driver);
-	if (err != 0) {
-		pr_err("ERROR: v4l2 capture:camera_init: "
-		       "platform_driver_register failed.\n");
-		return err;
-	}
-
-	/* Create g_cam and initialize it. */
-	g_cam = kmalloc(sizeof(cam_data), GFP_KERNEL);
-	if (g_cam == NULL) {
-		pr_err("ERROR: v4l2 capture: failed to register camera\n");
-		platform_driver_unregister(&csi_v4l2_driver);
-		return -1;
-	}
-	init_camera_struct(g_cam);
-
-	/* Set up the v4l2 device and register it */
-	csi_v4l2_int_device.priv = g_cam;
-	/* This function contains a bug that won't let this be rmmod'd. */
-	v4l2_int_device_register(&csi_v4l2_int_device);
-
-	/* Register the platform device */
-	err = platform_device_register(&csi_v4l2_devices);
-	if (err != 0) {
-		pr_err("ERROR: v4l2 capture: camera_init: "
-		       "platform_device_register failed.\n");
-		platform_driver_unregister(&csi_v4l2_driver);
-		kfree(g_cam);
-		g_cam = NULL;
-		return err;
-	}
-
-	/* register v4l video device */
-	if (video_register_device(g_cam->video_dev, VFL_TYPE_GRABBER, video_nr)
-	    == -1) {
-		platform_device_unregister(&csi_v4l2_devices);
-		platform_driver_unregister(&csi_v4l2_driver);
-		kfree(g_cam);
-		g_cam = NULL;
-		pr_err("ERROR: v4l2 capture: video_register_device failed\n");
-		return -1;
-	}
-	pr_debug("   Video device registered: %s #%d\n",
-		 g_cam->video_dev->name, g_cam->video_dev->minor);
-
-	return err;
+	return platform_driver_register(&csi_v4l2_driver);
 }
 
 /*!
@@ -1438,22 +1643,7 @@ static __init int camera_init(void)
  */
 static void __exit camera_exit(void)
 {
-	pr_debug("In MVC: %s\n", __func__);
-
-	if (g_cam->open_count) {
-		pr_err("ERROR: v4l2 capture:camera open "
-		       "-- setting ops to NULL\n");
-	} else {
-		pr_info("V4L2 freeing image input device\n");
-		v4l2_int_device_unregister(&csi_v4l2_int_device);
-		csi_stop_callback(g_cam);
-		video_unregister_device(g_cam->video_dev);
-		platform_driver_unregister(&csi_v4l2_driver);
-		platform_device_unregister(&csi_v4l2_devices);
-
-		kfree(g_cam);
-		g_cam = NULL;
-	}
+	platform_driver_unregister(&csi_v4l2_driver);
 }
 
 module_init(camera_init);
diff --git a/drivers/media/video/mxc/capture/fsl_csi.c b/drivers/media/video/mxc/capture/fsl_csi.c
index dba35c4499e2..33a82242e95e 100644
--- a/drivers/media/video/mxc/capture/fsl_csi.c
+++ b/drivers/media/video/mxc/capture/fsl_csi.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2009-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2009-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -20,6 +20,7 @@
  */
 #include <linux/types.h>
 #include <linux/init.h>
+#include <linux/platform_device.h>
 #include <linux/device.h>
 #include <linux/err.h>
 #include <linux/interrupt.h>
@@ -27,11 +28,14 @@
 #include <linux/delay.h>
 #include <linux/module.h>
 #include <linux/clk.h>
+#include <linux/sched.h>
 #include <mach/clock.h>
 
 #include "mxc_v4l2_capture.h"
 #include "fsl_csi.h"
 
+void __iomem *csi_regbase;
+static int irq_nr;
 static bool g_csi_mclk_on;
 static csi_irq_callback_t g_callback;
 static void *g_callback_data;
@@ -164,7 +168,7 @@ void csi_start_callback(void *data)
 {
 	cam_data *cam = (cam_data *) data;
 
-	if (request_irq(MXC_INT_CSI, csi_irq_handler, 0, "csi", cam) < 0)
+	if (request_irq(irq_nr, csi_irq_handler, 0, "csi", cam) < 0)
 		pr_debug("CSI error: irq request fail\n");
 
 }
@@ -174,7 +178,7 @@ void csi_stop_callback(void *data)
 {
 	cam_data *cam = (cam_data *) data;
 
-	free_irq(MXC_INT_CSI, cam);
+	free_irq(irq_nr, cam);
 }
 EXPORT_SYMBOL(csi_stop_callback);
 
@@ -254,10 +258,32 @@ void csi_mclk_disable(void)
 	__raw_writel(__raw_readl(CSI_CSICR1) & ~BIT_MCLKEN, CSI_CSICR1);
 }
 
-int32_t __init csi_init_module(void)
+static int __devinit csi_probe(struct platform_device *pdev)
 {
 	int ret = 0;
 	struct clk *per_clk;
+	struct resource *res;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "No csi irq found.\n");
+		ret = -ENODEV;
+		goto err;
+	}
+	irq_nr = res->start;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "No csi base address found.\n");
+		ret = -ENODEV;
+		goto err;
+	}
+	csi_regbase = ioremap(res->start, resource_size(res));
+	if (!csi_regbase) {
+		dev_err(&pdev->dev, "ioremap failed with csi base\n");
+		ret = -ENOMEM;
+		goto err;
+	}
 
 	csihw_reset();
 	csi_init_interface();
@@ -271,12 +297,34 @@ int32_t __init csi_init_module(void)
 	clk_enable(per_clk);
 	csi_mclk_recalc(&csi_mclk);
 
+err:
 	return ret;
 }
 
-void __exit csi_cleanup_module(void)
+static int __devexit csi_remove(struct platform_device *pdev)
 {
 	clk_disable(&csi_mclk);
+	iounmap(csi_regbase);
+
+	return 0;
+}
+
+static struct platform_driver csi_driver = {
+	.driver = {
+		   .name = "fsl_csi",
+		   },
+	.probe = csi_probe,
+	.remove = __devexit_p(csi_remove),
+};
+
+int32_t __init csi_init_module(void)
+{
+	return platform_driver_register(&csi_driver);
+}
+
+void __exit csi_cleanup_module(void)
+{
+	platform_driver_unregister(&csi_driver);
 }
 
 module_init(csi_init_module);
diff --git a/drivers/media/video/mxc/capture/fsl_csi.h b/drivers/media/video/mxc/capture/fsl_csi.h
index 00c389892224..8dfce286fe53 100644
--- a/drivers/media/video/mxc/capture/fsl_csi.h
+++ b/drivers/media/video/mxc/capture/fsl_csi.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2009-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2009-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -102,23 +102,22 @@
 #define CSI_MCLK_I2C		8
 #endif
 
-#define CSI_CSICR1		(IO_ADDRESS(CSI_BASE_ADDR))
-#define CSI_CSICR2		(IO_ADDRESS(CSI_BASE_ADDR + 0x4))
-#define CSI_CSICR3		(IO_ADDRESS(CSI_BASE_ADDR + 0x8))
-#define CSI_STATFIFO		(IO_ADDRESS(CSI_BASE_ADDR + 0xC))
-#define CSI_CSIRXFIFO		(IO_ADDRESS(CSI_BASE_ADDR + 0x10))
-#define CSI_CSIRXCNT		(IO_ADDRESS(CSI_BASE_ADDR + 0x14))
-#define CSI_CSISR		(IO_ADDRESS(CSI_BASE_ADDR + 0x18))
-
-#define CSI_CSIDBG		(IO_ADDRESS(CSI_BASE_ADDR + 0x1C))
-#define CSI_CSIDMASA_STATFIFO	(IO_ADDRESS(CSI_BASE_ADDR + 0x20))
-#define CSI_CSIDMATS_STATFIFO	(IO_ADDRESS(CSI_BASE_ADDR + 0x24))
-#define CSI_CSIDMASA_FB1	(IO_ADDRESS(CSI_BASE_ADDR + 0x28))
-#define CSI_CSIDMASA_FB2	(IO_ADDRESS(CSI_BASE_ADDR + 0x2C))
-#define CSI_CSIFBUF_PARA	(IO_ADDRESS(CSI_BASE_ADDR + 0x30))
-#define CSI_CSIIMAG_PARA	(IO_ADDRESS(CSI_BASE_ADDR + 0x34))
-
-#define CSI_CSIRXFIFO_PHYADDR	(CSI_BASE_ADDR + 0x10)
+extern void __iomem *csi_regbase;
+#define CSI_CSICR1		(csi_regbase)
+#define CSI_CSICR2		(csi_regbase + 0x4)
+#define CSI_CSICR3		(csi_regbase + 0x8)
+#define CSI_STATFIFO		(csi_regbase + 0xC)
+#define CSI_CSIRXFIFO		(csi_regbase + 0x10)
+#define CSI_CSIRXCNT		(csi_regbase + 0x14)
+#define CSI_CSISR		(csi_regbase + 0x18)
+
+#define CSI_CSIDBG		(csi_regbase + 0x1C)
+#define CSI_CSIDMASA_STATFIFO	(csi_regbase + 0x20)
+#define CSI_CSIDMATS_STATFIFO	(csi_regbase + 0x24)
+#define CSI_CSIDMASA_FB1	(csi_regbase + 0x28)
+#define CSI_CSIDMASA_FB2	(csi_regbase + 0x2C)
+#define CSI_CSIFBUF_PARA	(csi_regbase + 0x30)
+#define CSI_CSIIMAG_PARA	(csi_regbase + 0x34)
 
 static inline void csi_clear_status(unsigned long status)
 {
diff --git a/drivers/media/video/mxc/capture/ipu_prp_enc.c b/drivers/media/video/mxc/capture/ipu_prp_enc.c
index def416570de3..a3c90d53f2eb 100644
--- a/drivers/media/video/mxc/capture/ipu_prp_enc.c
+++ b/drivers/media/video/mxc/capture/ipu_prp_enc.c
@@ -70,7 +70,7 @@ static int prp_enc_setup(cam_data *cam)
 {
 	ipu_channel_params_t enc;
 	int err = 0;
-	dma_addr_t dummy = 0xdeadbeaf;
+	dma_addr_t dummy = cam->dummy_frame.buffer.m.offset;
 #ifdef CONFIG_MXC_MIPI_CSI2
 	void *mipi_csi2_info;
 	int ipu_id;
diff --git a/drivers/media/video/mxc/capture/mt9v111.c b/drivers/media/video/mxc/capture/mt9v111.c
index 4305c56c82d9..3ae65db04486 100644
--- a/drivers/media/video/mxc/capture/mt9v111.c
+++ b/drivers/media/video/mxc/capture/mt9v111.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2004-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -879,7 +879,7 @@ static int ioctl_dev_init(struct v4l2_int_device *s)
 
 	gpio_sensor_active();
 
-	set_mclk_rate(&clock_rate);
+	set_mclk_rate(&clock_rate, 0);
 	mt9v111_rate_cal(&reset_frame_rate, clock_rate);
 	mt9v111_sensor_lib(mt9v111_device.coreReg, mt9v111_device.ifpReg);
 
diff --git a/drivers/media/video/mxc/capture/mxc_v4l2_capture.h b/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
index bef4214c8149..d2efbe8453b2 100644
--- a/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
+++ b/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
@@ -31,6 +31,10 @@
 #include <linux/list.h>
 #include <linux/ipu.h>
 #include <linux/mxc_v4l2.h>
+#include <linux/completion.h>
+#include <linux/dmaengine.h>
+#include <linux/pxp_dma.h>
+#include <mach/dma.h>
 #include <mach/ipu-v3.h>
 
 #include <media/v4l2-dev.h>
@@ -206,6 +210,14 @@ typedef struct _cam_data {
 	struct v4l2_int_device *self;
 	int sensor_index;
 	void *ipu;
+
+	/* v4l2 buf elements related to PxP DMA */
+	struct completion pxp_tx_cmpl;
+	struct pxp_channel *pxp_chan;
+	struct pxp_config_data pxp_conf;
+	struct dma_async_tx_descriptor *txd;
+	dma_cookie_t cookie;
+	struct scatterlist sg[2];
 } cam_data;
 
 struct sensor_data {
@@ -233,11 +245,5 @@ struct sensor_data {
 	void (*io_init)(void);
 };
 
-#if defined(CONFIG_MXC_IPU_V1) || defined(CONFIG_VIDEO_MXC_EMMA_CAMERA) \
-			       || defined(CONFIG_VIDEO_MXC_CSI_CAMERA_MODULE) \
-			       || defined(CONFIG_VIDEO_MXC_CSI_CAMERA)
-void set_mclk_rate(uint32_t *p_mclk_freq);
-#else
 void set_mclk_rate(uint32_t *p_mclk_freq, uint32_t csi);
-#endif
 #endif				/* __MXC_V4L2_CAPTURE_H__ */
diff --git a/drivers/media/video/mxc/capture/ov2640.c b/drivers/media/video/mxc/capture/ov2640.c
index a0a050bca2e9..24ebd5027ff1 100644
--- a/drivers/media/video/mxc/capture/ov2640.c
+++ b/drivers/media/video/mxc/capture/ov2640.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2005-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2005-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -801,7 +801,7 @@ static int ioctl_dev_init(struct v4l2_int_device *s)
 
 	pr_debug("   Setting mclk to %d MHz\n",
 		tgt_xclk / 1000000);
-	set_mclk_rate(&ov2640_data.mclk);
+	set_mclk_rate(&ov2640_data.mclk, 0);
 
 	return ov2640_init_mode(sensor);
 }
diff --git a/drivers/media/video/mxc/capture/ov5640_mipi.c b/drivers/media/video/mxc/capture/ov5640_mipi.c
index 44cdd466337e..bde39a51d368 100644
--- a/drivers/media/video/mxc/capture/ov5640_mipi.c
+++ b/drivers/media/video/mxc/capture/ov5640_mipi.c
@@ -1494,12 +1494,15 @@ static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
 		}
 
 		orig_mode = sensor->streamcap.capturemode;
+		ret = ov5640_init_mode(frame_rate,
+				(u32)a->parm.capture.capturemode, orig_mode);
+		if (ret < 0)
+			return ret;
+
 		sensor->streamcap.timeperframe = *timeperframe;
 		sensor->streamcap.capturemode =
 				(u32)a->parm.capture.capturemode;
 
-		ret = ov5640_init_mode(frame_rate,
-				sensor->streamcap.capturemode, orig_mode);
 		break;
 
 	/* These are all the possible cases. */
diff --git a/drivers/media/video/mxc/capture/ov5642.c b/drivers/media/video/mxc/capture/ov5642.c
index 2321f68c6384..426438276908 100644
--- a/drivers/media/video/mxc/capture/ov5642.c
+++ b/drivers/media/video/mxc/capture/ov5642.c
@@ -861,7 +861,7 @@ static struct reg_value ov5642_setting_15fps_VGA_640_480[] = {
 	{0x3001, 0x00, 0, 0}, {0x3002, 0x5c, 0, 0}, {0x3003, 0x00, 0, 0},
 	{0x3004, 0xff, 0, 0}, {0x3005, 0xff, 0, 0}, {0x3006, 0x43, 0, 0},
 	{0x3007, 0x37, 0, 0}, {0x3011, 0x09, 0, 0}, {0x3012, 0x02, 0, 0},
-	{0x3010, 0x10, 0, 0}, {0x460c, 0x20, 0, 0}, {0x3815, 0x04, 0, 0},
+	{0x3010, 0x00, 0, 0}, {0x460c, 0x20, 0, 0}, {0x3815, 0x04, 0, 0},
 	{0x370c, 0xa0, 0, 0}, {0x3602, 0xfc, 0, 0}, {0x3612, 0xff, 0, 0},
 	{0x3634, 0xc0, 0, 0}, {0x3613, 0x00, 0, 0}, {0x3605, 0x7c, 0, 0},
 	{0x3621, 0x09, 0, 0}, {0x3622, 0x60, 0, 0}, {0x3604, 0x40, 0, 0},
@@ -879,8 +879,8 @@ static struct reg_value ov5642_setting_15fps_VGA_640_480[] = {
 	{0x3501, 0x73, 0, 0}, {0x3502, 0x80, 0, 0}, {0x350b, 0x00, 0, 0},
 	{0x3503, 0x07, 0, 0}, {0x3824, 0x11, 0, 0}, {0x3825, 0xb0, 0, 0},
 	{0x3501, 0x1e, 0, 0}, {0x3502, 0x80, 0, 0}, {0x350b, 0x7f, 0, 0},
-	{0x380c, 0x07, 0, 0}, {0x380d, 0x2a, 0, 0}, {0x380e, 0x03, 0, 0},
-	{0x380f, 0xe8, 0, 0}, {0x3a0d, 0x04, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x380c, 0x07, 0, 0}, {0x380d, 0x2a, 0, 0}, {0x380e, 0x07, 0, 0},
+	{0x380f, 0xd0, 0, 0}, {0x3a0d, 0x04, 0, 0}, {0x3a0e, 0x03, 0, 0},
 	{0x3818, 0xc1, 0, 0}, {0x3705, 0xdb, 0, 0}, {0x370a, 0x81, 0, 0},
 	{0x3801, 0x80, 0, 0}, {0x3621, 0xc7, 0, 0}, {0x3801, 0x50, 0, 0},
 	{0x3803, 0x08, 0, 0}, {0x3827, 0x08, 0, 0}, {0x3810, 0x80, 0, 0},
@@ -1202,7 +1202,7 @@ static struct reg_value ov5642_setting_15fps_XGA_1024_768[] = {
 	{0x3001, 0x00, 0, 0}, {0x3002, 0x5c, 0, 0}, {0x3003, 0x00, 0, 0},
 	{0x3004, 0xff, 0, 0}, {0x3005, 0xff, 0, 0}, {0x3006, 0x43, 0, 0},
 	{0x3007, 0x37, 0, 0}, {0x3011, 0x09, 0, 0}, {0x3012, 0x02, 0, 0},
-	{0x3010, 0x10, 0, 0}, {0x460c, 0x20, 0, 0}, {0x3815, 0x04, 0, 0},
+	{0x3010, 0x00, 0, 0}, {0x460c, 0x20, 0, 0}, {0x3815, 0x04, 0, 0},
 	{0x370c, 0xa0, 0, 0}, {0x3602, 0xfc, 0, 0}, {0x3612, 0xff, 0, 0},
 	{0x3634, 0xc0, 0, 0}, {0x3613, 0x00, 0, 0}, {0x3605, 0x7c, 0, 0},
 	{0x3621, 0x09, 0, 0}, {0x3622, 0x60, 0, 0}, {0x3604, 0x40, 0, 0},
@@ -1220,8 +1220,8 @@ static struct reg_value ov5642_setting_15fps_XGA_1024_768[] = {
 	{0x3501, 0x73, 0, 0}, {0x3502, 0x80, 0, 0}, {0x350b, 0x00, 0, 0},
 	{0x3503, 0x07, 0, 0}, {0x3824, 0x11, 0, 0}, {0x3825, 0xb0, 0, 0},
 	{0x3501, 0x1e, 0, 0}, {0x3502, 0x80, 0, 0}, {0x350b, 0x7f, 0, 0},
-	{0x380c, 0x07, 0, 0}, {0x380d, 0x2a, 0, 0}, {0x380e, 0x03, 0, 0},
-	{0x380f, 0xe8, 0, 0}, {0x3a0d, 0x04, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x380c, 0x07, 0, 0}, {0x380d, 0x2a, 0, 0}, {0x380e, 0x07, 0, 0},
+	{0x380f, 0xd0, 0, 0}, {0x3a0d, 0x04, 0, 0}, {0x3a0e, 0x03, 0, 0},
 	{0x3818, 0xc1, 0, 0}, {0x3705, 0xdb, 0, 0}, {0x370a, 0x81, 0, 0},
 	{0x3801, 0x80, 0, 0}, {0x3621, 0xc7, 0, 0}, {0x3801, 0x50, 0, 0},
 	{0x3803, 0x08, 0, 0}, {0x3827, 0x08, 0, 0}, {0x3810, 0x80, 0, 0},
@@ -1364,7 +1364,7 @@ static struct reg_value ov5642_setting_15fps_XGA_1024_768[] = {
 	{0x5087, 0x00, 0, 0}, {0x5088, 0x00, 0, 0}, {0x5089, 0x00, 0, 0},
 	{0x302b, 0x00, 0, 0}, {0x3621, 0x87, 0, 0}, {0x3a00, 0x78, 0, 0},
 	{0x3808, 0x04, 0, 0}, {0x3809, 0x00, 0, 0}, {0x380a, 0x03, 0, 0},
-	{0x380b, 0x00, 0, 0}, {0x3815, 0x02, 0, 0},
+	{0x380b, 0x00, 0, 0}, {0x3815, 0x02, 0, 0}, {0x302c, 0x60, 0x60, 0},
 };
 
 static struct reg_value ov5642_setting_30fps_QVGA_320_240[] = {
@@ -3526,6 +3526,9 @@ static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
 		ret = ov5642_change_mode(new_frame_rate, old_frame_rate,
 				a->parm.capture.capturemode,
 				sensor->streamcap.capturemode);
+		if (ret < 0)
+			return ret;
+
 		sensor->streamcap.timeperframe = *timeperframe;
 		sensor->streamcap.capturemode =
 				(u32)a->parm.capture.capturemode;
diff --git a/drivers/media/video/mxc/capture/sensor_clock.c b/drivers/media/video/mxc/capture/sensor_clock.c
index 8004aeee5dc2..150659fa5dc0 100644
--- a/drivers/media/video/mxc/capture/sensor_clock.c
+++ b/drivers/media/video/mxc/capture/sensor_clock.c
@@ -26,31 +26,6 @@
 #include <mach/hardware.h>
 #include <asm/mach-types.h>
 
-#if defined(CONFIG_MXC_IPU_V1) || defined(CONFIG_VIDEO_MXC_EMMA_CAMERA) \
-			       || defined(CONFIG_VIDEO_MXC_CSI_CAMERA_MODULE) \
-			       || defined(CONFIG_VIDEO_MXC_CSI_CAMERA)
-/*
- * set_mclk_rate
- *
- * @param       p_mclk_freq  mclk frequence
- *
- */
-void set_mclk_rate(uint32_t *p_mclk_freq)
-{
-	struct clk *clk;
-	uint32_t freq = 0;
-
-	clk = clk_get(NULL, "csi_clk");
-
-	freq = clk_round_rate(clk, *p_mclk_freq);
-	clk_set_rate(clk, freq);
-
-	*p_mclk_freq = freq;
-
-	clk_put(clk);
-	pr_debug("mclk frequency = %d\n", *p_mclk_freq);
-}
-#else
 /*
  * set_mclk_rate
  *
@@ -81,6 +56,8 @@ void set_mclk_rate(uint32_t *p_mclk_freq, uint32_t csi)
 			pr_err("invalid csi num %d\n", csi);
 			return;
 		};
+	} else if (cpu_is_mx25() || cpu_is_mx6sl()) {	/* only has CSI0 */
+		mclk = "csi_clk";
 	} else {
 		if (csi == 0) {
 			mclk = "csi_mclk1";
@@ -102,7 +79,6 @@ void set_mclk_rate(uint32_t *p_mclk_freq, uint32_t csi)
 	clk_put(clk);
 	pr_debug("%s frequency = %d\n", mclk, *p_mclk_freq);
 }
-#endif
 
 /* Exported symbols for modules. */
 EXPORT_SYMBOL(set_mclk_rate);
diff --git a/drivers/media/video/mxc/output/mxc_vout.c b/drivers/media/video/mxc/output/mxc_vout.c
index 366d27a92286..388c4b39570f 100644
--- a/drivers/media/video/mxc/output/mxc_vout.c
+++ b/drivers/media/video/mxc/output/mxc_vout.c
@@ -861,6 +861,7 @@ static int mxc_vout_open(struct file *file)
 
 		vout->win_pos.x = 0;
 		vout->win_pos.y = 0;
+		vout->release = true;
 	}
 
 	file->private_data = vout;
@@ -973,6 +974,7 @@ static inline int vdoaipu_try_task(struct mxc_vout_output *vout)
 	int is_1080p_stream;
 	size_t size;
 	struct ipu_task *ipu_task = &vout->task;
+	struct ipu_crop *icrop = &ipu_task->input.crop;
 	struct ipu_task *vdoa_task = &vout->vdoa_task;
 	u32 deinterlace = 0;
 	u32 in_fmt;
@@ -981,15 +983,23 @@ static inline int vdoaipu_try_task(struct mxc_vout_output *vout)
 		deinterlace = 1;
 
 	memset(vdoa_task, 0, sizeof(*vdoa_task));
-	memcpy(&vdoa_task->input, &ipu_task->input, sizeof(ipu_task->input));
 	vdoa_task->output.format = IPU_PIX_FMT_NV12;
-	vdoa_task->output.width = ipu_task->input.crop.w;
-	vdoa_task->output.height = ipu_task->input.crop.h;
-	vdoa_task->output.crop.w = ipu_task->input.crop.w;
-	vdoa_task->output.crop.h = ipu_task->input.crop.h;
-
-	size = PAGE_ALIGN(ipu_task->input.crop.w *
-					ipu_task->input.crop.h *
+	memcpy(&vdoa_task->input, &ipu_task->input,
+			sizeof(ipu_task->input));
+	if ((icrop->w % IPU_PIX_FMT_TILED_NV12_MBALIGN) ||
+		(icrop->h % IPU_PIX_FMT_TILED_NV12_MBALIGN)) {
+		vdoa_task->input.crop.w =
+			ALIGN(icrop->w, IPU_PIX_FMT_TILED_NV12_MBALIGN);
+		vdoa_task->input.crop.h =
+			ALIGN(icrop->h, IPU_PIX_FMT_TILED_NV12_MBALIGN);
+	}
+	vdoa_task->output.width = vdoa_task->input.crop.w;
+	vdoa_task->output.height = vdoa_task->input.crop.h;
+	vdoa_task->output.crop.w = vdoa_task->input.crop.w;
+	vdoa_task->output.crop.h = vdoa_task->input.crop.h;
+
+	size = PAGE_ALIGN(vdoa_task->input.crop.w *
+					vdoa_task->input.crop.h *
 					fmt_to_bpp(vdoa_task->output.format)/8);
 	if (size > vout->vdoa_work.size) {
 		if (vout->vdoa_work.vaddr)
@@ -1029,6 +1039,7 @@ static int mxc_vout_try_task(struct mxc_vout_output *vout)
 	u32 o_height = 0;
 	u32 ocrop_h = 0;
 	bool tiled_fmt = false;
+	bool tiled_need_pp = false;
 
 	vout->vdoa_1080p = CHECK_TILED_1080P_DISPLAY(vout);
 	if (vout->vdoa_1080p) {
@@ -1041,10 +1052,17 @@ static int mxc_vout_try_task(struct mxc_vout_output *vout)
 
 	if ((IPU_PIX_FMT_TILED_NV12 == input->format) ||
 		(IPU_PIX_FMT_TILED_NV12F == input->format)) {
-		crop->w -= crop->w % IPU_PIX_FMT_TILED_NV12_MBALIGN;
-		crop->h -= crop->h % IPU_PIX_FMT_TILED_NV12_MBALIGN;
-		crop->pos.x -= crop->pos.x % IPU_PIX_FMT_TILED_NV12_MBALIGN;
-		crop->pos.y -= crop->pos.y % IPU_PIX_FMT_TILED_NV12_MBALIGN;
+		if ((input->width % IPU_PIX_FMT_TILED_NV12_MBALIGN) ||
+			(input->height % IPU_PIX_FMT_TILED_NV12_MBALIGN) ||
+			(crop->pos.x % IPU_PIX_FMT_TILED_NV12_MBALIGN) ||
+			(crop->pos.y % IPU_PIX_FMT_TILED_NV12_MBALIGN)) {
+			v4l2_err(vout->vfd->v4l2_dev,
+				"ERR: tiled fmt needs 16 pixel align.\n");
+			return -EINVAL;
+		}
+		if ((crop->w % IPU_PIX_FMT_TILED_NV12_MBALIGN) ||
+			(crop->h % IPU_PIX_FMT_TILED_NV12_MBALIGN))
+			tiled_need_pp = true;
 	} else {
 		crop->w -= crop->w % 8;
 		crop->h -= crop->h % 8;
@@ -1072,7 +1090,8 @@ static int mxc_vout_try_task(struct mxc_vout_output *vout)
 		if ((IPU_PIX_FMT_TILED_NV12 == input->format) ||
 			(IPU_PIX_FMT_TILED_NV12F == input->format)) {
 			/* check resize/rotate/flip, or csc task */
-			if (!((IPU_ROTATE_NONE != output->rotate) ||
+			if (!(tiled_need_pp ||
+				(IPU_ROTATE_NONE != output->rotate) ||
 				(input->crop.w != output->crop.w) ||
 				(input->crop.h != output->crop.h) ||
 				(!vout->disp_support_csc &&
diff --git a/drivers/mfd/mxc-hdmi-core.c b/drivers/mfd/mxc-hdmi-core.c
index e9322477ff6d..a07db1b0c070 100644
--- a/drivers/mfd/mxc-hdmi-core.c
+++ b/drivers/mfd/mxc-hdmi-core.c
@@ -41,6 +41,7 @@
 #include <linux/mfd/mxc-hdmi-core.h>
 #include <linux/fsl_devices.h>
 #include <mach/hardware.h>
+#include <linux/mfd/mxc-hdmi-core.h>
 
 struct mxc_hdmi_data {
 	struct platform_device *pdev;
@@ -72,22 +73,29 @@ static spinlock_t hdmi_audio_lock, hdmi_blank_state_lock, hdmi_cable_state_lock;
 unsigned int hdmi_set_cable_state(unsigned int state)
 {
 	unsigned long flags;
+	struct snd_pcm_substream *substream = hdmi_audio_stream_playback;
 
 	spin_lock_irqsave(&hdmi_cable_state_lock, flags);
 	hdmi_cable_state = state;
 	spin_unlock_irqrestore(&hdmi_cable_state_lock, flags);
 
+	if (check_hdmi_state() && substream)
+		substream->ops->trigger(substream, SNDRV_PCM_TRIGGER_START);
 	return 0;
 }
 
 unsigned int hdmi_set_blank_state(unsigned int state)
 {
 	unsigned long flags;
+	struct snd_pcm_substream *substream = hdmi_audio_stream_playback;
 
 	spin_lock_irqsave(&hdmi_blank_state_lock, flags);
 	hdmi_blank_state = state;
 	spin_unlock_irqrestore(&hdmi_blank_state_lock, flags);
 
+	if (check_hdmi_state() && substream)
+		substream->ops->trigger(substream, SNDRV_PCM_TRIGGER_START);
+
 	return 0;
 }
 
@@ -98,7 +106,7 @@ static void hdmi_audio_abort_stream(struct snd_pcm_substream *substream)
 	snd_pcm_stream_lock_irqsave(substream, flags);
 
 	if (snd_pcm_running(substream))
-		snd_pcm_stop(substream, SNDRV_PCM_STATE_DISCONNECTED);
+		substream->ops->trigger(substream, SNDRV_PCM_TRIGGER_STOP);
 
 	snd_pcm_stream_unlock_irqrestore(substream, flags);
 }
@@ -114,7 +122,7 @@ int mxc_hdmi_abort_stream(void)
 	return 0;
 }
 
-static int check_hdmi_state(void)
+int check_hdmi_state(void)
 {
 	unsigned long flags1, flags2;
 	unsigned int ret;
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index 2c32faad3b4b..ba625b3ca376 100755
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -30,7 +30,7 @@
 #include "sdhci.h"
 
 #define DRIVER_NAME "sdhci"
-#define CLK_TIMEOUT    (10 * HZ)
+#define CLK_TIMEOUT    (1 * HZ)
 
 #define DBG(f, x...) \
 	pr_debug(DRIVER_NAME " [%s()]: " f, __func__,## x)
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-lib.c b/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
index 01c81faffa4a..2138dcb4f8a4 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
@@ -44,7 +44,7 @@ struct timing_threshod timing_default_threshold = {
 	.max_dll_delay_in_ns         = 16,
 };
 
-static struct clk *mxs_dma_clk;
+struct clk *mxs_dma_clk;
 
 static void setup_ddr_timing_onfi(struct gpmi_nand_data *this)
 {
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-nand.c b/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
index f25b7c27b101..9bd09fc490e5 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
@@ -439,6 +439,12 @@ static bool gpmi_dma_filter(struct dma_chan *chan, void *param)
 static void release_dma_channels(struct gpmi_nand_data *this)
 {
 	unsigned int i;
+
+	if (mxs_dma_clk) {
+		clk_put(mxs_dma_clk);
+		mxs_dma_clk = NULL;
+	}
+
 	for (i = 0; i < DMA_CHANS; i++)
 		if (this->dma_chans[i]) {
 			dma_release_channel(this->dma_chans[i]);
@@ -873,7 +879,7 @@ static void block_mark_swapping(struct gpmi_nand_data *this,
 }
 
 static int gpmi_ecc_read_page(struct mtd_info *mtd, struct nand_chip *chip,
-				uint8_t *buf, int page)
+				uint8_t *buf, int oob_required, int page)
 {
 	struct gpmi_nand_data *this = chip->priv;
 	struct bch_geometry *nfc_geo = &this->bch_geometry;
@@ -939,17 +945,20 @@ static int gpmi_ecc_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 		mtd->ecc_stats.corrected += corrected;
 	}
 
-	/*
-	 * It's time to deliver the OOB bytes. See gpmi_ecc_read_oob() for
-	 * details about our policy for delivering the OOB.
-	 *
-	 * We fill the caller's buffer with set bits, and then copy the block
-	 * mark to th caller's buffer. Note that, if block mark swapping was
-	 * necessary, it has already been done, so we can rely on the first
-	 * byte of the auxiliary buffer to contain the block mark.
-	 */
-	memset(chip->oob_poi, ~0, mtd->oobsize);
-	chip->oob_poi[0] = ((uint8_t *) auxiliary_virt)[0];
+	if (oob_required) {
+		/*
+		 * It's time to deliver the OOB bytes. See gpmi_ecc_read_oob()
+		 * for details about our policy for delivering the OOB.
+		 *
+		 * We fill the caller's buffer with set bits, and then copy the
+		 * block mark to th caller's buffer. Note that, if block mark
+		 * swapping was necessary, it has already been done, so we can
+		 * rely on the first byte of the auxiliary buffer to contain
+		 * the block mark.
+		 */
+		memset(chip->oob_poi, ~0, mtd->oobsize);
+		chip->oob_poi[0] = ((uint8_t *) auxiliary_virt)[0];
+	}
 
 	read_page_swap_end(this, buf, mtd->writesize,
 			this->payload_virt, this->payload_phys,
@@ -959,8 +968,8 @@ exit_nfc:
 	return ret;
 }
 
-static void gpmi_ecc_write_page(struct mtd_info *mtd,
-				struct nand_chip *chip, const uint8_t *buf)
+static void gpmi_ecc_write_page(struct mtd_info *mtd, struct nand_chip *chip,
+				const uint8_t *buf, int oob_required)
 {
 	struct gpmi_nand_data *this = chip->priv;
 	struct bch_geometry *nfc_geo = &this->bch_geometry;
@@ -1040,7 +1049,7 @@ static int gpmi_verify_buf(struct mtd_info *mtd, const uint8_t * buf, int len)
 {
 	struct nand_chip *nand = mtd->priv;
 
-	gpmi_ecc_read_page(mtd, nand, verify_buf, len);
+	gpmi_ecc_read_page(mtd, nand, verify_buf, 0, len);
 	if (memcmp(buf, verify_buf, len))
 		return -EFAULT;
 	return 0;
@@ -1353,7 +1362,7 @@ static int __devinit mx23_write_transcription_stamp(struct gpmi_nand_data *this)
 		/* Write the first page of the current stride. */
 		dev_dbg(dev, "Writing an NCB fingerprint in page 0x%x\n", page);
 		chip->cmdfunc(mtd, NAND_CMD_SEQIN, 0x00, page);
-		chip->ecc.write_page_raw(mtd, chip, buffer);
+		chip->ecc.write_page_raw(mtd, chip, buffer, 0);
 		chip->cmdfunc(mtd, NAND_CMD_PAGEPROG, -1, -1);
 
 		/* Wait for the write to finish. */
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-nand.h b/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
index 21e1f6569211..77d0d273a297 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
@@ -290,4 +290,6 @@ extern int gpmi_read_page(struct gpmi_nand_data *,
 #define GPMI_IS_MX23(x)		((x)->pdev->id_entry->driver_data == IS_MX23)
 #define GPMI_IS_MX28(x)		((x)->pdev->id_entry->driver_data == IS_MX28)
 #define GPMI_IS_MX6Q(x)		((x)->pdev->id_entry->driver_data == IS_MX6Q)
+
+extern struct clk *mxs_dma_clk;
 #endif
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index e14f8284e79f..6367eac4461b 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -1051,12 +1051,13 @@ EXPORT_SYMBOL(nand_lock);
  * @mtd:	mtd info structure
  * @chip:	nand chip info structure
  * @buf:	buffer to store read data
+ * @oob_required: caller requires OOB data read to chip->oob_poi
  * @page:	page number to read
  *
  * Not for syndrome calculating ecc controllers, which use a special oob layout
  */
 static int nand_read_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
-			      uint8_t *buf, int page)
+			      uint8_t *buf, int oob_required, int page)
 {
 	chip->read_buf(mtd, buf, mtd->writesize);
 	chip->read_buf(mtd, chip->oob_poi, mtd->oobsize);
@@ -1068,13 +1069,14 @@ static int nand_read_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
  * @mtd:	mtd info structure
  * @chip:	nand chip info structure
  * @buf:	buffer to store read data
+ * @oob_required: caller requires OOB data read to chip->oob_poi
  * @page:	page number to read
  *
  * We need a special oob layout and handling even when OOB isn't used.
  */
 static int nand_read_page_raw_syndrome(struct mtd_info *mtd,
-					struct nand_chip *chip,
-					uint8_t *buf, int page)
+					struct nand_chip *chip, uint8_t *buf,
+					int oob_required, int page)
 {
 	int eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
@@ -1111,10 +1113,11 @@ static int nand_read_page_raw_syndrome(struct mtd_info *mtd,
  * @mtd:	mtd info structure
  * @chip:	nand chip info structure
  * @buf:	buffer to store read data
+ * @oob_required: caller requires OOB data read to chip->oob_poi
  * @page:	page number to read
  */
 static int nand_read_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
-				uint8_t *buf, int page)
+				uint8_t *buf, int oob_required, int page)
 {
 	int i, eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
@@ -1124,7 +1127,7 @@ static int nand_read_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
 	uint8_t *ecc_code = chip->buffers->ecccode;
 	uint32_t *eccpos = chip->ecc.layout->eccpos;
 
-	chip->ecc.read_page_raw(mtd, chip, buf, page);
+	chip->ecc.read_page_raw(mtd, chip, buf, 1, page);
 
 	for (i = 0; eccsteps; eccsteps--, i += eccbytes, p += eccsize)
 		chip->ecc.calculate(mtd, p, &ecc_calc[i]);
@@ -1239,12 +1242,13 @@ static int nand_read_subpage(struct mtd_info *mtd, struct nand_chip *chip,
  * @mtd:	mtd info structure
  * @chip:	nand chip info structure
  * @buf:	buffer to store read data
+ * @oob_required: caller requires OOB data read to chip->oob_poi
  * @page:	page number to read
  *
  * Not for syndrome calculating ecc controllers which need a special oob layout
  */
 static int nand_read_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
-				uint8_t *buf, int page)
+				uint8_t *buf, int oob_required, int page)
 {
 	int i, eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
@@ -1284,6 +1288,7 @@ static int nand_read_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
  * @mtd:	mtd info structure
  * @chip:	nand chip info structure
  * @buf:	buffer to store read data
+ * @oob_required: caller requires OOB data read to chip->oob_poi
  * @page:	page number to read
  *
  * Hardware ECC for large page chips, require OOB to be read first.
@@ -1294,7 +1299,7 @@ static int nand_read_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
  * overwriting the NAND manufacturer bad block markings.
  */
 static int nand_read_page_hwecc_oob_first(struct mtd_info *mtd,
-	struct nand_chip *chip, uint8_t *buf, int page)
+	struct nand_chip *chip, uint8_t *buf, int oob_required, int page)
 {
 	int i, eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
@@ -1333,13 +1338,14 @@ static int nand_read_page_hwecc_oob_first(struct mtd_info *mtd,
  * @mtd:	mtd info structure
  * @chip:	nand chip info structure
  * @buf:	buffer to store read data
+ * @oob_required: caller requires OOB data read to chip->oob_poi
  * @page:	page number to read
  *
  * The hw generator calculates the error syndrome automatically. Therefor
  * we need a special oob layout and handling.
  */
 static int nand_read_page_syndrome(struct mtd_info *mtd, struct nand_chip *chip,
-				   uint8_t *buf, int page)
+				   uint8_t *buf, int oob_required, int page)
 {
 	int i, eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
@@ -1443,7 +1449,7 @@ static uint8_t *nand_transfer_oob(struct nand_chip *chip, uint8_t *oob,
 int nand_do_read_ops(struct mtd_info *mtd, loff_t from,
 			    struct mtd_oob_ops *ops)
 {
-	int chipnr, page, realpage, col, bytes, aligned;
+	int chipnr, page, realpage, col, bytes, aligned, oob_required;
 	struct nand_chip *chip = mtd->priv;
 	struct mtd_ecc_stats stats;
 	int blkcheck = (1 << (chip->phys_erase_shift - chip->page_shift)) - 1;
@@ -1468,6 +1474,7 @@ int nand_do_read_ops(struct mtd_info *mtd, loff_t from,
 
 	buf = ops->datbuf;
 	oob = ops->oobbuf;
+	oob_required = oob ? 1 : 0;
 
 	while (1) {
 		bytes = min(mtd->writesize - col, readlen);
@@ -1485,13 +1492,15 @@ int nand_do_read_ops(struct mtd_info *mtd, loff_t from,
 			/* Now read the page into the buffer */
 			if (unlikely(ops->mode == MTD_OOB_RAW))
 				ret = chip->ecc.read_page_raw(mtd, chip,
-							      bufpoi, page);
+							      bufpoi,
+							      oob_required,
+							      page);
 			else if (!aligned && NAND_SUBPAGE_READ(chip) && !oob)
 				ret = chip->ecc.read_subpage(mtd, chip,
 							col, bytes, bufpoi);
 			else
 				ret = chip->ecc.read_page(mtd, chip, bufpoi,
-							  page);
+							  oob_required, page);
 			if (ret < 0)
 				break;
 
@@ -1893,11 +1902,12 @@ out:
  * @mtd:	mtd info structure
  * @chip:	nand chip info structure
  * @buf:	data buffer
+ * @oob_required: must write chip->oob_poi to OOB
  *
  * Not for syndrome calculating ecc controllers, which use a special oob layout
  */
 static void nand_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
-				const uint8_t *buf)
+				const uint8_t *buf, int oob_required)
 {
 	chip->write_buf(mtd, buf, mtd->writesize);
 	chip->write_buf(mtd, chip->oob_poi, mtd->oobsize);
@@ -1908,12 +1918,13 @@ static void nand_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
  * @mtd:	mtd info structure
  * @chip:	nand chip info structure
  * @buf:	data buffer
+ * @oob_required: must write chip->oob_poi to OOB
  *
  * We need a special oob layout and handling even when ECC isn't checked.
  */
 static void nand_write_page_raw_syndrome(struct mtd_info *mtd,
 					struct nand_chip *chip,
-					const uint8_t *buf)
+					const uint8_t *buf, int oob_required)
 {
 	int eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
@@ -1947,9 +1958,10 @@ static void nand_write_page_raw_syndrome(struct mtd_info *mtd,
  * @mtd:	mtd info structure
  * @chip:	nand chip info structure
  * @buf:	data buffer
+ * @oob_required: must write chip->oob_poi to OOB
  */
 static void nand_write_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
-				  const uint8_t *buf)
+				  const uint8_t *buf, int oob_required)
 {
 	int i, eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
@@ -1965,7 +1977,7 @@ static void nand_write_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
 	for (i = 0; i < chip->ecc.total; i++)
 		chip->oob_poi[eccpos[i]] = ecc_calc[i];
 
-	chip->ecc.write_page_raw(mtd, chip, buf);
+	chip->ecc.write_page_raw(mtd, chip, buf, 1);
 }
 
 /**
@@ -1973,9 +1985,10 @@ static void nand_write_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
  * @mtd:	mtd info structure
  * @chip:	nand chip info structure
  * @buf:	data buffer
+ * @oob_required: must write chip->oob_poi to OOB
  */
 static void nand_write_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
-				  const uint8_t *buf)
+				  const uint8_t *buf, int oob_required)
 {
 	int i, eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
@@ -2001,12 +2014,14 @@ static void nand_write_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
  * @mtd:	mtd info structure
  * @chip:	nand chip info structure
  * @buf:	data buffer
+ * @oob_required: must write chip->oob_poi to OOB
  *
  * The hw generator calculates the error syndrome automatically. Therefor
  * we need a special oob layout and handling.
  */
 static void nand_write_page_syndrome(struct mtd_info *mtd,
-				    struct nand_chip *chip, const uint8_t *buf)
+				    struct nand_chip *chip,
+				    const uint8_t *buf, int oob_required)
 {
 	int i, eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
@@ -2045,21 +2060,23 @@ static void nand_write_page_syndrome(struct mtd_info *mtd,
  * @mtd:	MTD device structure
  * @chip:	NAND chip descriptor
  * @buf:	the data to write
+ * @oob_required: must write chip->oob_poi to OOB
  * @page:	page number to write
  * @cached:	cached programming
  * @raw:	use _raw version of write_page
  */
 static int nand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
-			   const uint8_t *buf, int page, int cached, int raw)
+			   const uint8_t *buf, int oob_required, int page,
+			   int cached, int raw)
 {
 	int status;
 
 	chip->cmdfunc(mtd, NAND_CMD_SEQIN, 0x00, page);
 
 	if (unlikely(raw))
-		chip->ecc.write_page_raw(mtd, chip, buf);
+		chip->ecc.write_page_raw(mtd, chip, buf, oob_required);
 	else
-		chip->ecc.write_page(mtd, chip, buf);
+		chip->ecc.write_page(mtd, chip, buf, oob_required);
 
 	/*
 	 * Cached progamming disabled for now, Not sure if its worth the
@@ -2176,6 +2193,7 @@ int nand_do_write_ops(struct mtd_info *mtd, loff_t to,
 	uint8_t *oob = ops->oobbuf;
 	uint8_t *buf = ops->datbuf;
 	int ret, subpage;
+	int oob_required = oob ? 1 : 0;
 
 	ops->retlen = 0;
 	if (!writelen)
@@ -2238,7 +2256,7 @@ int nand_do_write_ops(struct mtd_info *mtd, loff_t to,
 			memset(chip->oob_poi, 0xff, mtd->oobsize);
 		}
 
-		ret = chip->write_page(mtd, chip, wbuf, page, cached,
+		ret = chip->write_page(mtd, chip, wbuf, oob_required, page, cached,
 				       (ops->mode == MTD_OOB_RAW));
 		if (ret)
 			break;
diff --git a/drivers/mxc/gpu-viv/arch/GC350/hal/kernel/gc_hal_kernel_hardware_vg.c b/drivers/mxc/gpu-viv/arch/GC350/hal/kernel/gc_hal_kernel_hardware_vg.c
index 4670a503ceb4..ec5d85887b70 100644
--- a/drivers/mxc/gpu-viv/arch/GC350/hal/kernel/gc_hal_kernel_hardware_vg.c
+++ b/drivers/mxc/gpu-viv/arch/GC350/hal/kernel/gc_hal_kernel_hardware_vg.c
@@ -362,6 +362,8 @@ gckVGHARDWARE_Construct(
         /* Set default event mask. */
         hardware->eventMask = 0xFFFFFFFF;
 
+        gcmkERR_BREAK(gckOS_AtomConstruct(Os, &hardware->pageTableDirty));
+
         /* Set fast clear to auto. */
         gcmkVERIFY_OK(gckVGHARDWARE_SetFastClear(hardware, -1));
 
@@ -384,6 +386,11 @@ gckVGHARDWARE_Construct(
     }
     while (gcvFALSE);
 
+    if (hardware->pageTableDirty != gcvNULL)
+    {
+        gcmkVERIFY_OK(gckOS_AtomDestroy(Os, hardware->pageTableDirty));
+    }
+
     if (hardware != gcvNULL)
     {
         gcmkVERIFY_OK(gckOS_Free(Os, hardware));
@@ -439,6 +446,11 @@ gckVGHARDWARE_Destroy(
             Hardware->os, Hardware->idleSignal));
     }
 
+    if (Hardware->pageTableDirty != gcvNULL)
+    {
+        gcmkVERIFY_OK(gckOS_AtomDestroy(Hardware->os, Hardware->pageTableDirty));
+    }
+
     /* Free the object. */
     status = gckOS_Free(Hardware->os, Hardware);
     gcmkFOOTER();
@@ -1277,26 +1289,6 @@ gceSTATUS gckVGHARDWARE_FlushMMU(
             | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 2:2) - (0 ? 2:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 2:2) - (0 ? 2:2) + 1))))))) << (0 ? 2:2))) | (((gctUINT32) (0x1 & ((gctUINT32) ((((1 ? 2:2) - (0 ? 2:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 2:2) - (0 ? 2:2) + 1))))))) << (0 ? 2:2)))
             | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 3:3) - (0 ? 3:3) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 3:3) - (0 ? 3:3) + 1))))))) << (0 ? 3:3))) | (((gctUINT32) (0x1 & ((gctUINT32) ((((1 ? 3:3) - (0 ? 3:3) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 3:3) - (0 ? 3:3) + 1))))))) << (0 ? 3:3)))
             | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 4:4) - (0 ? 4:4) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 4:4) - (0 ? 4:4) + 1))))))) << (0 ? 4:4))) | (((gctUINT32) (0x1 & ((gctUINT32) ((((1 ? 4:4) - (0 ? 4:4) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 4:4) - (0 ? 4:4) + 1))))))) << (0 ? 4:4)));
-#if gcdPOWER_MANAGEMENT
-        /* Acquire the power management semaphore. */
-        gcmkERR_BREAK(gckOS_AcquireSemaphore(Hardware->os,
-                               command->powerSemaphore));
-
-        status = gckVGCOMMAND_Execute(
-                            command,
-                            commandBuffer
-                            );
-        /* Acquire the power management semaphore. */
-        gcmkVERIFY_OK(gckOS_ReleaseSemaphore(Hardware->os,
-                               command->powerSemaphore));
-
-        gcmkERR_BREAK(status);
-#else
-        gcmkERR_BREAK(gckVGCOMMAND_Execute(
-            command,
-            commandBuffer
-            ));
-#endif
     }
     while(gcvFALSE);
 
diff --git a/drivers/mxc/gpu-viv/arch/GC350/hal/kernel/gc_hal_kernel_hardware_vg.h b/drivers/mxc/gpu-viv/arch/GC350/hal/kernel/gc_hal_kernel_hardware_vg.h
index bca6f57e0982..e3714b76db77 100644
--- a/drivers/mxc/gpu-viv/arch/GC350/hal/kernel/gc_hal_kernel_hardware_vg.h
+++ b/drivers/mxc/gpu-viv/arch/GC350/hal/kernel/gc_hal_kernel_hardware_vg.h
@@ -68,7 +68,7 @@ struct _gckVGHARDWARE
     gctUINT32                   powerOffTimeout;
     gctTHREAD                   timeIdleThread;
     gctBOOL                     killThread;
-
+    gctPOINTER                  pageTableDirty;
 };
 
 #endif /* __gc_hal_kernel_hardware_h_ */
diff --git a/drivers/mxc/gpu-viv/arch/XAQ2/hal/kernel/gc_hal_kernel_context.c b/drivers/mxc/gpu-viv/arch/XAQ2/hal/kernel/gc_hal_kernel_context.c
index 633081b09c07..d8898a376fe1 100644
--- a/drivers/mxc/gpu-viv/arch/XAQ2/hal/kernel/gc_hal_kernel_context.c
+++ b/drivers/mxc/gpu-viv/arch/XAQ2/hal/kernel/gc_hal_kernel_context.c
@@ -905,11 +905,12 @@ _DestroyContext(
             /* Free state delta map. */
             if (buffer->logical != gcvNULL)
             {
-                gcmkONERROR(gckOS_FreeContiguous(
-                    Context->os,
+                gcmkONERROR(gckEVENT_FreeContiguousMemory(
+                    Context->hardware->kernel->eventObj,
+                    Context->totalSize,
                     buffer->physical,
                     buffer->logical,
-                    Context->totalSize
+                    gcvKERNEL_PIXEL
                     ));
 
                 buffer->logical = gcvNULL;
diff --git a/drivers/mxc/gpu-viv/arch/XAQ2/hal/kernel/gc_hal_kernel_hardware.c b/drivers/mxc/gpu-viv/arch/XAQ2/hal/kernel/gc_hal_kernel_hardware.c
index 07c549e8f784..377ce32ed4c3 100644
--- a/drivers/mxc/gpu-viv/arch/XAQ2/hal/kernel/gc_hal_kernel_hardware.c
+++ b/drivers/mxc/gpu-viv/arch/XAQ2/hal/kernel/gc_hal_kernel_hardware.c
@@ -603,6 +603,8 @@ gckHARDWARE_Construct(
 
     gcmkONERROR(gckOS_CreateMutex(Os, &hardware->powerMutex));
     gcmkONERROR(gckOS_CreateSemaphore(Os, &hardware->globalSemaphore));
+    hardware->startIsr = gcvNULL;
+    hardware->stopIsr = gcvNULL;
 
 #if gcdPOWEROFF_TIMEOUT
     hardware->powerOffTimeout = gcdPOWEROFF_TIMEOUT;
@@ -1010,6 +1012,12 @@ gckHARDWARE_InitializeHardware(
         }
     }
 
+    /* Update GPU AXI cache atttribute. */
+    gcmkONERROR(gckOS_WriteRegisterEx(Hardware->os,
+                                      Hardware->core,
+                                      0x00008,
+                                      0x00002200));
+
     /* Success. */
     gcmkFOOTER_NO();
     return gcvSTATUS_OK;
@@ -2210,7 +2218,7 @@ gckHARDWARE_Link(
 
         /* Append LINK(bytes / 8), FetchAddress. */
         link = ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 31:27) - (0 ? 31:27) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 31:27) - (0 ? 31:27) + 1))))))) << (0 ? 31:27))) | (((gctUINT32) (0x08 & ((gctUINT32) ((((1 ? 31:27) - (0 ? 31:27) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 31:27) - (0 ? 31:27) + 1))))))) << (0 ? 31:27)))
-                   | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 15:0) - (0 ? 15:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 15:0) - (0 ? 15:0) + 1))))))) << (0 ? 15:0))) | (((gctUINT32) ((gctUINT32) (bytes >> 3) & ((gctUINT32) ((((1 ? 15:0) - (0 ? 15:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 15:0) - (0 ? 15:0) + 1))))))) << (0 ? 15:0)));
+             | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 15:0) - (0 ? 15:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 15:0) - (0 ? 15:0) + 1))))))) << (0 ? 15:0))) | (((gctUINT32) ((gctUINT32) (bytes >> 3) & ((gctUINT32) ((((1 ? 15:0) - (0 ? 15:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 15:0) - (0 ? 15:0) + 1))))))) << (0 ? 15:0)));
 
         gcmkONERROR(
             gckOS_WriteMemory(Hardware->os, logical, link));
@@ -2838,10 +2846,10 @@ gckHARDWARE_FlushMMU(
 
         buffer[1]
             = ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) | (((gctUINT32) (0x1 & ((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0)))
-              | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) | (((gctUINT32) (0x1 & ((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1)))
-              | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 2:2) - (0 ? 2:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 2:2) - (0 ? 2:2) + 1))))))) << (0 ? 2:2))) | (((gctUINT32) (0x1 & ((gctUINT32) ((((1 ? 2:2) - (0 ? 2:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 2:2) - (0 ? 2:2) + 1))))))) << (0 ? 2:2)))
-              | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 3:3) - (0 ? 3:3) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 3:3) - (0 ? 3:3) + 1))))))) << (0 ? 3:3))) | (((gctUINT32) (0x1 & ((gctUINT32) ((((1 ? 3:3) - (0 ? 3:3) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 3:3) - (0 ? 3:3) + 1))))))) << (0 ? 3:3)))
-              | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 4:4) - (0 ? 4:4) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 4:4) - (0 ? 4:4) + 1))))))) << (0 ? 4:4))) | (((gctUINT32) (0x1 & ((gctUINT32) ((((1 ? 4:4) - (0 ? 4:4) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 4:4) - (0 ? 4:4) + 1))))))) << (0 ? 4:4)));
+            | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) | (((gctUINT32) (0x1 & ((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1)))
+            | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 2:2) - (0 ? 2:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 2:2) - (0 ? 2:2) + 1))))))) << (0 ? 2:2))) | (((gctUINT32) (0x1 & ((gctUINT32) ((((1 ? 2:2) - (0 ? 2:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 2:2) - (0 ? 2:2) + 1))))))) << (0 ? 2:2)))
+            | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 3:3) - (0 ? 3:3) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 3:3) - (0 ? 3:3) + 1))))))) << (0 ? 3:3))) | (((gctUINT32) (0x1 & ((gctUINT32) ((((1 ? 3:3) - (0 ? 3:3) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 3:3) - (0 ? 3:3) + 1))))))) << (0 ? 3:3)))
+            | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 4:4) - (0 ? 4:4) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 4:4) - (0 ? 4:4) + 1))))))) << (0 ? 4:4))) | (((gctUINT32) (0x1 & ((gctUINT32) ((((1 ? 4:4) - (0 ? 4:4) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 4:4) - (0 ? 4:4) + 1))))))) << (0 ? 4:4)));
 
         gcmkONERROR(gckCOMMAND_Execute(command, 8));
     }
@@ -2849,9 +2857,9 @@ gckHARDWARE_FlushMMU(
     {
         flushSize =  16 * 4;
 
-    gcmkONERROR(gckCOMMAND_Reserve(
+        gcmkONERROR(gckCOMMAND_Reserve(
             command, flushSize, &pointer, &bufferSize
-        ));
+            ));
 
         buffer = (gctUINT32_PTR) pointer;
 
@@ -2860,8 +2868,8 @@ gckHARDWARE_FlushMMU(
         gcmkONERROR(gckOS_GetPhysicalAddress(command->os, buffer, &physical));
 
         /* Flush cache. */
-    buffer[0]
-        = ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 31:27) - (0 ? 31:27) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 31:27) - (0 ? 31:27) + 1))))))) << (0 ? 31:27))) | (((gctUINT32) (0x01 & ((gctUINT32) ((((1 ? 31:27) - (0 ? 31:27) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 31:27) - (0 ? 31:27) + 1))))))) << (0 ? 31:27)))
+        buffer[0]
+            = ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 31:27) - (0 ? 31:27) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 31:27) - (0 ? 31:27) + 1))))))) << (0 ? 31:27))) | (((gctUINT32) (0x01 & ((gctUINT32) ((((1 ? 31:27) - (0 ? 31:27) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 31:27) - (0 ? 31:27) + 1))))))) << (0 ? 31:27)))
             | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 25:16) - (0 ? 25:16) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 25:16) - (0 ? 25:16) + 1))))))) << (0 ? 25:16))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 25:16) - (0 ? 25:16) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 25:16) - (0 ? 25:16) + 1))))))) << (0 ? 25:16)))
             | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 15:0) - (0 ? 15:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 15:0) - (0 ? 15:0) + 1))))))) << (0 ? 15:0))) | (((gctUINT32) ((gctUINT32) (0x0E03) & ((gctUINT32) ((((1 ? 15:0) - (0 ? 15:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 15:0) - (0 ? 15:0) + 1))))))) << (0 ? 15:0)));
 
@@ -2903,7 +2911,7 @@ gckHARDWARE_FlushMMU(
         buffer[8]
             = ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 31:27) - (0 ? 31:27) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 31:27) - (0 ? 31:27) + 1))))))) << (0 ? 31:27))) | (((gctUINT32) (0x01 & ((gctUINT32) ((((1 ? 31:27) - (0 ? 31:27) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 31:27) - (0 ? 31:27) + 1))))))) << (0 ? 31:27)))
             | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 15:0) - (0 ? 15:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 15:0) - (0 ? 15:0) + 1))))))) << (0 ? 15:0))) | (((gctUINT32) ((gctUINT32) (0x0061) & ((gctUINT32) ((((1 ? 15:0) - (0 ? 15:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 15:0) - (0 ? 15:0) + 1))))))) << (0 ? 15:0)))
-        | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 25:16) - (0 ? 25:16) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 25:16) - (0 ? 25:16) + 1))))))) << (0 ? 25:16))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 25:16) - (0 ? 25:16) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 25:16) - (0 ? 25:16) + 1))))))) << (0 ? 25:16)));
+            | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 25:16) - (0 ? 25:16) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 25:16) - (0 ? 25:16) + 1))))))) << (0 ? 25:16))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 25:16) - (0 ? 25:16) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 25:16) - (0 ? 25:16) + 1))))))) << (0 ? 25:16)));
 
         buffer[9]
             = (((((gctUINT32) (~0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 4:4) - (0 ? 4:4) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 4:4) - (0 ? 4:4) + 1))))))) << (0 ? 4:4))) | (((gctUINT32) (0x1 & ((gctUINT32) ((((1 ? 4:4) - (0 ? 4:4) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 4:4) - (0 ? 4:4) + 1))))))) << (0 ? 4:4))) &  ((((gctUINT32) (~0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 7:7) - (0 ? 7:7) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 7:7) - (0 ? 7:7) + 1))))))) << (0 ? 7:7))) | (((gctUINT32) (0x0 & ((gctUINT32) ((((1 ? 7:7) - (0 ? 7:7) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 7:7) - (0 ? 7:7) + 1))))))) << (0 ? 7:7))) );
@@ -3460,62 +3468,6 @@ _PowerEnum(gceCHIPPOWERSTATE State)
 }
 #endif
 
-static gceSTATUS
-_GetClock(
-    IN gckHARDWARE Hardware,
-    IN gceCHIPPOWERSTATE State,
-    OUT gctUINT_PTR Clock
-)
-{
-    gctUINT clock;
-
-    /* Clocks. */
-    static const gctUINT clocks[4] =
-    {
-        /* gcvPOWER_ON */
-        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) | (((gctUINT32) ((gctUINT32) (0) & ((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) |
-        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) | (((gctUINT32) ((gctUINT32) (0) & ((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) |
-        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) | (((gctUINT32) ((gctUINT32) (64) & ((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) |
-        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))),
-
-        /* gcvPOWER_OFF */
-        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) |
-        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) |
-        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) |
-        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))),
-
-        /* gcvPOWER_IDLE */
-        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) | (((gctUINT32) ((gctUINT32) (0) & ((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) |
-        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) | (((gctUINT32) ((gctUINT32) (0) & ((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) |
-        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) |
-        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))),
-
-        /* gcvPOWER_SUSPEND */
-        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) |
-        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) |
-        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) |
-        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))),
-    };
-
-    gcmkHEADER();
-
-    gcmkVERIFY_ARGUMENT(State >= gcvPOWER_ON && State <= gcvPOWER_SUSPEND);
-
-    clock = clocks[State];
-
-#if gcdENABLE_FSCALE_VAL_ADJUST
-    if (State == gcvPOWER_ON)
-    {
-        clock = ((((gctUINT32) (clock)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) | (((gctUINT32) ((gctUINT32) (Hardware->powerOnFscaleVal) & ((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2)));
-    }
-#endif
-
-    *Clock = clock;
-
-    gcmkFOOTER();
-    return gcvSTATUS_OK;
-}
-
 /*******************************************************************************
 **
 **  gckHARDWARE_SetPowerManagementState
@@ -3555,6 +3507,9 @@ gckHARDWARE_SetPowerManagementState(
 #endif
     gctUINT32 process, thread;
     gctBOOL commitEntered = gcvFALSE;
+    gctBOOL commandStarted = gcvFALSE;
+    gctBOOL isrStarted = gcvFALSE;
+
 #if gcdENABLE_PROFILING
     gctUINT64 time, freq, mutexTime, onTime, stallTime, stopTime, delayTime,
               initTime, offTime, startTime, totalTime;
@@ -3619,6 +3574,34 @@ gckHARDWARE_SetPowerManagementState(
         },
     };
 
+    /* Clocks. */
+    static const gctUINT clocks[4] =
+    {
+        /* gcvPOWER_ON */
+        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) | (((gctUINT32) ((gctUINT32) (0) & ((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) |
+        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) | (((gctUINT32) ((gctUINT32) (0) & ((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) |
+        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) | (((gctUINT32) ((gctUINT32) (64) & ((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) |
+        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))),
+
+        /* gcvPOWER_OFF */
+        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) |
+        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) |
+        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) |
+        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))),
+
+        /* gcvPOWER_IDLE */
+        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) | (((gctUINT32) ((gctUINT32) (0) & ((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) |
+        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) | (((gctUINT32) ((gctUINT32) (0) & ((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) |
+        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) |
+        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))),
+
+        /* gcvPOWER_SUSPEND */
+        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) |
+        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) |
+        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) |
+        ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))),
+    };
+
     gcmkHEADER_ARG("Hardware=0x%x State=%d", Hardware, State);
 #if gcmIS_DEBUG(gcdDEBUG_TRACE)
     gcmkTRACE_ZONE(gcvLEVEL_INFO, gcvZONE_HARDWARE,
@@ -3759,7 +3742,14 @@ gckHARDWARE_SetPowerManagementState(
 
     /* Grab control flags and clock. */
     flag  = flags[Hardware->chipPowerState][State];
-    gcmkONERROR(_GetClock(Hardware, State, &clock));
+    clock = clocks[State];
+
+#if gcdENABLE_FSCALE_VAL_ADJUST
+    if (State == gcvPOWER_ON)
+    {
+        clock = ((((gctUINT32) (clock)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) | (((gctUINT32) ((gctUINT32) (Hardware->powerOnFscaleVal) & ((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2)));
+    }
+#endif
 
     if (State == gcvPOWER_SUSPEND && Hardware->chipPowerState == gcvPOWER_OFF && broadcast)
     {
@@ -4011,12 +4001,11 @@ gckHARDWARE_SetPowerManagementState(
         /* Stop the command parser. */
         gcmkONERROR(gckCOMMAND_Stop(command, gcvFALSE));
 
-#ifndef __QNXNTO__
         /* Stop the Isr. */
-        gcmkONERROR(Hardware->stopIsr(Hardware->isrContext));
-#else
-        /* QNX does not need to attach-dettach ISP that often */
-#endif
+        if (Hardware->stopIsr)
+        {
+            gcmkONERROR(Hardware->stopIsr(Hardware->isrContext));
+        }
     }
 
     /* Get time until stopped. */
@@ -4101,14 +4090,14 @@ gckHARDWARE_SetPowerManagementState(
     {
         /* Start the command processor. */
         gcmkONERROR(gckCOMMAND_Start(command));
+        commandStarted = gcvTRUE;
 
-#ifndef __QNXNTO__
-        /* Start the Isr. */
-        gcmkONERROR(Hardware->startIsr(Hardware->isrContext));
-#else
-        /* XSUN: QNX does not need to attach-dettach ISP that often
-         * with the current release */
-#endif
+        if (Hardware->startIsr)
+        {
+            /* Start the Isr. */
+            gcmkONERROR(Hardware->startIsr(Hardware->isrContext));
+            isrStarted = gcvTRUE;
+        }
 
         /* Set NEW MMU. */
         if (Hardware->mmuVersion != 0 && configMmu)
@@ -4198,6 +4187,16 @@ gckHARDWARE_SetPowerManagementState(
     return gcvSTATUS_OK;
 
 OnError:
+    if (commandStarted)
+    {
+        gcmkVERIFY_OK(gckCOMMAND_Stop(command, gcvFALSE));
+    }
+
+    if (isrStarted)
+    {
+        gcmkVERIFY_OK(Hardware->stopIsr(Hardware->isrContext));
+    }
+
     if (commitEntered)
     {
         /* Release the command queue mutex. */
@@ -4277,33 +4276,33 @@ gckHARDWARE_SetFscaleValue(
     gctUINT32 clock;
     gctBOOL acquired = gcvFALSE;
 
-    gcmkHEADER_ARG("Hardware=0x%x FscaleVal=%d", Hardware, FscaleVal);
+    gcmkHEADER_ARG("Hardware=0x%x FscaleValue=%d", Hardware, FscaleValue);
 
     gcmkVERIFY_ARGUMENT(FscaleValue > 0 && FscaleValue <= 64);
 
-    Hardware->powerOnFscaleVal = FscaleValue;
-
     gcmkONERROR(
         gckOS_AcquireMutex(Hardware->os, Hardware->powerMutex, gcvINFINITE));
     acquired =  gcvTRUE;
 
+    Hardware->powerOnFscaleVal = FscaleValue;
+
     if (Hardware->chipPowerState == gcvPOWER_ON)
     {
 		gctUINT32 data;
 
         gcmkONERROR(
-            gckOS_ReadRegisterEx(Hardware->os, 
-                                 Hardware->core, 
+            gckOS_ReadRegisterEx(Hardware->os,
+                                 Hardware->core,
                                  Hardware->powerBaseAddress
-                                 + 0x00104, 
+                                 + 0x00104,
                                  &data));
 
         /* Disable all clock gating. */
         gcmkONERROR(
-            gckOS_WriteRegisterEx(Hardware->os, 
-                                  Hardware->core, 
+            gckOS_WriteRegisterEx(Hardware->os,
+                                  Hardware->core,
                                   Hardware->powerBaseAddress
-                                  + 0x00104, 
+                                  + 0x00104,
                                   ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0)))
                                   | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1)))
                                   | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 2:2) - (0 ? 2:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 2:2) - (0 ? 2:2) + 1))))))) << (0 ? 2:2))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 2:2) - (0 ? 2:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 2:2) - (0 ? 2:2) + 1))))))) << (0 ? 2:2)))
@@ -4316,13 +4315,16 @@ gckHARDWARE_SetFscaleValue(
                                   | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9)))
                                   | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 11:11) - (0 ? 11:11) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 11:11) - (0 ? 11:11) + 1))))))) << (0 ? 11:11))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 11:11) - (0 ? 11:11) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 11:11) - (0 ? 11:11) + 1))))))) << (0 ? 11:11)))));
 
-        gcmkONERROR(_GetClock(Hardware, gcvPOWER_ON, &clock));
+        clock = ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0))) | (((gctUINT32) ((gctUINT32) (0) & ((gctUINT32) ((((1 ? 0:0) - (0 ? 0:0) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 0:0) - (0 ? 0:0) + 1))))))) << (0 ? 0:0)))
+              | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1))) | (((gctUINT32) ((gctUINT32) (0) & ((gctUINT32) ((((1 ? 1:1) - (0 ? 1:1) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 1:1) - (0 ? 1:1) + 1))))))) << (0 ? 1:1)))
+              | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2))) | (((gctUINT32) ((gctUINT32) (FscaleValue) & ((gctUINT32) ((((1 ? 8:2) - (0 ? 8:2) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:2) - (0 ? 8:2) + 1))))))) << (0 ? 8:2)))
+              | ((((gctUINT32) (0)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9)));
 
         gcmkONERROR(gckOS_WriteRegisterEx(Hardware->os,
                                           Hardware->core,
                                           0x00000,
                                           clock));
-        
+
         /* Done loading the frequency scaler. */
         gcmkONERROR(gckOS_WriteRegisterEx(Hardware->os,
                                           Hardware->core,
@@ -4331,12 +4333,11 @@ gckHARDWARE_SetFscaleValue(
 
         /* Restore all clock gating. */
         gcmkONERROR(
-            gckOS_WriteRegisterEx(Hardware->os, 
-                                  Hardware->core, 
+            gckOS_WriteRegisterEx(Hardware->os,
+                                  Hardware->core,
                                   Hardware->powerBaseAddress
-                                  + 0x00104, 
+                                  + 0x00104,
                                   data));
-
     }
 
     gcmkVERIFY(gckOS_ReleaseMutex(Hardware->os, Hardware->powerMutex));
@@ -4777,7 +4778,7 @@ _ResetGPU(
     {
         /* Disable clock gating. */
         gcmkONERROR(gckOS_WriteRegisterEx(Os,
-                                     Core,
+                    Core,
                     Hardware->powerBaseAddress +
                     0x00104,
                     0x00000000));
@@ -4802,7 +4803,7 @@ _ResetGPU(
 
         gcmkONERROR(gckOS_WriteRegisterEx(Os,
                     Core,
-                                     0x00000,
+                    0x00000,
                     ((((gctUINT32) (0x00000900)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9))) | (((gctUINT32) ((gctUINT32) (1) & ((gctUINT32) ((((1 ? 9:9) - (0 ? 9:9) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 9:9) - (0 ? 9:9) + 1))))))) << (0 ? 9:9)))));
 
         gcmkONERROR(gckOS_WriteRegisterEx(Os,
@@ -4889,6 +4890,8 @@ gckHARDWARE_Reset(
     gceSTATUS status;
     gckCOMMAND command;
     gctBOOL acquired = gcvFALSE;
+    gctBOOL mutexAcquired = gcvFALSE;
+    gctUINT32 process, thread;
 
     gcmkHEADER_ARG("Hardware=0x%x", Hardware);
 
@@ -4904,6 +4907,25 @@ gckHARDWARE_Reset(
         gcmkONERROR(gcvSTATUS_NOT_SUPPORTED);
     }
 
+    status = gckOS_AcquireMutex(Hardware->os, Hardware->powerMutex, 0);
+    if (status == gcvSTATUS_TIMEOUT)
+    {
+        gcmkONERROR(gckOS_GetProcessID(&process));
+        gcmkONERROR(gckOS_GetThreadID(&thread));
+
+        if ((Hardware->powerProcess == process)
+        &&  (Hardware->powerThread  == thread))
+        {
+            /* No way to recovery from a error in power management. */
+            gcmkFOOTER_NO();
+            return gcvSTATUS_OK;
+        }
+    }
+    else
+    {
+        mutexAcquired = gcvTRUE;
+    }
+
     if (Hardware->chipPowerState == gcvPOWER_ON)
     {
         /* Acquire the power management semaphore. */
@@ -4920,10 +4942,11 @@ gckHARDWARE_Reset(
         gcmkONERROR(gckCOMMAND_Stop(command, gcvTRUE));
     }
 
-#ifndef __QNXNTO__
     /* Stop isr, we will start it again when power on GPU. */
-    gcmkONERROR(Hardware->stopIsr(Hardware->isrContext));
-#endif
+    if (Hardware->stopIsr)
+    {
+        gcmkONERROR(Hardware->stopIsr(Hardware->isrContext));
+    }
 
     /* Hardware reset. */
     status = gckOS_ResetGPU(Hardware->os, Hardware->core);
@@ -4936,7 +4959,9 @@ gckHARDWARE_Reset(
 
     /* Force an OFF to ON power switch. */
     Hardware->chipPowerState = gcvPOWER_OFF;
-    gcmkONERROR(gckHARDWARE_SetPowerManagementState(Hardware, gcvPOWER_ON));
+
+    gcmkONERROR(gckOS_ReleaseMutex(Hardware->os, Hardware->powerMutex));
+    mutexAcquired = gcvFALSE;
 
     /* Success. */
     gcmkFOOTER_NO();
@@ -4950,6 +4975,11 @@ OnError:
             gckOS_ReleaseSemaphore(Hardware->os, command->powerSemaphore));
     }
 
+    if (mutexAcquired)
+    {
+        gckOS_ReleaseMutex(Hardware->os, Hardware->powerMutex);
+    }
+
     /* Return the error. */
     gcmkFOOTER();
     return status;
diff --git a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel.c b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel.c
index b14bf79c038a..15ed52c75208 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel.c
+++ b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel.c
@@ -89,6 +89,18 @@ gctCONST_STRING _DispatchText[] =
 };
 #endif
 
+#if gcdENABLE_RECOVERY
+void
+_ResetFinishFunction(
+    gctPOINTER Data
+    )
+{
+    gckKERNEL kernel = (gckKERNEL)Data;
+
+    gckOS_AtomSet(kernel->os, kernel->resetAtom, 0);
+}
+#endif
+
 /*******************************************************************************
 **
 **  gckKERNEL_Construct
@@ -246,6 +258,17 @@ gckKERNEL_Construct(
         /* Construct the gckMMU object. */
         gcmkONERROR(
             gckMMU_Construct(kernel, gcdMMU_SIZE, &kernel->mmu));
+
+#if gcdENABLE_RECOVERY
+        gcmkONERROR(
+            gckOS_AtomConstruct(Os, &kernel->resetAtom));
+
+        gcmkVERIFY_OK(
+            gckOS_CreateTimer(Os,
+                              (gctTIMERFUNCTION)_ResetFinishFunction,
+                              (gctPOINTER)kernel,
+                              &kernel->resetFlagClearTimer));
+#endif
     }
 
 #if VIVANTE_PROFILER
@@ -302,6 +325,19 @@ OnError:
             gcmkVERIFY_OK(gckOS_AtomDestroy(Os, kernel->atomClients));
         }
 
+#if gcdENABLE_RECOVERY
+        if (kernel->resetAtom != gcvNULL)
+        {
+            gcmkVERIFY_OK(gckOS_AtomDestroy(Os, kernel->resetAtom));
+        }
+
+        if (kernel->resetFlagClearTimer)
+        {
+            gcmkVERIFY_OK(gckOS_StopTimer(Os, kernel->resetFlagClearTimer));
+            gcmkVERIFY_OK(gckOS_DestoryTimer(Os, kernel->resetFlagClearTimer));
+        }
+#endif
+
         if (kernel->dbCreated && kernel->db != gcvNULL)
         {
             if (kernel->db->dbMutex != gcvNULL)
@@ -409,6 +445,16 @@ gckKERNEL_Destroy(
 
         /* Destroy the gckHARDWARE object. */
         gcmkVERIFY_OK(gckHARDWARE_Destroy(Kernel->hardware));
+
+#if gcdENABLE_RECOVERY
+        gcmkVERIFY_OK(gckOS_AtomDestroy(Kernel->os, Kernel->resetAtom));
+
+        if (Kernel->resetFlagClearTimer)
+        {
+            gcmkVERIFY_OK(gckOS_StopTimer(Kernel->os, Kernel->resetFlagClearTimer));
+            gcmkVERIFY_OK(gckOS_DestoryTimer(Kernel->os, Kernel->resetFlagClearTimer));
+        }
+#endif
     }
 
     /* Detsroy the client atom. */
@@ -787,6 +833,12 @@ gckKERNEL_Dispatch(
     case gcvHAL_FREE_NON_PAGED_MEMORY:
         physical = Interface->u.FreeNonPagedMemory.physical;
 
+        /* Unmap user logical out of physical memory first. */
+        gcmkONERROR(gckOS_UnmapUserLogical(Kernel->os,
+                                           physical,
+                                           Interface->u.FreeNonPagedMemory.bytes,
+                                           Interface->u.FreeNonPagedMemory.logical));
+
         /* Free non-paged memory. */
         gcmkONERROR(
             gckOS_FreeNonPagedMemory(Kernel->os,
@@ -833,6 +885,12 @@ gckKERNEL_Dispatch(
     case gcvHAL_FREE_CONTIGUOUS_MEMORY:
         physical = Interface->u.FreeContiguousMemory.physical;
 
+        /* Unmap user logical out of physical memory first. */
+        gcmkONERROR(gckOS_UnmapUserLogical(Kernel->os,
+                                           physical,
+                                           Interface->u.FreeContiguousMemory.bytes,
+                                           Interface->u.FreeContiguousMemory.logical));
+
         /* Free contiguous memory. */
         gcmkONERROR(
             gckOS_FreeContiguous(Kernel->os,
@@ -1124,10 +1182,70 @@ gckKERNEL_Dispatch(
             break;
 
         case gcvUSER_SIGNAL_WAIT:
-            /* Wait on the signal. */
-            status = gckOS_WaitUserSignal(Kernel->os,
-                                          Interface->u.UserSignal.id,
-                                          Interface->u.UserSignal.wait);
+#if gcdGPU_TIMEOUT
+            if (Interface->u.UserSignal.wait == gcvINFINITE)
+            {
+                gckHARDWARE hardware;
+                gctUINT32 timer = 0;
+
+                for(;;)
+                {
+                    /* Wait on the signal. */
+                    status = gckOS_WaitUserSignal(Kernel->os,
+                                                  Interface->u.UserSignal.id,
+                                                  gcdGPU_ADVANCETIMER);
+
+                    if (status == gcvSTATUS_TIMEOUT)
+                    {
+                        gcmkONERROR(
+                            gckOS_SignalQueryHardware(Kernel->os,
+                                                      (gctSIGNAL)Interface->u.UserSignal.id,
+                                                      &hardware));
+
+                        if (hardware)
+                        {
+                            /* This signal is bound to a hardware,
+                            ** so the timeout is limited by gcdGPU_TIMEOUT.
+                            */
+                            timer += gcdGPU_ADVANCETIMER;
+                        }
+
+                        if (timer >= gcdGPU_TIMEOUT)
+                        {
+                            gcmkONERROR(
+                                gckOS_Broadcast(Kernel->os,
+                                                hardware,
+                                                gcvBROADCAST_GPU_STUCK));
+
+                            timer = 0;
+
+                            /* If a few process try to reset GPU, only one
+                            ** of them can do the real reset, other processes
+                            ** still need to wait for this signal is triggered,
+                            ** which menas reset is finished.
+                            */
+                            continue;
+                        }
+                    }
+                    else
+                    {
+                        /* Bail out on other error. */
+                        gcmkONERROR(status);
+
+                        /* Wait for signal successfully. */
+                        break;
+                    }
+                }
+            }
+            else
+#endif
+            {
+                /* Wait on the signal. */
+                status = gckOS_WaitUserSignal(Kernel->os,
+                                              Interface->u.UserSignal.id,
+                                              Interface->u.UserSignal.wait);
+            }
+
             break;
 
         case gcvUSER_SIGNAL_MAP:
@@ -1823,6 +1941,7 @@ gckKERNEL_Dispatch(
         status = gcvSTATUS_NOT_SUPPORTED;
 #endif
         break;
+
     default:
         /* Invalid command. */
         gcmkONERROR(gcvSTATUS_INVALID_ARGUMENT);
@@ -1983,6 +2102,24 @@ gckKERNEL_AttachProcessEx(
             Kernel->db->lastProcessID = PID;
         }
 
+#if gcdENABLE_VG
+        if (Kernel->vg == gcvNULL)
+#endif
+        {
+            status = gckEVENT_Submit(Kernel->eventObj, gcvTRUE, gcvFALSE);
+
+            if (status == gcvSTATUS_INTERRUPTED && Kernel->eventObj->submitTimer)
+            {
+                gcmkONERROR(gckOS_StartTimer(Kernel->os,
+                                             Kernel->eventObj->submitTimer,
+                                             1));
+            }
+            else
+            {
+                gcmkONERROR(status);
+            }
+        }
+
         /* Decrement the number of clients attached. */
         gcmkONERROR(
             gckOS_AtomDecrement(Kernel->os, Kernel->atomClients, &old));
@@ -2530,7 +2667,7 @@ gckKERNEL_Recovery(
     )
 {
 #if gcdENABLE_RECOVERY
-#define gcvEVENT_MASK 0x3FFFFFFF
+#define gcdEVENT_MASK 0x3FFFFFFF
     gceSTATUS status;
     gckEVENT eventObj;
     gckHARDWARE hardware;
@@ -2538,7 +2675,7 @@ gckKERNEL_Recovery(
     gctUINT32 processID;
     gcskSECURE_CACHE_PTR cache;
 #endif
-
+    gctUINT32 oldValue;
     gcmkHEADER_ARG("Kernel=0x%x", Kernel);
 
     /* Validate the arguemnts. */
@@ -2552,22 +2689,6 @@ gckKERNEL_Recovery(
     hardware = Kernel->hardware;
     gcmkVERIFY_OBJECT(hardware, gcvOBJ_HARDWARE);
 
-    /* Handle all outstanding events now. */
-#if gcdSMP
-    gcmkONERROR(gckOS_AtomSet(Kernel->os, eventObj->pending, gcvEVENT_MASK));
-#else
-    eventObj->pending = gcvEVENT_MASK;
-#endif
-    gcmkONERROR(gckEVENT_Notify(eventObj, 1));
-
-    /* Again in case more events got submitted. */
-#if gcdSMP
-    gcmkONERROR(gckOS_AtomSet(Kernel->os, eventObj->pending, gcvEVENT_MASK));
-#else
-    eventObj->pending = gcvEVENT_MASK;
-#endif
-    gcmkONERROR(gckEVENT_Notify(eventObj, 2));
-
 #if gcdSECURE_USER
     /* Flush the secure mapping cache. */
     gcmkONERROR(gckOS_GetProcessID(&processID));
@@ -2575,6 +2696,23 @@ gckKERNEL_Recovery(
     gcmkONERROR(gckKERNEL_FlushTranslationCache(Kernel, cache, gcvNULL, 0));
 #endif
 
+    gcmkONERROR(
+        gckOS_AtomicExchange(Kernel->os, Kernel->resetAtom, 1, &oldValue));
+
+    if (oldValue)
+    {
+        /* Some one else will recovery GPU. */
+        return gcvSTATUS_OK;
+    }
+
+    /* Start a timer to clear reset flag, before timer is expired,
+    ** other recovery request is ignored. */
+    gcmkVERIFY_OK(
+        gckOS_StartTimer(Kernel->os,
+                         Kernel->resetFlagClearTimer,
+                         gcdGPU_TIMEOUT - 500));
+
+
     /* Try issuing a soft reset for the GPU. */
     status = gckHARDWARE_Reset(hardware);
     if (status == gcvSTATUS_NOT_SUPPORTED)
@@ -2591,6 +2729,22 @@ gckKERNEL_Recovery(
         gcmkONERROR(status);
     }
 
+    /* Handle all outstanding events now. */
+#if gcdSMP
+    gcmkONERROR(gckOS_AtomSet(Kernel->os, eventObj->pending, gcdEVENT_MASK));
+#else
+    eventObj->pending = gcdEVENT_MASK;
+#endif
+    gcmkONERROR(gckEVENT_Notify(eventObj, 1));
+
+    /* Again in case more events got submitted. */
+#if gcdSMP
+    gcmkONERROR(gckOS_AtomSet(Kernel->os, eventObj->pending, gcdEVENT_MASK));
+#else
+    eventObj->pending = gcdEVENT_MASK;
+#endif
+    gcmkONERROR(gckEVENT_Notify(eventObj, 2));
+
     /* Success. */
     gcmkFOOTER_NO();
     return gcvSTATUS_OK;
diff --git a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel.h b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel.h
index b627f3b1d5f9..1f3c9ccc20a8 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel.h
@@ -338,6 +338,11 @@ struct _gckKERNEL
     gckDB                       db;
     gctBOOL                     dbCreated;
 
+#if gcdENABLE_RECOVERY
+    gctPOINTER                  resetFlagClearTimer;
+    gctPOINTER                  resetAtom;
+#endif
+
     /* Pointer to gckEVENT object. */
     gcsTIMER                    timers[8];
     gctUINT32                   timeOut;
@@ -525,6 +530,8 @@ struct _gckEVENT
     gcsEVENT_QUEUE_PTR          freeList;
     gcsEVENT_QUEUE              repoList[gcdREPO_LIST_COUNT];
     gctPOINTER                  eventListMutex;
+
+    gctPOINTER                  submitTimer;
 };
 
 /* Free all events belonging to a process. */
diff --git a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_command.c b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_command.c
index 8cf0509cfd92..fbaff6666a10 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_command.c
+++ b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_command.c
@@ -30,6 +30,9 @@
 
 #define _GC_OBJ_ZONE            gcvZONE_COMMAND
 
+#if gcdENABLE_FSCALE_VAL_ADJUST
+extern int thermal_hot;
+#endif
 /******************************************************************************\
 ********************************* Support Code *********************************
 \******************************************************************************/
@@ -1080,6 +1083,24 @@ gckCOMMAND_Commit(
     /* Extract the gckHARDWARE and gckEVENT objects. */
     hardware = Command->kernel->hardware;
 
+#if gcdENABLE_FSCALE_VAL_ADJUST
+    if(hardware->core == gcvCORE_MAJOR){
+        static gctUINT orgFscale,minFscale,maxFscale;
+        static gctBOOL bAlreadyTooHot = gcvFALSE;
+        if((thermal_hot > 0) && (!bAlreadyTooHot)) {
+            gckHARDWARE_GetFscaleValue(hardware,&orgFscale,&minFscale, &maxFscale);
+            gckHARDWARE_SetFscaleValue(hardware, minFscale);
+            bAlreadyTooHot = gcvTRUE;
+            gckOS_Print("System is too hot. GPU3D will work at %d/64 clock.\n", minFscale);
+        } else if((!(thermal_hot > 0)) && bAlreadyTooHot) {
+            gckHARDWARE_SetFscaleValue(hardware, orgFscale);
+            gckOS_Print("Hot alarm is canceled. GPU3D clock will return to %d/64\n", orgFscale);
+            bAlreadyTooHot = gcvFALSE;
+        }
+
+    }
+#endif
+
     /* Check wehther we need to copy the structures or not. */
     gcmkONERROR(gckOS_QueryNeedCopy(Command->os, ProcessID, &needCopy));
 
@@ -2619,6 +2640,12 @@ gckCOMMAND_Detach(
     /* Construct a gckCONTEXT object. */
     gcmkONERROR(gckCONTEXT_Destroy(Context));
 
+    if (Command->currContext == Context)
+    {
+        /* Detach from gckCOMMAND object if the destoryed context is current context. */
+        Command->currContext = gcvNULL;
+    }
+
     /* Release the context switching mutex. */
     gcmkONERROR(gckOS_ReleaseMutex(Command->os, Command->mutexContext));
     acquired = gcvFALSE;
diff --git a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_command_vg.c b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_command_vg.c
index 227ab683778e..e5b659262a46 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_command_vg.c
+++ b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_command_vg.c
@@ -97,6 +97,30 @@ gcsQUEUE_UPDATE_CONTROL;
 /******************************************************************************\
 ********************************* Support Code *********************************
 \******************************************************************************/
+static gceSTATUS
+_FlushMMU(
+    IN gckVGCOMMAND Command
+    )
+{
+    gceSTATUS status;
+    gctUINT32 oldValue;
+    gckVGHARDWARE hardware = Command->hardware;
+
+    gcmkONERROR(gckOS_AtomicExchange(Command->os,
+                                     hardware->pageTableDirty,
+                                     0,
+                                     &oldValue));
+
+    if (oldValue)
+    {
+        /* Page Table is upated, flush mmu before commit. */
+        gcmkONERROR(gckVGHARDWARE_FlushMMU(hardware));
+    }
+
+    return gcvSTATUS_OK;
+OnError:
+    return status;
+}
 
 static gceSTATUS
 _WaitForIdle(
@@ -577,6 +601,67 @@ _FreeTaskContainer(
     }
 }
 
+gceSTATUS
+_RemoveRecordFromProcesDB(
+    IN gckVGCOMMAND Command,
+    IN gcsTASK_HEADER_PTR Task
+    )
+{
+    gcsTASK_PTR task = (gcsTASK_PTR)((gctUINT8_PTR)Task - sizeof(gcsTASK));
+    gcsTASK_FREE_VIDEO_MEMORY_PTR freeVideoMemory;
+    gcsTASK_UNLOCK_VIDEO_MEMORY_PTR unlockVideoMemory;
+    gctINT pid;
+    gctUINT32 size;
+
+    /* Get the total size of all tasks. */
+    size = task->size;
+
+    gcmkVERIFY_OK(gckOS_GetProcessID((gctUINT32_PTR)&pid));
+
+    do
+    {
+        switch (Task->id)
+        {
+        case gcvTASK_FREE_VIDEO_MEMORY:
+            freeVideoMemory = (gcsTASK_FREE_VIDEO_MEMORY_PTR)Task;
+
+            /* Remove record from process db. */
+            gcmkVERIFY_OK(gckKERNEL_RemoveProcessDB(
+                Command->kernel->kernel,
+                pid,
+                gcvDB_VIDEO_MEMORY,
+                freeVideoMemory->node));
+
+            /* Advance to next task. */
+            size -= sizeof(gcsTASK_FREE_VIDEO_MEMORY);
+            Task = (gcsTASK_HEADER_PTR)(freeVideoMemory + 1);
+
+            break;
+        case gcvTASK_UNLOCK_VIDEO_MEMORY:
+            unlockVideoMemory = (gcsTASK_UNLOCK_VIDEO_MEMORY_PTR)Task;
+
+            /* Remove record from process db. */
+            gcmkVERIFY_OK(gckKERNEL_RemoveProcessDB(
+                Command->kernel->kernel,
+                pid,
+                gcvDB_VIDEO_MEMORY_LOCKED,
+                unlockVideoMemory->node));
+
+            /* Advance to next task. */
+            size -= sizeof(gcsTASK_UNLOCK_VIDEO_MEMORY);
+            Task = (gcsTASK_HEADER_PTR)(unlockVideoMemory + 1);
+
+            break;
+        default:
+            /* Skip the whole task. */
+            size = 0;
+            break;
+        }
+    }
+    while(size);
+
+    return gcvSTATUS_OK;
+}
 
 /******************************************************************************\
 ********************************* Task Scheduling ******************************
@@ -701,6 +786,8 @@ _ScheduleTasks(
                 {
                     gcsTASK_HEADER_PTR taskHeader = (gcsTASK_HEADER_PTR) (userTask + 1);
 
+                    gcmkVERIFY_OK(_RemoveRecordFromProcesDB(Command, taskHeader));
+
                     gcmkTRACE_ZONE(
                         gcvLEVEL_VERBOSE, gcvZONE_COMMAND,
                         "    task ID = %d, size = %d\n",
@@ -3363,6 +3450,8 @@ gckVGCOMMAND_Commit(
             break;
         }
 #endif
+        gcmkERR_BREAK(_FlushMMU(Command));
+
         do
         {
             /* Assign a context ID if not yet assigned. */
diff --git a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_db.c b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_db.c
index d94d8e2d6704..9dbb9d3cab25 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_db.c
+++ b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_db.c
@@ -1144,6 +1144,12 @@ gckKERNEL_DestroyProcessDB(
             break;
 
         case gcvDB_NON_PAGED:
+            /* Unmap user logical memory first. */
+            status = gckOS_UnmapUserLogical(Kernel->os,
+                                            record->physical,
+                                            record->bytes,
+                                            record->data);
+
             /* Free the non paged memory. */
             status = gckOS_FreeNonPagedMemory(Kernel->os,
                                               record->bytes,
@@ -1156,11 +1162,18 @@ gckKERNEL_DestroyProcessDB(
             break;
 
         case gcvDB_CONTIGUOUS:
+            /* Unmap user logical memory first. */
+            status = gckOS_UnmapUserLogical(Kernel->os,
+                                            record->physical,
+                                            record->bytes,
+                                            record->data);
+
             /* Free the contiguous memory. */
-            status = gckOS_FreeContiguous(Kernel->os,
-                                          record->physical,
-                                          record->data,
-                                          record->bytes);
+            status = gckEVENT_FreeContiguousMemory(Kernel->eventObj,
+                                                   record->bytes,
+                                                   record->physical,
+                                                   record->data,
+                                                   gcvKERNEL_PIXEL);
 
             gcmkTRACE_ZONE(gcvLEVEL_WARNING, gcvZONE_DATABASE,
                            "DB: CONTIGUOUS 0x%x bytes=%lu (status=%d)",
diff --git a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_event.c b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_event.c
index e1b21a0df70b..d38312d3bf9b 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_event.c
+++ b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_event.c
@@ -328,6 +328,12 @@ __RemoveRecordFromProcessDB(
                 Record->info.u.UnmapUserMemory.info));
             break;
 
+        case gcvHAL_SIGNAL:
+            gcmkVERIFY_OK(gckOS_SignalSetHardware(Event->os,
+                Record->info.u.Signal.signal,
+                Event->kernel->hardware));
+            break;
+
         default:
             break;
         }
@@ -338,6 +344,15 @@ __RemoveRecordFromProcessDB(
     return gcvSTATUS_OK;
 }
 
+void
+_SubmitTimerFunction(
+    gctPOINTER Data
+    )
+{
+    gckEVENT event = (gckEVENT)Data;
+    gcmkVERIFY_OK(gckEVENT_Submit(event, gcvTRUE, gcvFALSE));
+}
+
 /******************************************************************************\
 ******************************* gckEVENT API Code *******************************
 \******************************************************************************/
@@ -430,6 +445,11 @@ gckEVENT_Construct(
     gcmkONERROR(gckOS_AtomConstruct(os, &eventObj->pending));
 #endif
 
+    gcmkVERIFY_OK(gckOS_CreateTimer(os,
+                                    _SubmitTimerFunction,
+                                    (gctPOINTER)eventObj,
+                                    &eventObj->submitTimer));
+
     /* Return pointer to the gckEVENT object. */
     *Event = eventObj;
 
@@ -511,6 +531,12 @@ gckEVENT_Destroy(
     /* Verify the arguments. */
     gcmkVERIFY_OBJECT(Event, gcvOBJ_EVENT);
 
+    if (Event->submitTimer != gcvNULL)
+    {
+        gcmkVERIFY_OK(gckOS_StopTimer(Event->os, Event->submitTimer));
+        gcmkVERIFY_OK(gckOS_DestoryTimer(Event->os, Event->submitTimer));
+    }
+
     /* Delete the queue mutex. */
     gcmkVERIFY_OK(gckOS_DeleteMutex(Event->os, Event->eventQueueMutex));
 
@@ -723,11 +749,6 @@ gckEVENT_GetEvent(
                 __FUNCTION__, __LINE__
                 );
 
-            /* Broadcast GPU stuck. */
-            gcmkONERROR(gckOS_Broadcast(Event->os,
-                                        Event->kernel->hardware,
-                                        gcvBROADCAST_GPU_STUCK));
-
             /* Bail out. */
             gcmkONERROR(gcvSTATUS_GPU_NOT_RESPONDING);
         }
@@ -961,6 +982,31 @@ gckEVENT_AddList(
         queue->tail       = record;
     }
 
+    /* Unmap user space logical address.
+     * Linux kernel does not support unmap the memory of other process any more since 3.5.
+     * Let's unmap memory of self process before submit the event to gpu.
+     * */
+    switch(Interface->command)
+    {
+    case gcvHAL_FREE_NON_PAGED_MEMORY:
+        gcmkONERROR(gckOS_UnmapUserLogical(
+                        Event->os,
+                        Interface->u.FreeNonPagedMemory.physical,
+                        Interface->u.FreeNonPagedMemory.bytes,
+                        Interface->u.FreeNonPagedMemory.logical));
+        break;
+    case gcvHAL_FREE_CONTIGUOUS_MEMORY:
+        gcmkONERROR(gckOS_UnmapUserLogical(
+                        Event->os,
+                        Interface->u.FreeContiguousMemory.physical,
+                        Interface->u.FreeContiguousMemory.bytes,
+                        Interface->u.FreeContiguousMemory.logical));
+        break;
+    default:
+        break;
+    }
+
+
     /* Release the mutex. */
     gcmkONERROR(gckOS_ReleaseMutex(Event->os, Event->eventListMutex));
 
@@ -1494,6 +1540,14 @@ OnError:
         Event->queues[id].head = gcvNULL;
     }
 
+    if (status == gcvSTATUS_GPU_NOT_RESPONDING)
+    {
+        /* Broadcast GPU stuck. */
+        status = gckOS_Broadcast(Event->os,
+                                 Event->kernel->hardware,
+                                 gcvBROADCAST_GPU_STUCK);
+    }
+
     /* Return the status. */
     gcmkFOOTER();
     return status;
diff --git a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_mmu.c b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_mmu.c
index f7a70ac96f58..9012599b1bef 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_mmu.c
+++ b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_mmu.c
@@ -79,6 +79,23 @@ static gcsSharedPageTable_PTR sharedPageTable = gcvNULL;
 #endif
 
 static gceSTATUS
+_FillPageTable(
+    IN gctUINT32_PTR PageTable,
+    IN gctUINT32     PageCount,
+    IN gctUINT32     EntryValue
+)
+{
+    gctUINT i;
+
+    for (i = 0; i < PageCount; i++)
+    {
+        PageTable[i] = EntryValue;
+    }
+
+    return gcvSTATUS_OK;
+}
+
+static gceSTATUS
 _Link(
     IN gckMMU Mmu,
     IN gctUINT32 Index,
@@ -1036,7 +1053,7 @@ gckMMU_AllocatePages(
     }
 
     /* Mark node as used. */
-    pageTable[index] = gcvMMU_USED;
+    gcmkONERROR(_FillPageTable(&pageTable[index], PageCount, gcvMMU_USED));
 
     /* Return pointer to page table. */
     *PageTable = &pageTable[index];
@@ -1112,6 +1129,8 @@ gckMMU_FreePages(
     )
 {
     gctUINT32_PTR pageTable;
+    gceSTATUS status;
+    gctBOOL acquired = gcvFALSE;
 
     gcmkHEADER_ARG("Mmu=0x%x PageTable=0x%x PageCount=%lu",
                    Mmu, PageTable, PageCount);
@@ -1124,7 +1143,11 @@ gckMMU_FreePages(
     /* Convert the pointer. */
     pageTable = (gctUINT32_PTR) PageTable;
 
+    gcmkONERROR(gckOS_AcquireMutex(Mmu->os, Mmu->pageTableMutex, gcvINFINITE));
+    acquired = gcvTRUE;
+
 #if gcdMMU_CLEAR_VALUE
+    if (Mmu->hardware->mmuVersion == 0)
     {
         gctUINT32 i;
 
@@ -1150,9 +1173,21 @@ gckMMU_FreePages(
     /* We have free nodes. */
     Mmu->freeNodes = gcvTRUE;
 
+    gcmkVERIFY_OK(gckOS_ReleaseMutex(Mmu->os, Mmu->pageTableMutex));
+    acquired = gcvFALSE;
+
     /* Success. */
     gcmkFOOTER_NO();
     return gcvSTATUS_OK;
+
+OnError:
+    if (acquired)
+    {
+        gcmkVERIFY_OK(gckOS_ReleaseMutex(Mmu->os, Mmu->pageTableMutex));
+    }
+
+    gcmkFOOTER();
+    return status;
 }
 
 gceSTATUS
diff --git a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_mmu_vg.c b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_mmu_vg.c
index a4a5e0c802f5..46d1761dac2c 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_mmu_vg.c
+++ b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_mmu_vg.c
@@ -344,9 +344,6 @@ gceSTATUS gckVGMMU_AllocatePages(
 
     if (!allocated)
     {
-        /* Flush the MMU. */
-        status = gckVGHARDWARE_FlushMMU(Mmu->hardware);
-
         if (status >= 0)
         {
             /* Walk all entries until we find enough slots. */
@@ -506,4 +503,22 @@ gckVGMMU_SetPage(
     return gcvSTATUS_OK;
 }
 
+gceSTATUS
+gckVGMMU_Flush(
+   IN gckVGMMU Mmu
+   )
+{
+    gckVGHARDWARE hardware;
+
+    gcmkHEADER_ARG("Mmu=0x%x", Mmu);
+
+    hardware = Mmu->hardware;
+    gcmkVERIFY_OK(
+        gckOS_AtomSet(hardware->os, hardware->pageTableDirty, 1));
+
+    /* Success. */
+    gcmkFOOTER_NO();
+    return gcvSTATUS_OK;
+}
+
 #endif /* gcdENABLE_VG */
diff --git a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_vg.c b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_vg.c
index bb5330c2bbb0..ee63e9b59373 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_vg.c
+++ b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_vg.c
@@ -438,6 +438,14 @@ gceSTATUS gckVGKERNEL_Dispatch(
         break;
 
     case gcvHAL_FREE_NON_PAGED_MEMORY:
+        /* Unmap user logical out of physical memory first. */
+        gcmkERR_BREAK(gckOS_UnmapUserLogical(
+            Kernel->os,
+            kernelInterface->u.AllocateNonPagedMemory.physical,
+            kernelInterface->u.AllocateNonPagedMemory.bytes,
+            kernelInterface->u.AllocateNonPagedMemory.logical
+            ));
+
         /* Free non-paged memory. */
         gcmkERR_BREAK(gckOS_FreeNonPagedMemory(
             Kernel->os,
@@ -459,6 +467,14 @@ gceSTATUS gckVGKERNEL_Dispatch(
         break;
 
     case gcvHAL_FREE_CONTIGUOUS_MEMORY:
+        /* Unmap user logical out of physical memory first. */
+        gcmkERR_BREAK(gckOS_UnmapUserLogical(
+            Kernel->os,
+            kernelInterface->u.AllocateNonPagedMemory.physical,
+            kernelInterface->u.AllocateNonPagedMemory.bytes,
+            kernelInterface->u.AllocateNonPagedMemory.logical
+            ));
+
         /* Free contiguous memory. */
         gcmkERR_BREAK(gckOS_FreeContiguous(
             Kernel->os,
@@ -522,6 +538,14 @@ gceSTATUS gckVGKERNEL_Dispatch(
             kernelInterface->u.AllocateLinearVideoMemory.type,
             &kernelInterface->u.AllocateLinearVideoMemory.node
             ));
+
+        gcmkERR_BREAK(gckKERNEL_AddProcessDB(Kernel,
+           processID, gcvDB_VIDEO_MEMORY,
+           Interface->u.AllocateLinearVideoMemory.node,
+           gcvNULL,
+           kernelInterface->u.AllocateLinearVideoMemory.bytes
+           ));
+
         break;
 
     case gcvHAL_FREE_VIDEO_MEMORY:
@@ -544,6 +568,13 @@ gceSTATUS gckVGKERNEL_Dispatch(
         gcmkERR_BREAK(gckVIDMEM_Free(
             Interface->u.FreeVideoMemory.node
             ));
+
+        gcmkERR_BREAK(gckKERNEL_RemoveProcessDB(
+            Kernel,
+            processID, gcvDB_VIDEO_MEMORY,
+            Interface->u.FreeVideoMemory.node
+            ));
+
         break;
 
     case gcvHAL_MAP_MEMORY:
@@ -637,6 +668,12 @@ gceSTATUS gckVGKERNEL_Dispatch(
         Interface->u.LockVideoMemory.address =
             gcmPTR2INT(Interface->u.LockVideoMemory.memory);
 #endif
+        gcmkERR_BREAK(
+            gckKERNEL_AddProcessDB(Kernel,
+                                   processID, gcvDB_VIDEO_MEMORY_LOCKED,
+                                   Interface->u.LockVideoMemory.node,
+                                   gcvNULL,
+                                   0));
         break;
 
     case gcvHAL_UNLOCK_VIDEO_MEMORY:
@@ -674,6 +711,16 @@ gceSTATUS gckVGKERNEL_Dispatch(
                                                           bytes));
         }
 #endif
+
+        if (Interface->u.UnlockVideoMemory.asynchroneous == gcvFALSE)
+        {
+            /* There isn't a event to unlock this node, remove record now */
+            gcmkERR_BREAK(
+                    gckKERNEL_RemoveProcessDB(Kernel,
+                        processID, gcvDB_VIDEO_MEMORY_LOCKED,
+                        Interface->u.UnlockVideoMemory.node));
+        }
+
         break;
     case gcvHAL_USER_SIGNAL:
 #if !USE_NEW_LINUX_SIGNAL
diff --git a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_video_memory.c b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_video_memory.c
index 9d6da636755c..dc39b3927fff 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_video_memory.c
+++ b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_video_memory.c
@@ -1830,7 +1830,11 @@ gckVIDMEM_Lock(
 #endif
 
 #if gcdENABLE_VG
-                if (Kernel->core != gcvCORE_VG)
+                if (Kernel->core == gcvCORE_VG)
+                {
+                    gcmkONERROR(gckVGMMU_Flush(Kernel->vg->mmu));
+                }
+                else
 #endif
                 {
                     gcmkONERROR(gckMMU_Flush(Kernel->mmu));
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal.h
index 25ec3eab95f2..5bf887054e04 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal.h
@@ -30,6 +30,9 @@
 #include "gc_hal_base.h"
 #include "gc_hal_profiler.h"
 #include "gc_hal_driver.h"
+#ifndef VIVANTE_NO_3D
+#include "gc_hal_statistics.h"
+#endif
 
 #ifdef __cplusplus
 extern "C" {
@@ -503,6 +506,17 @@ gckOS_UnmapMemory(
     IN gctPOINTER Logical
     );
 
+/* Unmap user logical memory out of physical memory.
+ * This function is only supported in Linux currently.
+ */
+gceSTATUS
+gckOS_UnmapUserLogical(
+    IN gckOS Os,
+    IN gctPHYS_ADDR Physical,
+    IN gctSIZE_T Bytes,
+    IN gctPOINTER Logical
+    );
+
 /* Create a new mutex. */
 gceSTATUS
 gckOS_CreateMutex(
@@ -1950,6 +1964,22 @@ gckHARDWARE_QueryPowerManagementState(
     OUT gceCHIPPOWERSTATE* State
     );
 
+#if gcdENABLE_FSCALE_VAL_ADJUST
+gceSTATUS
+gckHARDWARE_SetFscaleValue(
+    IN gckHARDWARE Hardware,
+    IN gctUINT32   FscaleValue
+    );
+
+gceSTATUS
+gckHARDWARE_GetFscaleValue(
+    IN gckHARDWARE Hardware,
+    IN gctUINT * FscaleValue,
+    IN gctUINT * MinFscaleValue,
+    IN gctUINT * MaxFscaleValue
+    );
+#endif
+
 #if gcdPOWEROFF_TIMEOUT
 gceSTATUS
 gckHARDWARE_SetPowerOffTimeout(
@@ -2361,7 +2391,19 @@ gckHARDWARE_QueryProfileRegisters(
     );
 #endif
 
+gceSTATUS
+gckOS_SignalQueryHardware(
+    IN gckOS Os,
+    IN gctSIGNAL Signal,
+    OUT gckHARDWARE * Hardware
+    );
 
+gceSTATUS
+gckOS_SignalSetHardware(
+    IN gckOS Os,
+    IN gctSIGNAL Signal,
+    gckHARDWARE Hardware
+    );
 
 #ifdef __cplusplus
 }
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_base.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_base.h
index 85b41312f772..7cfd2e119f00 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_base.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_base.h
@@ -747,6 +747,25 @@ gcoOS_FreeContiguous(
     IN gctSIZE_T Bytes
     );
 
+/* Allocate video memory. */
+gceSTATUS
+gcoOS_AllocateVideoMemory(
+    IN gcoOS Os,
+    IN gctBOOL InUserSpace,
+    IN gctBOOL InCacheable,
+    IN OUT gctSIZE_T * Bytes,
+    OUT gctUINT32 * Physical,
+    OUT gctPOINTER * Logical,
+    OUT gctPOINTER * Handle
+    );
+
+/* Free video memory. */
+gceSTATUS
+gcoOS_FreeVideoMemory(
+    IN gcoOS Os,
+    IN gctPOINTER Handle
+    );
+
 #if gcdENABLE_BANK_ALIGNMENT
 gceSTATUS
 gcoSURF_GetBankOffsetBytes(
@@ -3659,6 +3678,42 @@ gckOS_DebugStatus2Name(
 
 #define MAX_LOOP_COUNT 0x7FFFFFFF
 
+/******************************************************************************\
+****************************** User Debug Option ******************************
+\******************************************************************************/
+
+/* User option. */
+typedef enum _gceDEBUG_MSG
+{
+    gcvDEBUG_MSG_NONE,
+    gcvDEBUG_MSG_ERROR,
+    gcvDEBUG_MSG_WARNING
+}
+gceDEBUG_MSG;
+
+typedef struct _gcsUSER_DEBUG_OPTION
+{
+    gceDEBUG_MSG        debugMsg;
+}
+gcsUSER_DEBUG_OPTION;
+
+gcsUSER_DEBUG_OPTION *
+gcGetUserDebugOption(
+    void
+    );
+
+#define gcmUSER_DEBUG_MSG(level, ...) \
+    do \
+    { \
+        if (level <= gcGetUserDebugOption()->debugMsg) \
+        { \
+            gcoOS_Print(__VA_ARGS__); \
+        } \
+    } while (gcvFALSE)
+
+#define gcmUSER_DEBUG_ERROR_MSG(...)   gcmUSER_DEBUG_MSG(gcvDEBUG_MSG_ERROR, "Error: " __VA_ARGS__)
+#define gcmUSER_DEBUG_WARNING_MSG(...) gcmUSER_DEBUG_MSG(gcvDEBUG_MSG_WARNING, "Warring: " __VA_ARGS__)
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_eglplatform.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_eglplatform.h
index d0a98a2b71c8..86ab105b389a 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_eglplatform.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_eglplatform.h
@@ -27,216 +27,11 @@
 /* Include VDK types. */
 #include "gc_hal_types.h"
 #include "gc_hal_base.h"
+#include "gc_hal_eglplatform_type.h"
 #ifdef __cplusplus
 extern "C" {
 #endif
 
-/*******************************************************************************
-** Events. *********************************************************************
-*/
-
-typedef enum _halEventType
-{
-	/* Keyboard event. */
-    HAL_KEYBOARD,
-
-	/* Mouse move event. */
-    HAL_POINTER,
-
-	/* Mouse button event. */
-    HAL_BUTTON,
-
-	/* Application close event. */
-	HAL_CLOSE,
-
-	/* Application window has been updated. */
-	HAL_WINDOW_UPDATE
-}
-halEventType;
-
-/* Scancodes for keyboard. */
-typedef enum _halKeys
-{
-    HAL_UNKNOWN = -1,
-
-    HAL_BACKSPACE = 0x08,
-    HAL_TAB,
-    HAL_ENTER = 0x0D,
-    HAL_ESCAPE = 0x1B,
-
-    HAL_SPACE = 0x20,
-    HAL_SINGLEQUOTE = 0x27,
-    HAL_PAD_ASTERISK = 0x2A,
-    HAL_COMMA = 0x2C,
-    HAL_HYPHEN,
-    HAL_PERIOD,
-    HAL_SLASH,
-    HAL_0,
-    HAL_1,
-    HAL_2,
-    HAL_3,
-    HAL_4,
-    HAL_5,
-    HAL_6,
-    HAL_7,
-    HAL_8,
-    HAL_9,
-    HAL_SEMICOLON = 0x3B,
-    HAL_EQUAL = 0x3D,
-    HAL_A = 0x41,
-    HAL_B,
-    HAL_C,
-    HAL_D,
-    HAL_E,
-    HAL_F,
-    HAL_G,
-    HAL_H,
-    HAL_I,
-    HAL_J,
-    HAL_K,
-    HAL_L,
-    HAL_M,
-    HAL_N,
-    HAL_O,
-    HAL_P,
-    HAL_Q,
-    HAL_R,
-    HAL_S,
-    HAL_T,
-    HAL_U,
-    HAL_V,
-    HAL_W,
-    HAL_X,
-    HAL_Y,
-    HAL_Z,
-    HAL_LBRACKET,
-    HAL_BACKSLASH,
-    HAL_RBRACKET,
-    HAL_BACKQUOTE = 0x60,
-
-    HAL_F1 = 0x80,
-    HAL_F2,
-    HAL_F3,
-    HAL_F4,
-    HAL_F5,
-    HAL_F6,
-    HAL_F7,
-    HAL_F8,
-    HAL_F9,
-    HAL_F10,
-    HAL_F11,
-    HAL_F12,
-
-    HAL_LCTRL,
-    HAL_RCTRL,
-    HAL_LSHIFT,
-    HAL_RSHIFT,
-    HAL_LALT,
-    HAL_RALT,
-    HAL_CAPSLOCK,
-    HAL_NUMLOCK,
-    HAL_SCROLLLOCK,
-    HAL_PAD_0,
-    HAL_PAD_1,
-    HAL_PAD_2,
-    HAL_PAD_3,
-    HAL_PAD_4,
-    HAL_PAD_5,
-    HAL_PAD_6,
-    HAL_PAD_7,
-    HAL_PAD_8,
-    HAL_PAD_9,
-    HAL_PAD_HYPHEN,
-    HAL_PAD_PLUS,
-    HAL_PAD_SLASH,
-    HAL_PAD_PERIOD,
-    HAL_PAD_ENTER,
-    HAL_SYSRQ,
-    HAL_PRNTSCRN,
-    HAL_BREAK,
-    HAL_UP,
-    HAL_LEFT,
-    HAL_RIGHT,
-    HAL_DOWN,
-    HAL_HOME,
-    HAL_END,
-    HAL_PGUP,
-    HAL_PGDN,
-    HAL_INSERT,
-    HAL_DELETE,
-    HAL_LWINDOW,
-    HAL_RWINDOW,
-    HAL_MENU,
-    HAL_POWER,
-    HAL_SLEEP,
-    HAL_WAKE
-}
-halKeys;
-
-/* Structure that defined keyboard mapping. */
-typedef struct _halKeyMap
-{
-	/* Normal key. */
-    halKeys normal;
-
-	/* Extended key. */
-    halKeys extended;
-}
-halKeyMap;
-
-/* Event structure. */
-typedef struct _halEvent
-{
-	/* Event type. */
-    halEventType type;
-
-	/* Event data union. */
-    union _halEventData
-    {
-		/* Event data for keyboard. */
-        struct _halKeyboard
-        {
-			/* Scancode. */
-            halKeys	scancode;
-
-			/* ASCII characte of the key pressed. */
-            gctCHAR	key;
-
-			/* Flag whether the key was pressed (1) or released (0). */
-            gctCHAR	pressed;
-        }
-        keyboard;
-
-		/* Event data for pointer. */
-        struct _halPointer
-        {
-			/* Current pointer coordinate. */
-            gctINT		x;
-            gctINT		y;
-        }
-        pointer;
-
-		/* Event data for mouse buttons. */
-        struct _halButton
-        {
-			/* Left button state. */
-            gctINT		left;
-
-			/* Middle button state. */
-            gctINT		middle;
-
-			/* Right button state. */
-            gctINT		right;
-
-			/* Current pointer coordinate. */
-			gctINT		x;
-			gctINT		y;
-        }
-        button;
-    }
-    data;
-}
-halEvent;
 
 #if defined(_WIN32) || defined(__VC32__) && !defined(__CYGWIN__) && !defined(__SCITECH_SNAP__)
 /* Win32 and Windows CE platforms. */
@@ -379,7 +174,16 @@ typedef void *  HALNativePixmapType;
 
 #endif
 
-
+/* define DUMMY according to the system */
+#if defined(EGL_API_WL)
+#   define WL_DUMMY (31415926)
+#   define EGL_DUMMY WL_DUMMY
+#elif defined(__ANDROID__) || defined(ANDROID)
+#   define ANDROID_DUMMY (31415926)
+#   define EGL_DUMMY ANDROID_DUMMY
+#else
+#   define EGL_DUMMY (31415926)
+#endif
 
 /*******************************************************************************
 ** Display. ********************************************************************
@@ -408,51 +212,7 @@ gcoOS_GetDisplayInfo(
     OUT gctINT * BitsPerPixel
     );
 
-/* VFK_DISPLAY_INFO structure defining information returned by
-   vdkGetDisplayInfoEx. */
-typedef struct _halDISPLAY_INFO
-{
-    /* The size of the display in pixels. */
-    gctINT                         width;
-    gctINT                         height;
-
-    /* The stride of the dispay. -1 is returned if the stride is not known
-    ** for the specified display.*/
-    gctINT                         stride;
-
-    /* The color depth of the display in bits per pixel. */
-    gctINT                         bitsPerPixel;
 
-    /* The logical pointer to the display memory buffer. NULL is returned
-    ** if the pointer is not known for the specified display. */
-    gctPOINTER                      logical;
-
-    /* The physical address of the display memory buffer. ~0 is returned
-    ** if the address is not known for the specified display. */
-    gctSIZE_T               physical;
-
-    gctBOOL                wrapFB;   /* true if compositor, false otherwise. */
-
-#ifndef __QNXNTO__
-    /* 355_FB_MULTI_BUFFER */
-    gctINT                      multiBuffer;
-    gctINT                      backBufferY;
-#endif
-
-    /* The color info of the display. */
-    gctUINT                alphaLength;
-    gctUINT                alphaOffset;
-    gctUINT                redLength;
-    gctUINT                redOffset;
-    gctUINT                greenLength;
-    gctUINT                greenOffset;
-    gctUINT                blueLength;
-    gctUINT                blueOffset;
-
-    /* Display flip support. */
-    gctINT                         flip;
-}
-halDISPLAY_INFO;
 
 gceSTATUS
 gcoOS_GetDisplayInfoEx(
@@ -498,6 +258,19 @@ gcoOS_SetDisplayVirtual(
     );
 
 gceSTATUS
+gcoOS_SetSwapInterval(
+	IN HALNativeDisplayType Display,
+	IN gctINT Interval
+);
+
+gceSTATUS
+gcoOS_GetSwapInterval(
+	IN HALNativeDisplayType Display,
+	IN gctINT_PTR Min,
+	IN gctINT_PTR Max
+);
+
+gceSTATUS
 gcoOS_DisplayBufferRegions(
     IN HALNativeDisplayType Display,
     IN HALNativeWindowType Window,
@@ -779,7 +552,48 @@ gcoOS_DestroyClientBuffer(
     IN gctPOINTER ClientBuffer
     );
 
+gceSTATUS
+gcoOS_DestroyContext(
+    IN gctPOINTER Display,
+    IN gctPOINTER Context
+    );
 
+gceSTATUS
+gcoOS_CreateContext(
+    IN gctPOINTER LocalDisplay,
+    IN gctPOINTER Context
+    );
+
+gceSTATUS
+gcoOS_MakeCurrent(
+    IN gctPOINTER LocalDisplay,
+    IN HALNativeWindowType DrawDrawable,
+    IN HALNativeWindowType ReadDrawable,
+    IN gctPOINTER Context,
+    IN gcoSURF ResolveTarget
+    );
+
+gceSTATUS
+gcoOS_CreateDrawable(
+    IN gctPOINTER LocalDisplay,
+    IN HALNativeWindowType Drawable
+    );
+
+gceSTATUS
+gcoOS_DestroyDrawable(
+    IN gctPOINTER LocalDisplay,
+    IN HALNativeWindowType Drawable
+    );
+gceSTATUS
+gcoOS_SwapBuffers(
+    IN gctPOINTER LocalDisplay,
+    IN HALNativeWindowType Drawable,
+    IN gcoSURF RenderTarget,
+    IN gcoSURF ResolveTarget,
+    IN gctPOINTER ResolveBits,
+    OUT gctUINT *Width,
+    OUT gctUINT *Height
+    );
 #ifdef __cplusplus
 }
 #endif
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_eglplatform_type.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_eglplatform_type.h
new file mode 100644
index 000000000000..727758f4d39f
--- /dev/null
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_eglplatform_type.h
@@ -0,0 +1,287 @@
+/****************************************************************************
+*
+*    Copyright (C) 2005 - 2012 by Vivante Corp.
+*
+*    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., 675 Mass Ave, Cambridge, MA 02139, USA.
+*
+*****************************************************************************/
+
+
+
+#ifndef __gc_hal_eglplatform_type_h_
+#define __gc_hal_eglplatform_type_h_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*******************************************************************************
+** Events. *********************************************************************
+*/
+
+typedef enum _halEventType
+{
+	/* Keyboard event. */
+    HAL_KEYBOARD,
+
+	/* Mouse move event. */
+    HAL_POINTER,
+
+	/* Mouse button event. */
+    HAL_BUTTON,
+
+	/* Application close event. */
+	HAL_CLOSE,
+
+	/* Application window has been updated. */
+	HAL_WINDOW_UPDATE
+}
+halEventType;
+
+/* Scancodes for keyboard. */
+typedef enum _halKeys
+{
+    HAL_UNKNOWN = -1,
+
+    HAL_BACKSPACE = 0x08,
+    HAL_TAB,
+    HAL_ENTER = 0x0D,
+    HAL_ESCAPE = 0x1B,
+
+    HAL_SPACE = 0x20,
+    HAL_SINGLEQUOTE = 0x27,
+    HAL_PAD_ASTERISK = 0x2A,
+    HAL_COMMA = 0x2C,
+    HAL_HYPHEN,
+    HAL_PERIOD,
+    HAL_SLASH,
+    HAL_0,
+    HAL_1,
+    HAL_2,
+    HAL_3,
+    HAL_4,
+    HAL_5,
+    HAL_6,
+    HAL_7,
+    HAL_8,
+    HAL_9,
+    HAL_SEMICOLON = 0x3B,
+    HAL_EQUAL = 0x3D,
+    HAL_A = 0x41,
+    HAL_B,
+    HAL_C,
+    HAL_D,
+    HAL_E,
+    HAL_F,
+    HAL_G,
+    HAL_H,
+    HAL_I,
+    HAL_J,
+    HAL_K,
+    HAL_L,
+    HAL_M,
+    HAL_N,
+    HAL_O,
+    HAL_P,
+    HAL_Q,
+    HAL_R,
+    HAL_S,
+    HAL_T,
+    HAL_U,
+    HAL_V,
+    HAL_W,
+    HAL_X,
+    HAL_Y,
+    HAL_Z,
+    HAL_LBRACKET,
+    HAL_BACKSLASH,
+    HAL_RBRACKET,
+    HAL_BACKQUOTE = 0x60,
+
+    HAL_F1 = 0x80,
+    HAL_F2,
+    HAL_F3,
+    HAL_F4,
+    HAL_F5,
+    HAL_F6,
+    HAL_F7,
+    HAL_F8,
+    HAL_F9,
+    HAL_F10,
+    HAL_F11,
+    HAL_F12,
+
+    HAL_LCTRL,
+    HAL_RCTRL,
+    HAL_LSHIFT,
+    HAL_RSHIFT,
+    HAL_LALT,
+    HAL_RALT,
+    HAL_CAPSLOCK,
+    HAL_NUMLOCK,
+    HAL_SCROLLLOCK,
+    HAL_PAD_0,
+    HAL_PAD_1,
+    HAL_PAD_2,
+    HAL_PAD_3,
+    HAL_PAD_4,
+    HAL_PAD_5,
+    HAL_PAD_6,
+    HAL_PAD_7,
+    HAL_PAD_8,
+    HAL_PAD_9,
+    HAL_PAD_HYPHEN,
+    HAL_PAD_PLUS,
+    HAL_PAD_SLASH,
+    HAL_PAD_PERIOD,
+    HAL_PAD_ENTER,
+    HAL_SYSRQ,
+    HAL_PRNTSCRN,
+    HAL_BREAK,
+    HAL_UP,
+    HAL_LEFT,
+    HAL_RIGHT,
+    HAL_DOWN,
+    HAL_HOME,
+    HAL_END,
+    HAL_PGUP,
+    HAL_PGDN,
+    HAL_INSERT,
+    HAL_DELETE,
+    HAL_LWINDOW,
+    HAL_RWINDOW,
+    HAL_MENU,
+    HAL_POWER,
+    HAL_SLEEP,
+    HAL_WAKE
+}
+halKeys;
+
+/* Structure that defined keyboard mapping. */
+typedef struct _halKeyMap
+{
+	/* Normal key. */
+    halKeys normal;
+
+	/* Extended key. */
+    halKeys extended;
+}
+halKeyMap;
+
+/* Event structure. */
+typedef struct _halEvent
+{
+	/* Event type. */
+    halEventType type;
+
+	/* Event data union. */
+    union _halEventData
+    {
+		/* Event data for keyboard. */
+        struct _halKeyboard
+        {
+			/* Scancode. */
+            halKeys	scancode;
+
+			/* ASCII characte of the key pressed. */
+            char	key;
+
+			/* Flag whether the key was pressed (1) or released (0). */
+            char	pressed;
+        }
+        keyboard;
+
+		/* Event data for pointer. */
+        struct _halPointer
+        {
+			/* Current pointer coordinate. */
+            int		x;
+            int		y;
+        }
+        pointer;
+
+		/* Event data for mouse buttons. */
+        struct _halButton
+        {
+			/* Left button state. */
+            int		left;
+
+			/* Middle button state. */
+            int		middle;
+
+			/* Right button state. */
+            int		right;
+
+			/* Current pointer coordinate. */
+			int		x;
+			int		y;
+        }
+        button;
+    }
+    data;
+}
+halEvent;
+
+/* VFK_DISPLAY_INFO structure defining information returned by
+   vdkGetDisplayInfoEx. */
+typedef struct _halDISPLAY_INFO
+{
+    /* The size of the display in pixels. */
+    int                         width;
+    int                         height;
+
+    /* The stride of the dispay. -1 is returned if the stride is not known
+    ** for the specified display.*/
+    int                         stride;
+
+    /* The color depth of the display in bits per pixel. */
+    int                         bitsPerPixel;
+
+    /* The logical pointer to the display memory buffer. NULL is returned
+    ** if the pointer is not known for the specified display. */
+    void *                      logical;
+
+    /* The physical address of the display memory buffer. ~0 is returned
+    ** if the address is not known for the specified display. */
+    unsigned long               physical;
+
+    int                wrapFB;   /* true if compositor, false otherwise. */
+
+#ifndef __QNXNTO__
+    /* 355_FB_MULTI_BUFFER */
+    int                      multiBuffer;
+    int                      backBufferY;
+#endif
+
+    /* The color info of the display. */
+    unsigned int                alphaLength;
+    unsigned int                alphaOffset;
+    unsigned int                redLength;
+    unsigned int                redOffset;
+    unsigned int                greenLength;
+    unsigned int                greenOffset;
+    unsigned int                blueLength;
+    unsigned int                blueOffset;
+
+    /* Display flip support. */
+    int                         flip;
+}
+halDISPLAY_INFO;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __gc_hal_eglplatform_type_h_ */
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_engine.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_engine.h
index c7543072f349..7400b3201b91 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_engine.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_engine.h
@@ -871,6 +871,19 @@ gco3D_SetAllEarlyDepthModes(
     IN gctBOOL Disable
     );
 
+/* Switch dynamic early mode */
+gceSTATUS
+gco3D_SwitchDynamicEarlyDepthMode(
+    IN gco3D Engine
+    );
+
+/* Set dynamic early mode */
+gceSTATUS
+gco3D_DisableDynamicEarlyDepthMode(
+    IN gco3D Engine,
+    IN gctBOOL Disable
+    );
+
 /* Enable or disable depth-only mode. */
 gceSTATUS
 gco3D_SetDepthOnly(
@@ -1450,6 +1463,21 @@ gcoTEXTURE_UploadCompressed(
     IN gctSIZE_T Bytes
     );
 
+/* Upload compressed sub data to an gcoTEXTURE object. */
+gceSTATUS
+gcoTEXTURE_UploadCompressedSub(
+    IN gcoTEXTURE Texture,
+    IN gctUINT MipMap,
+    IN gceTEXTURE_FACE Face,
+    IN gctUINT XOffset,
+    IN gctUINT YOffset,
+    IN gctUINT Width,
+    IN gctUINT Height,
+    IN gctUINT Slice,
+    IN gctCONST_POINTER Memory,
+    IN gctSIZE_T Size
+    );
+
 /* Get gcoSURF object for a mipmap level. */
 gceSTATUS
 gcoTEXTURE_GetMipMap(
@@ -1780,6 +1808,16 @@ gcoVERTEXARRAY_Bind(
     IN OUT gctUINT * PrimitiveCount
     );
 
+gctUINT
+gcoVERTEXARRAY_GetMaxStream(
+    IN gcoVERTEXARRAY Vertex
+);
+
+gceSTATUS
+gcoVERTEXARRAY_SetMaxStream(
+    IN gcoVERTEXARRAY Vertex,
+    gctUINT maxStreams
+);
 /*******************************************************************************
 ***** Composition *************************************************************/
 
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_enum.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_enum.h
index b2e36a258da8..d951c3a965d9 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_enum.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_enum.h
@@ -209,10 +209,14 @@ typedef enum _gceSURF_TYPE
 #if gcdANDROID_UNALIGNED_LINEAR_COMPOSITION_ADJUST
     gcvSURF_FLIP           = 0x800, /* The Resolve Target the will been flip resolve from RT */
 #endif
+    gcvSURF_TILE_STATUS_DIRTY  = 0x1000, /* Init tile status to all dirty */
 
     gcvSURF_RENDER_TARGET_NO_TILE_STATUS = gcvSURF_RENDER_TARGET
                                          | gcvSURF_NO_TILE_STATUS,
 
+    gcvSURF_RENDER_TARGET_TS_DIRTY = gcvSURF_RENDER_TARGET
+                                         | gcvSURF_TILE_STATUS_DIRTY,
+
     gcvSURF_DEPTH_NO_TILE_STATUS         = gcvSURF_DEPTH
                                          | gcvSURF_NO_TILE_STATUS,
 
@@ -646,6 +650,12 @@ typedef enum _gce2D_QUERY
 }
 gce2D_QUERY;
 
+typedef enum _gce2D_STATE
+{
+    gcv2D_STATE_SPECIAL_FILTER_MIRROR_MODE       = 1,
+}
+gce2D_STATE;
+
 #ifndef VIVANTE_NO_3D
 /* Texture functions. */
 typedef enum _gceTEXTURE_FUNCTION
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_options.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_options.h
index d1ddd004dc3c..33fddfbbfed2 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_options.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_options.h
@@ -161,6 +161,14 @@
 #endif
 
 /*
+    gcdENABLE_FSCALE_VAL_ADJUST
+        When non-zero, FSCALE_VAL when gcvPOWER_ON can be adjusted externally.
+ */
+#ifndef gcdENABLE_FSCALE_VAL_ADJUST
+#   define gcdENABLE_FSCALE_VAL_ADJUST          1
+#endif
+
+/*
     gcdDUMP_IN_KERNEL
 
         When set to 1, all dumps will happen in the kernel.  This is handy if
@@ -765,7 +773,7 @@
         limited by gcdCONTIGUOUS_SIZE_LIMIT.
  */
 #ifndef gcdCONTIGUOUS_SIZE_LIMIT
-#   define gcdCONTIGUOUS_SIZE_LIMIT             0
+#   define gcdCONTIGUOUS_SIZE_LIMIT             4096
 #endif
 
 /*  gcdALPHA_KILL_IN_SHADER
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_raster.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_raster.h
index 53e9cc6e760b..e3ceadfce7ee 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_raster.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_raster.h
@@ -950,6 +950,13 @@ gco2D_QueryU32(
     OUT gctUINT32_PTR Value
     );
 
+gceSTATUS
+gco2D_SetStateU32(
+    IN gco2D Engine,
+    IN gce2D_STATE State,
+    IN OUT gctUINT32_PTR Value
+    );
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_statistics.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_statistics.h
new file mode 100644
index 000000000000..1e85458625e7
--- /dev/null
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_statistics.h
@@ -0,0 +1,116 @@
+/****************************************************************************
+*
+*    Copyright (C) 2005 - 2012 by Vivante Corp.
+*
+*    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., 675 Mass Ave, Cambridge, MA 02139, USA.
+*
+*****************************************************************************/
+
+
+
+#ifndef __gc_hal_statistics_h_
+#define __gc_hal_statistics_h_
+
+
+#define VIV_STAT_ENABLE_STATISTICS              0
+
+/*  Toal number of frames for which the frame time is accounted. We have storage
+    to keep frame times for last this many frames.
+*/
+#define VIV_STAT_FRAME_BUFFER_SIZE              30
+
+/*
+    Total number of frames sampled for a mode. This means
+
+    # of frames for HZ Current  : VIV_STAT_EARLY_Z_SAMPLE_FRAMES
+    # of frames for HZ Switched : VIV_STAT_EARLY_Z_SAMPLE_FRAMES
+  +
+  --------------------------------------------------------
+                                : (2 * VIV_STAT_EARLY_Z_SAMPLE_FRAMES) frames needed
+
+    IMPORTANT: This total must be smaller than VIV_STAT_FRAME_BUFFER_SIZE
+*/
+#define VIV_STAT_EARLY_Z_SAMPLE_FRAMES          7
+#define VIV_STAT_EARLY_Z_LATENCY_FRAMES         2
+
+/* Multiplication factor for previous Hz off mode. Make it more than 1.0 to advertise HZ on.*/
+#define VIV_STAT_EARLY_Z_FACTOR                 (1.05f)
+
+/* Defines the statistical data keys monitored by the statistics module */
+typedef enum _gceSTATISTICS
+{
+    gcvFRAME_FPS        =   1,
+}
+gceSTATISTICS;
+
+/* HAL statistics information. */
+typedef struct _gcsSTATISTICS_EARLYZ
+{
+    gctUINT                     switchBackCount;
+    gctUINT                     nextCheckPoint;
+    gctBOOL                     disabled;
+}
+gcsSTATISTICS_EARLYZ;
+
+
+/* Defines the statistical data keys monitored by the statistics module */
+typedef enum _gceSTATISTICS_Call
+{
+	gcvSTAT_ES11_GLDRAWELEMENTS      =   1,
+}
+gceSTATISTICS_Call;
+
+
+/* HAL statistics information. */
+typedef struct _gcsSTATISTICS
+{
+    gctUINT64                   frameTime[VIV_STAT_FRAME_BUFFER_SIZE];
+    gctUINT64                   previousFrameTime;
+    gctUINT                     frame;
+    gcsSTATISTICS_EARLYZ        earlyZ;
+	gctUINT						ES11_drawElementsCount;
+	gctBOOL						applyRTestVAFix;
+}
+gcsSTATISTICS;
+
+
+/* Add a frame based data into current statistics. */
+void
+gcfSTATISTICS_AddData(
+    IN gceSTATISTICS Key,
+    IN gctUINT Value
+    );
+
+/* Marks the frame end and triggers statistical calculations and decisions.*/
+void
+gcfSTATISTICS_MarkFrameEnd (
+    void
+    );
+
+/* Sets whether the dynmaic HZ is disabled or not .*/
+void
+gcfSTATISTICS_DisableDynamicEarlyZ (
+    IN gctBOOL Disabled
+    );
+
+/* Checks whether or not glDrawArray function call will be discarded */
+gctBOOL
+gcfSTATISTICS_DiscardCall(
+	gceSTATISTICS_Call Function
+	);
+
+
+#endif /*__gc_hal_statistics_h_ */
+
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_types.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_types.h
index e9ad6f611820..4e063776cd44 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_types.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_types.h
@@ -285,6 +285,8 @@ typedef enum _gceSTATUS
 	gcvSTATUS_EXECUTED				=	18,
     gcvSTATUS_TERMINATE             =   19,
 
+    gcvSTATUS_CONVERT_TO_SINGLE_STREAM    =   20,
+
     gcvSTATUS_INVALID_ARGUMENT      =   -1,
     gcvSTATUS_INVALID_OBJECT        =   -2,
     gcvSTATUS_OUT_OF_MEMORY         =   -3,
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_vg.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_vg.h
index 372c8ac7817f..87311376e640 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_vg.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_vg.h
@@ -895,6 +895,12 @@ gckVGMMU_SetPage(
    IN gctUINT32 *PageEntry
    );
 
+/* Flush MMU */
+gceSTATUS
+gckVGMMU_Flush(
+   IN gckVGMMU Mmu
+   );
+
 #endif /* gcdENABLE_VG */
 
 #ifdef __cplusplus
diff --git a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_debugfs.c b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_debugfs.c
index cff2448d7f92..5a21a94ed7fd 100644
--- a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_debugfs.c
+++ b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_debugfs.c
@@ -43,21 +43,21 @@
 #include <linux/completion.h>
 #include "gc_hal_kernel_linux.h"
 
-/* 
+/*
    Prequsite:
 
    1) Debugfs feature must be enabled in the kernel.
        1.a) You can enable this, in the compilation of the uImage, all you have to do is, In the "make menuconfig" part,
        you have to enable the debugfs in the kernel hacking part of the menu.
-  
+
    HOW TO USE:
-   1) insert the driver with the following option logFileSize, Ex: insmod galcore.ko ...... logFileSize=10240 
+   1) insert the driver with the following option logFileSize, Ex: insmod galcore.ko ...... logFileSize=10240
    This gives a circular buffer of 10 MB
-   
+
    2)Usually after inserting the driver, the debug file system is mounted under /sys/kernel/debug/
 
         2.a)If the debugfs is not mounted, you must do "mount -t debugfs none /sys/kernel/debug"
-   	
+
    3) To read what is being printed in the debugfs file system:
         Ex : cat /sys/kernel/debug/gpu/galcore_trace
 
@@ -81,7 +81,7 @@
     4) insmod it with the logFileSize option
     5) Run an application
     6) You can get the dump by cat /sys/kernel/debug/gpu/galcore_trace
-   
+
  */
 
 /**/
@@ -151,7 +151,7 @@ static gcsDebugFileSystem gc_dbgfs ;
 /*******************************************************************************
  **
  **		READ & WRITE FUNCTIONS (START)
- **		  
+ **
  *******************************************************************************/
 
 /*******************************************************************************
@@ -159,9 +159,9 @@ static gcsDebugFileSystem gc_dbgfs ;
  **  _ReadFromNode
  **
  **	1) reading bytes out of a circular buffer with wraparound.
- **	2)returns caddr_t, pointer to data read, which the caller must free.  
+ **	2)returns caddr_t, pointer to data read, which the caller must free.
  **	3) length is (a pointer to) the number of bytes to be read, which will be set by this function to
- **	    be the number of bytes actually returned 
+ **	    be the number of bytes actually returned
  **
  *******************************************************************************/
 static caddr_t
@@ -222,7 +222,7 @@ _ReadFromNode (
  **
  **  _WriteToNode
  **
- ** 1) writes to a circular buffer with wraparound. 
+ ** 1) writes to a circular buffer with wraparound.
  ** 2)in case of an overflow, it overwrites the oldest unread data.
  **
  *********************************************************************************/
@@ -275,7 +275,7 @@ _WriteToNode (
 /*******************************************************************************
  **
  ** 		PRINTING UTILITY (START)
- ** 		
+ **
  *******************************************************************************/
 
 /*******************************************************************************
@@ -369,7 +369,7 @@ _DebugFSPrint (
 /*******************************************************************************
  **
  **                     LINUX SYSTEM FUNCTIONS (START)
- **		  
+ **
  *******************************************************************************/
 
 /*******************************************************************************
@@ -538,7 +538,7 @@ static const struct file_operations debugfs_operations = {
 /*******************************************************************************
  **
  **                             INTERFACE FUNCTIONS (START)
- **		  
+ **
  *******************************************************************************/
 
 /*******************************************************************************
diff --git a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_debugfs.h b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_debugfs.h
index b0031a83ef41..e0b61424482f 100644
--- a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_debugfs.h
+++ b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_debugfs.h
@@ -35,7 +35,7 @@
 /*******************************************************************************
  **
  **                             System Related
- **		  
+ **
  *******************************************************************************/
 
 gctINT    gckDebugFileSystemIsEnabled(void);
@@ -48,7 +48,7 @@ gctINT   gckDebugFileSystemTerminate(void);
 /*******************************************************************************
  **
  **                             Node Related
- **		  
+ **
  *******************************************************************************/
 
 gctINT gckDebugFileSystemCreateNode(
diff --git a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_device.c b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_device.c
index 890d13e17c50..eba81b64be92 100644
--- a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_device.c
+++ b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_device.c
@@ -358,6 +358,15 @@ gckGALDEVICE_Construct(
 	 	gckDebugFileSystemSetCurrentNode(device->dbgnode);
 	}
     }
+    /*get gpu regulator*/
+    device->gpu_regulator = regulator_get(NULL, "cpu_vddgpu");
+    if (IS_ERR(device->gpu_regulator)) {
+	gcmkTRACE_ZONE(gcvLEVEL_ERROR, gcvZONE_DRIVER,
+		"%s(%d): Failed to get gpu regulator  %s/%s \n",
+		__FUNCTION__, __LINE__,
+		PARENT_FILE, DEBUG_FILE);
+	gcmkONERROR(gcvSTATUS_NOT_FOUND);
+    }
 
     /*Initialize the clock structure*/
     if (IrqLine != -1) {
diff --git a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_device.h b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_device.h
index 03f7f9b2201e..c15989894600 100644
--- a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_device.h
+++ b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_device.h
@@ -89,6 +89,9 @@ typedef struct _gckGALDEVICE
     struct clk         *clk_2d_axi;
     struct clk         *clk_vg_axi;
 
+    /*Power management.*/
+    struct regulator      *gpu_regulator;
+
 }
 * gckGALDEVICE;
 
diff --git a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_driver.c b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_driver.c
index 3f873f09d1a0..b326463b8133 100644
--- a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_driver.c
+++ b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_driver.c
@@ -495,8 +495,7 @@ long drv_ioctl(
         }
 
         iface.u.ChipInfo.count = count;
-        status = gcvSTATUS_OK;
-        iface.status = status;
+        iface.status = status = gcvSTATUS_OK;
     }
     else
     {
diff --git a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_linux.h b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_linux.h
index 30fb13596dda..a6ed03ffc0f9 100644
--- a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_linux.h
+++ b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_linux.h
@@ -47,6 +47,7 @@
 
 #if ENABLE_GPU_CLOCK_BY_DRIVER && LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
 #include <linux/clk.h>
+#include <linux/regulator/consumer.h>
 #endif
 
 #define NTSTRSAFE_NO_CCH_FUNCTIONS
diff --git a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_os.c b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_os.c
index 21e6a22317e5..a44180bfc71a 100644
--- a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_os.c
+++ b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_os.c
@@ -38,6 +38,7 @@
 #if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,23)
 #include <linux/math64.h>
 #endif
+#include <linux/delay.h>
 
 #define _GC_OBJ_ZONE    gcvZONE_OS
 
@@ -197,6 +198,8 @@ typedef struct _gcsSIGNAL
     /* The owner of the signal. */
     gctHANDLE process;
 
+    gckHARDWARE hardware;
+
     /* ID. */
     gctUINT32 id;
 }
@@ -1158,32 +1161,24 @@ _UnmapUserLogical(
     IN gctUINT32  Size
 )
 {
-    struct task_struct *task;
-    struct mm_struct *mm;
-
-    /* Get the task_struct of the task with stored pid. */
-    rcu_read_lock();
-
-    task = FIND_TASK_BY_PID(Pid);
-
-    if (task == gcvNULL)
+    if (unlikely(current->mm == gcvNULL))
     {
-        rcu_read_unlock();
+        /* Do nothing if process is exiting. */
         return;
     }
 
-    /* Get the mm_struct. */
-    mm = get_task_mm(task);
-
-    rcu_read_unlock();
-
-    if (mm == gcvNULL)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,5,0)
+    if (vm_munmap((unsigned long)Logical, Size) < 0)
     {
-        return;
+        gcmkTRACE_ZONE(
+                gcvLEVEL_WARNING, gcvZONE_OS,
+                "%s(%d): vm_munmap failed",
+                __FUNCTION__, __LINE__
+                );
     }
-
-    down_write(&mm->mmap_sem);
-    if (do_munmap(mm, (unsigned long)Logical, Size) < 0)
+#else
+    down_write(&current->mm->mmap_sem);
+    if (do_munmap(current->mm, (unsigned long)Logical, Size) < 0)
     {
         gcmkTRACE_ZONE(
                 gcvLEVEL_WARNING, gcvZONE_OS,
@@ -1191,10 +1186,8 @@ _UnmapUserLogical(
                 __FUNCTION__, __LINE__
                 );
     }
-    up_write(&mm->mmap_sem);
-
-    /* Dereference. */
-    mmput(mm);
+    up_write(&current->mm->mmap_sem);
+#endif
 }
 
 /*******************************************************************************
@@ -1983,6 +1976,55 @@ gckOS_UnmapMemoryEx(
 
 /*******************************************************************************
 **
+**  gckOS_UnmapUserLogical
+**
+**  Unmap user logical memory out of physical memory.
+**
+**  INPUT:
+**
+**      gckOS Os
+**          Pointer to an gckOS object.
+**
+**      gctPHYS_ADDR Physical
+**          Start of physical address memory.
+**
+**      gctSIZE_T Bytes
+**          Number of bytes to unmap.
+**
+**      gctPOINTER Memory
+**          Pointer to a previously mapped memory region.
+**
+**  OUTPUT:
+**
+**      Nothing.
+*/
+gceSTATUS
+gckOS_UnmapUserLogical(
+    IN gckOS Os,
+    IN gctPHYS_ADDR Physical,
+    IN gctSIZE_T Bytes,
+    IN gctPOINTER Logical
+    )
+{
+    gcmkHEADER_ARG("Os=0x%X Physical=0x%X Bytes=%lu Logical=0x%X",
+                   Os, Physical, Bytes, Logical);
+
+    /* Verify the arguments. */
+    gcmkVERIFY_OBJECT(Os, gcvOBJ_OS);
+    gcmkVERIFY_ARGUMENT(Physical != 0);
+    gcmkVERIFY_ARGUMENT(Bytes > 0);
+    gcmkVERIFY_ARGUMENT(Logical != gcvNULL);
+
+    gckOS_UnmapMemory(Os, Physical, Bytes, Logical);
+
+    /* Success. */
+    gcmkFOOTER_NO();
+    return gcvSTATUS_OK;
+
+}
+
+/*******************************************************************************
+**
 **  gckOS_AllocateNonPagedMemory
 **
 **  Allocate a number of pages from non-paged memory.
@@ -2377,8 +2419,8 @@ gceSTATUS gckOS_FreeNonPagedMemory(
     {
         if (mdlMap->vmaAddr != gcvNULL)
         {
-            _UnmapUserLogical(mdlMap->pid, mdlMap->vmaAddr, mdl->numPages * PAGE_SIZE);
-            mdlMap->vmaAddr = gcvNULL;
+            /* No mapped memory exists when free nonpaged memory */
+            gcmkASSERT(0);
         }
 
         mdlMap = mdlMap->next;
@@ -3762,24 +3804,14 @@ gckOS_Delay(
 
     if (Delay > 0)
     {
-#if gcdHIGH_PRECISION_DELAY_ENABLE
-        ktime_t wait = ns_to_ktime(Delay * 1000 * 1000);
-        set_current_state(TASK_INTERRUPTIBLE);
-        schedule_hrtimeout(&wait, HRTIMER_MODE_REL);
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 28)
+        ktime_t delay = ktime_set(0, Delay * NSEC_PER_MSEC);
+        __set_current_state(TASK_UNINTERRUPTIBLE);
+        schedule_hrtimeout(&delay, HRTIMER_MODE_REL);
 #else
-        struct timeval now;
-        unsigned long jiffies;
-
-        /* Convert milliseconds into seconds and microseconds. */
-        now.tv_sec  = Delay / 1000;
-        now.tv_usec = (Delay % 1000) * 1000;
-
-        /* Convert timeval to jiffies. */
-        jiffies = timeval_to_jiffies(&now);
-
-        /* Schedule timeout. */
-        schedule_timeout_interruptible(jiffies);
+        msleep(Delay);
 #endif
+
     }
 
     /* Success. */
@@ -5642,7 +5674,11 @@ OnError:
         }
 
 #if gcdENABLE_VG
-        if (Core != gcvCORE_VG)
+        if (Core == gcvCORE_VG)
+        {
+            gcmkONERROR(gckVGMMU_Flush(Os->device->kernels[Core]->vg->mmu));
+        }
+        else
 #endif
         {
             gcmkONERROR(gckMMU_Flush(Os->device->kernels[Core]->mmu));
@@ -6517,7 +6553,9 @@ gckOS_Broadcast(
 
     case gcvBROADCAST_GPU_STUCK:
         gcmkTRACE_N(gcvLEVEL_ERROR, 0, "gcvBROADCAST_GPU_STUCK\n");
+#if !gcdENABLE_RECOVERY
         gcmkONERROR(_DumpGPUState(Os, Hardware->core));
+#endif
         gcmkONERROR(gckKERNEL_Recovery(Hardware->kernel));
         break;
 
@@ -6943,6 +6981,7 @@ gckOS_SetGPUPower(
     struct clk *clk_vg_axi = Os->device->clk_vg_axi;
 
     gctBOOL oldClockState = gcvFALSE;
+    gctBOOL oldPowerState = gcvFALSE;
 
     gcmkHEADER_ARG("Os=0x%X Core=%d Clock=%d Power=%d", Os, Core, Clock, Power);
 
@@ -6952,15 +6991,20 @@ gckOS_SetGPUPower(
         if (Core == gcvCORE_VG)
         {
             oldClockState = Os->device->kernels[Core]->vg->hardware->clockState;
+            oldPowerState = Os->device->kernels[Core]->vg->hardware->powerState;
         }
         else
         {
 #endif
             oldClockState = Os->device->kernels[Core]->hardware->clockState;
+            oldPowerState = Os->device->kernels[Core]->hardware->powerState;
 #if gcdENABLE_VG
         }
 #endif
     }
+	if((Power == gcvTRUE) && (oldPowerState == gcvFALSE) &&
+		!IS_ERR(Os->device->gpu_regulator))
+            regulator_enable(Os->device->gpu_regulator);
 
     if (Clock == gcvTRUE) {
         if (oldClockState == gcvFALSE) {
@@ -7003,8 +7047,10 @@ gckOS_SetGPUPower(
             }
         }
     }
-
-
+	if((Power == gcvFALSE) && (oldPowerState == gcvTRUE) &&
+		!IS_ERR(Os->device->gpu_regulator))
+            regulator_disable(Os->device->gpu_regulator);
+    /* TODO: Put your code here. */
     gcmkFOOTER_NO();
     return gcvSTATUS_OK;
 }
@@ -7195,6 +7241,7 @@ gckOS_CreateSignal(
     /* Save the process ID. */
     signal->process = (gctHANDLE) _GetProcessID();
     signal->manualReset = ManualReset;
+    signal->hardware = gcvNULL;
     init_completion(&signal->obj);
     atomic_set(&signal->ref, 1);
 
@@ -7215,6 +7262,61 @@ OnError:
     return status;
 }
 
+gceSTATUS
+gckOS_SignalQueryHardware(
+    IN gckOS Os,
+    IN gctSIGNAL Signal,
+    OUT gckHARDWARE * Hardware
+    )
+{
+    gceSTATUS status;
+    gcsSIGNAL_PTR signal;
+
+    gcmkHEADER_ARG("Os=0x%X Signal=0x%X Hardware=0x%X", Os, Signal, Hardware);
+
+    /* Verify the arguments. */
+    gcmkVERIFY_OBJECT(Os, gcvOBJ_OS);
+    gcmkVERIFY_ARGUMENT(Signal != gcvNULL);
+    gcmkVERIFY_ARGUMENT(Hardware != gcvNULL);
+
+    gcmkONERROR(_QueryIntegerId(&Os->signalDB, (gctUINT32)Signal, (gctPOINTER)&signal));
+
+    *Hardware = signal->hardware;
+
+    gcmkFOOTER_NO();
+    return gcvSTATUS_OK;
+OnError:
+    gcmkFOOTER();
+    return status;
+}
+
+gceSTATUS
+gckOS_SignalSetHardware(
+    IN gckOS Os,
+    IN gctSIGNAL Signal,
+    IN gckHARDWARE Hardware
+    )
+{
+    gceSTATUS status;
+    gcsSIGNAL_PTR signal;
+
+    gcmkHEADER_ARG("Os=0x%X Signal=0x%X Hardware=0x%X", Os, Signal, Hardware);
+
+    /* Verify the arguments. */
+    gcmkVERIFY_OBJECT(Os, gcvOBJ_OS);
+    gcmkVERIFY_ARGUMENT(Signal != gcvNULL);
+
+    gcmkONERROR(_QueryIntegerId(&Os->signalDB, (gctUINT32)Signal, (gctPOINTER)&signal));
+
+    signal->hardware = Hardware;
+
+    gcmkFOOTER_NO();
+    return gcvSTATUS_OK;
+OnError:
+    gcmkFOOTER();
+    return status;
+}
+
 /*******************************************************************************
 **
 **  gckOS_DestroySignal
@@ -7330,6 +7432,9 @@ gckOS_Signal(
 
     if (State)
     {
+        /* unbind the signal from hardware. */
+        signal->hardware = gcvNULL;
+
         /* Set the event to a signaled state. */
         complete(&signal->obj);
     }
diff --git a/drivers/mxc/ipu3/ipu_device.c b/drivers/mxc/ipu3/ipu_device.c
index d12ffdfc7847..be1725129ce2 100644
--- a/drivers/mxc/ipu3/ipu_device.c
+++ b/drivers/mxc/ipu3/ipu_device.c
@@ -340,6 +340,7 @@ struct ipu_alloc_list {
 	dma_addr_t phy_addr;
 	void *cpu_addr;
 	u32 size;
+	void *file_index;
 };
 
 static LIST_HEAD(ipu_alloc_list);
@@ -350,6 +351,7 @@ static DEFINE_SPINLOCK(ipu_task_list_lock);
 static DECLARE_WAIT_QUEUE_HEAD(thread_waitq);
 static DECLARE_WAIT_QUEUE_HEAD(res_waitq);
 static atomic_t req_cnt;
+static atomic_t file_index = ATOMIC_INIT(1);
 static int major;
 static int max_ipu_no;
 static int thread_id;
@@ -3274,6 +3276,7 @@ EXPORT_SYMBOL_GPL(ipu_queue_task);
 
 static int mxc_ipu_open(struct inode *inode, struct file *file)
 {
+	file->private_data = (void *)atomic_inc_return(&file_index);
 	return 0;
 }
 
@@ -3330,6 +3333,7 @@ static long mxc_ipu_ioctl(struct file *file,
 				kfree(mem);
 				return -ENOMEM;
 			}
+			mem->file_index = file->private_data;
 			mutex_lock(&ipu_alloc_lock);
 			list_add(&mem->list, &ipu_alloc_list);
 			mutex_unlock(&ipu_alloc_lock);
@@ -3413,6 +3417,26 @@ static int mxc_ipu_mmap(struct file *file, struct vm_area_struct *vma)
 
 static int mxc_ipu_release(struct inode *inode, struct file *file)
 {
+	struct ipu_alloc_list *mem;
+	struct ipu_alloc_list *n;
+
+	mutex_lock(&ipu_alloc_lock);
+	list_for_each_entry_safe(mem, n, &ipu_alloc_list, list) {
+		if ((mem->cpu_addr != 0) &&
+			(file->private_data == mem->file_index)) {
+			list_del(&mem->list);
+			dma_free_coherent(ipu_dev,
+					  mem->size,
+					  mem->cpu_addr,
+					  mem->phy_addr);
+			dev_dbg(ipu_dev, "rel-free %d bytes @ 0x%08X\n",
+				mem->size, mem->phy_addr);
+			kfree(mem);
+		}
+	}
+	mutex_unlock(&ipu_alloc_lock);
+	atomic_dec(&file_index);
+
 	return 0;
 }
 
diff --git a/drivers/mxc/thermal/cooling.c b/drivers/mxc/thermal/cooling.c
index 0feefeaa6008..7019d9949c8a 100644
--- a/drivers/mxc/thermal/cooling.c
+++ b/drivers/mxc/thermal/cooling.c
@@ -57,6 +57,7 @@ cpufreq, it minor 1, and when we promote cpufreq, it add 1, so
 if it is 0, mean we didn't change the cpufreq */
 static int cpufreq_change_count;
 
+extern int thermal_hot;
 int anatop_thermal_get_cpufreq_cur(void)
 {
 	int ret = -EINVAL;
@@ -237,6 +238,7 @@ imx_processor_set_cur_state(struct thermal_cooling_device *cdev,
 	secondary CPUs that detached by thermal driver */
 	if (cooling_cpuhotplug) {
 		if (!state) {
+			thermal_hot = 0;
 			for (i = 1; i < 4; i++) {
 				if (cpu_mask && (0x1 << i)) {
 					anatop_thermal_cpu_hotplug(true);
@@ -247,6 +249,7 @@ imx_processor_set_cur_state(struct thermal_cooling_device *cdev,
 		}
 	} else {
 		if (!state) {
+			thermal_hot = 0;
 			if (cpufreq_change_count < 0)
 				anatop_thermal_cpufreq_up();
 			else if (cpufreq_change_count > 0)
diff --git a/drivers/mxc/vpu/mxc_vpu.c b/drivers/mxc/vpu/mxc_vpu.c
index 92a1d0dc98c7..6d77afb8a3d6 100644
--- a/drivers/mxc/vpu/mxc_vpu.c
+++ b/drivers/mxc/vpu/mxc_vpu.c
@@ -39,6 +39,7 @@
 #include <linux/workqueue.h>
 #include <linux/sched.h>
 #include <linux/vmalloc.h>
+#include <linux/regulator/consumer.h>
 #include <linux/page-flags.h>
 #include <linux/mm_types.h>
 #include <linux/types.h>
@@ -110,6 +111,7 @@ static int vpu_jpu_irq;
 #endif
 
 static unsigned int regBk[64];
+static struct regulator *vpu_regulator;
 
 #define	READ_REG(x)		__raw_readl(vpu_base + x)
 #define	WRITE_REG(val, x)	__raw_writel(val, vpu_base + x)
@@ -245,8 +247,12 @@ bool vpu_is_valid_phy_memory(u32 paddr)
  */
 static int vpu_open(struct inode *inode, struct file *filp)
 {
+
 	mutex_lock(&vpu_data.lock);
-	open_count++;
+
+	if (open_count++ == 0 && !IS_ERR(vpu_regulator))
+		regulator_enable(vpu_regulator);
+
 	filp->private_data = (void *)(&vpu_data);
 	mutex_unlock(&vpu_data.lock);
 	return 0;
@@ -531,6 +537,9 @@ static int vpu_release(struct inode *inode, struct file *filp)
 
 	mutex_lock(&vpu_data.lock);
 	if (open_count > 0 && !(--open_count)) {
+		if (!IS_ERR(vpu_regulator))
+			regulator_disable(vpu_regulator);
+
 		vpu_free_buffers();
 
 		/* Free shared memory when vpu device is idle */
@@ -705,6 +714,12 @@ static int vpu_dev_probe(struct platform_device *pdev)
 			  (void *)(&vpu_data));
 	if (err)
 		goto err_out_class;
+	vpu_regulator = regulator_get(NULL, "cpu_vddvpu");
+	if (IS_ERR(vpu_regulator)) {
+		printk(KERN_ERR
+			"%s: failed to get vpu regulator\n", __func__);
+		goto err_out_class;
+	}
 
 #ifdef MXC_VPU_HAS_JPU
 	vpu_jpu_irq = platform_get_irq_byname(pdev, "vpu_jpu_irq");
@@ -752,7 +767,8 @@ static int vpu_dev_remove(struct platform_device *pdev)
 	iounmap(vpu_base);
 	if (vpu_plat && vpu_plat->iram_enable && vpu_plat->iram_size)
 		iram_free(iram.start,  vpu_plat->iram_size);
-
+	if (!IS_ERR(vpu_regulator))
+		regulator_put(vpu_regulator);
 	return 0;
 }
 
@@ -762,6 +778,8 @@ static int vpu_suspend(struct platform_device *pdev, pm_message_t state)
 	int i;
 	unsigned long timeout;
 
+	if (!IS_ERR(vpu_regulator))
+		regulator_enable(vpu_regulator);
 	/* Wait for vpu go to idle state, suspect vpu cannot be changed
 	   to idle state after about 1 sec */
 	if (open_count > 0) {
@@ -794,10 +812,21 @@ static int vpu_suspend(struct platform_device *pdev, pm_message_t state)
 	if (vpu_plat->pg)
 		vpu_plat->pg(1);
 
+	/* If VPU is working before suspend, disable
+	 * regulator to make usecount right. */
+	if (open_count > 0) {
+		if (!IS_ERR(vpu_regulator))
+			regulator_disable(vpu_regulator);
+	}
+
+	if (!IS_ERR(vpu_regulator))
+		regulator_disable(vpu_regulator);
 	return 0;
 
 out:
 	clk_disable(vpu_clk);
+	if (!IS_ERR(vpu_regulator))
+		regulator_disable(vpu_regulator);
 	return -EAGAIN;
 
 }
@@ -809,9 +838,18 @@ static int vpu_resume(struct platform_device *pdev)
 	if (cpu_is_mx53())
 		goto recover_clk;
 
+	if (!IS_ERR(vpu_regulator))
+		regulator_enable(vpu_regulator);
 	if (vpu_plat->pg)
 		vpu_plat->pg(0);
 
+	/* If VPU is working before suspend, enable
+	 * regulator to make usecount right. */
+	if (open_count > 0) {
+		if (!IS_ERR(vpu_regulator))
+			regulator_enable(vpu_regulator);
+	}
+
 	if (bitwork_mem.cpu_addr != 0) {
 		u32 *p = (u32 *) bitwork_mem.cpu_addr;
 		u32 data, pc;
@@ -869,6 +907,8 @@ recover_clk:
 	/* Recover vpu clock */
 	for (i = 0; i < vpu_clk_usercount; i++)
 		clk_enable(vpu_clk);
+	if (!IS_ERR(vpu_regulator))
+		regulator_disable(vpu_regulator);
 
 	return 0;
 }
diff --git a/drivers/net/fec.c b/drivers/net/fec.c
index 5af378f391ee..86e4ea3404b3 100755
--- a/drivers/net/fec.c
+++ b/drivers/net/fec.c
@@ -397,7 +397,12 @@ fec_timeout(struct net_device *ndev)
 
 	ndev->stats.tx_errors++;
 
+	netif_device_detach(ndev);
+	fec_stop(ndev);
+
 	fec_restart(ndev, fep->full_duplex);
+	netif_device_attach(ndev);
+	ndev->trans_start = jiffies; /* prevent tx timeout */
 	if (fep->link && !fep->tx_full)
 		netif_wake_queue(ndev);
 }
@@ -1320,10 +1325,7 @@ fec_enet_close(struct net_device *ndev)
 {
 	struct fec_enet_private *fep = netdev_priv(ndev);
 
-	/* Don't know what to do yet. */
 	fep->opened = 0;
-	netif_stop_queue(ndev);
-	netif_carrier_off(ndev);
 	if (fep->use_napi)
 		napi_disable(&fep->napi);
 
@@ -1760,7 +1762,10 @@ fec_stop(struct net_device *dev)
 	if (fep->ptimer_present)
 		fec_ptp_stop(fep->ptp_priv);
 	writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK);
-	netif_stop_queue(dev);
+
+	if (netif_running(dev))
+		netif_stop_queue(dev);
+	netif_carrier_off(dev);     /* prevent tx timeout */
 	fep->link = 0;
 }
 
@@ -1921,7 +1926,6 @@ fec_suspend(struct device *dev)
 	if (netif_running(ndev)) {
 		netif_device_detach(ndev);
 		fec_stop(ndev);
-		netif_carrier_off(ndev);
 		clk_disable(fep->clk);
 	}
 
diff --git a/drivers/power/Kconfig b/drivers/power/Kconfig
index 6e53cce3c7fd..a24653787d1d 100755
--- a/drivers/power/Kconfig
+++ b/drivers/power/Kconfig
@@ -222,7 +222,6 @@ config CHARGER_MAX8903
 config SABRESD_MAX8903
         tristate "Sabresd Board Battery DC-DC Charger for USB and Adapter Power"
         depends on GENERIC_HARDIRQS
-	depends on TOUCHSCREEN_MAX11801
         help
           Say Y to enable support for the MAX8903 DC-DC charger and sysfs on
 	  sabresd board.The driver supports controlling charger and battery
diff --git a/drivers/power/sabresd_battery.c b/drivers/power/sabresd_battery.c
index c7318a6eb02c..648922139790 100755
--- a/drivers/power/sabresd_battery.c
+++ b/drivers/power/sabresd_battery.c
@@ -65,13 +65,11 @@ typedef struct {
 	u32 voltage;
 	u32 percent;
 } battery_capacity , *pbattery_capacity;
-
 static int cpu_type_flag;
 static int offset_discharger;
 static int offset_charger;
 static int offset_usb_charger;
 
-
 static battery_capacity chargingTable[] = {
 	{4050,	99},
 	{4040,	98},
@@ -188,9 +186,9 @@ static enum power_supply_property max8903_battery_props[] = {
 	POWER_SUPPLY_PROP_HEALTH,
 	POWER_SUPPLY_PROP_CAPACITY_LEVEL,
 };
-
+#ifdef CONFIG_TOUCHSCREEN_MAX11801
 extern u32 max11801_read_adc(void);
-
+#endif
 static void max8903_charger_update_status(struct max8903_data *data)
 {
 	if (data->usb_in || data->ta_in) {
@@ -205,26 +203,35 @@ static void max8903_charger_update_status(struct max8903_data *data)
 		  }
 	if (data->charger_online == 0 && data->usb_charger_online == 0) {
 			data->battery_status = POWER_SUPPLY_STATUS_DISCHARGING;
-		} else {
-					if (gpio_get_value(data->pdata->chg) == 0) {
-						data->battery_status = POWER_SUPPLY_STATUS_CHARGING;
-					} else if (data->ta_in &&
-						gpio_get_value(data->pdata->chg) == 1) {
-						if (data->percent >= 99)
-							data->battery_status = POWER_SUPPLY_STATUS_FULL;
-						else
-							data->battery_status = POWER_SUPPLY_STATUS_NOT_CHARGING;
-						}
-					  else if (data->usb_in &&
-						gpio_get_value(data->pdata->chg) == 1) {
-						if (data->percent >= 99)
-							data->battery_status = POWER_SUPPLY_STATUS_FULL;
-						else
-							data->battery_status = POWER_SUPPLY_STATUS_NOT_CHARGING;
-						}
+	} else {
+		if (gpio_get_value(data->pdata->chg) == 0) {
+			data->battery_status = POWER_SUPPLY_STATUS_CHARGING;
+		} else if (data->ta_in && gpio_get_value(data->pdata->chg) == 1) {
+		  if (!data->pdata->feature_flag) {
+			if (data->percent >= 99)
+				data->battery_status = POWER_SUPPLY_STATUS_FULL;
+			else
+				data->battery_status = POWER_SUPPLY_STATUS_NOT_CHARGING;
+			} else {
+				data->battery_status = POWER_SUPPLY_STATUS_FULL;
+			}
+		} else if (data->usb_in && gpio_get_value(data->pdata->chg) == 1) {
+		  if (!data->pdata->feature_flag) {
+			if (data->percent >= 99)
+				data->battery_status = POWER_SUPPLY_STATUS_FULL;
+			else
+				data->battery_status = POWER_SUPPLY_STATUS_NOT_CHARGING;
+			} else {
+				data->battery_status = POWER_SUPPLY_STATUS_FULL;
+			}
 		}
-	pr_debug("chg: %d \n", gpio_get_value(data->pdata->chg));
+	}
+	pr_debug("chg: %d\n", gpio_get_value(data->pdata->chg));
+	pr_debug("dc: %d\n", gpio_get_value(data->pdata->dok));
+	pr_debug("flt: %d\n", gpio_get_value(data->pdata->flt));
 }
+
+#ifdef CONFIG_TOUCHSCREEN_MAX11801
 static int cmp_func(const void *_a, const void *_b)
 {
 	const int *a = _a, *b = _b;
@@ -235,6 +242,7 @@ static int cmp_func(const void *_a, const void *_b)
 		return -1;
 	return 0;
 }
+
 u32 calibration_voltage(struct max8903_data *data)
 {
 	int volt[ADC_SAMPLE_COUNT];
@@ -275,51 +283,54 @@ u32 calibration_voltage(struct max8903_data *data)
 	voltage_data = (volt[2] + volt[ADC_SAMPLE_COUNT - 3]) / 2;
 	return voltage_data;
 }
-
+#endif
 static void max8903_battery_update_status(struct max8903_data *data)
 {
-	int temp;
+	int temp = 0;
 	static int temp_last;
 	bool changed_flag;
 	changed_flag = false;
 	mutex_lock(&data->work_lock);
-	temp = calibration_voltage(data);
-	if (temp_last == 0) {
-		data->voltage_uV = temp;
-		temp_last = temp;
-	}
-	if (data->charger_online == 0 && temp_last != 0) {
-		if (temp < temp_last) {
-		temp_last = temp;
-		data->voltage_uV = temp;
-		} else {
-		data->voltage_uV = temp_last;
+	if (!data->pdata->feature_flag) {
+#ifdef CONFIG_TOUCHSCREEN_MAX11801
+		temp = calibration_voltage(data);
+#endif
+		if (temp_last == 0) {
+			data->voltage_uV = temp;
+			temp_last = temp;
+		}
+		if (data->charger_online == 0 && temp_last != 0) {
+			if (temp < temp_last) {
+				temp_last = temp;
+				data->voltage_uV = temp;
+			} else {
+			data->voltage_uV = temp_last;
+			}
+		}
+		if (data->charger_online == 1 || data->usb_charger_online == 1) {
+			data->voltage_uV = temp;
+			temp_last = temp;
+		}
+		data->percent = calibrate_battery_capability_percent(data);
+		if (data->percent != data->old_percent) {
+			data->old_percent = data->percent;
+			changed_flag = true;
+		}
+		if (changed_flag) {
+			changed_flag = false;
+			power_supply_changed(&data->bat);
+		}
+		 /*
+		  *because boot time gap between led framwork and charger
+		  *framwork,when system boots with charger attatched,
+		  *charger led framwork loses the first charger online event,
+		  *add once extra power_supply_changed can fix this issure
+		  */
+		if (data->first_delay_count < 200) {
+			data->first_delay_count = data->first_delay_count + 1 ;
+			power_supply_changed(&data->bat);
 		}
 	}
-	if (data->charger_online == 1 || data->usb_charger_online == 1) {
-		data->voltage_uV = temp;
-		temp_last = temp;
-	}
-	data->percent = calibrate_battery_capability_percent(data);
-	if (data->percent != data->old_percent) {
-		data->old_percent = data->percent;
-		changed_flag = true;
-	}
-	if (changed_flag) {
-		changed_flag = false;
-		power_supply_changed(&data->bat);
-	}
-	/*
-	    because boot time gap between led framwork and charger
-	    framwork,when system boots with charger attatched, charger
-	    led framwork loses the first charger online event,add once extra
-	    power_supply_changed can fix this issure
-	*/
-	if (data->first_delay_count < 200) {
-		data->first_delay_count = data->first_delay_count + 1 ;
-		power_supply_changed(&data->bat);
-	}
-
 	mutex_unlock(&data->work_lock);
 }
 
@@ -332,22 +343,27 @@ static int max8903_battery_get_property(struct power_supply *bat,
 	switch (psp) {
 	case POWER_SUPPLY_PROP_STATUS:
 		val->intval = POWER_SUPPLY_STATUS_UNKNOWN;
-				if (gpio_get_value(di->pdata->chg) == 0) {
-					di->battery_status = POWER_SUPPLY_STATUS_CHARGING;
-				} else if (di->ta_in &&
-					gpio_get_value(di->pdata->chg) == 1) {
+			if (gpio_get_value(di->pdata->chg) == 0) {
+				di->battery_status = POWER_SUPPLY_STATUS_CHARGING;
+			} else if (di->ta_in && gpio_get_value(di->pdata->chg) == 1) {
+				if (!di->pdata->feature_flag) {
 					if (di->percent >= 99)
 						di->battery_status = POWER_SUPPLY_STATUS_FULL;
 					else
 						di->battery_status = POWER_SUPPLY_STATUS_NOT_CHARGING;
+					} else {
+							di->battery_status = POWER_SUPPLY_STATUS_FULL;
 					}
-				  else if (di->usb_in &&
-					gpio_get_value(di->pdata->chg) == 1) {
+			 } else if (di->usb_in && gpio_get_value(di->pdata->chg) == 1) {
+				if (!di->pdata->feature_flag) {
 					if (di->percent >= 99)
 					    di->battery_status = POWER_SUPPLY_STATUS_FULL;
 					else
-					  di->battery_status = POWER_SUPPLY_STATUS_NOT_CHARGING;
+						di->battery_status = POWER_SUPPLY_STATUS_NOT_CHARGING;
+					} else {
+						di->battery_status = POWER_SUPPLY_STATUS_FULL;
 					}
+			}
 		val->intval = di->battery_status;
 		return 0;
 	default:
@@ -795,14 +811,13 @@ static __devinit int max8903_probe(struct platform_device *pdev)
 		ret = request_threaded_irq(gpio_to_irq(pdata->chg),
 				NULL, max8903_chg,
 				IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
-				"MAX8903 Fault", data);
+				"MAX8903 Status", data);
 		if (ret) {
-			dev_err(dev, "Cannot request irq %d for Fault (%d)\n",
+			dev_err(dev, "Cannot request irq %d for Status (%d)\n",
 					gpio_to_irq(pdata->flt), ret);
 			goto err_chg_irq;
 		}
 	}
-
 	ret = device_create_file(&pdev->dev, &max8903_discharger_dev_attr);
 	if (ret)
 		dev_err(&pdev->dev, "create device file failed!\n");
@@ -822,7 +837,6 @@ static __devinit int max8903_probe(struct platform_device *pdev)
 			offset_charger = 1485;
 			offset_usb_charger = 1285;
 	}
-
 	max8903_charger_update_status(data);
 	max8903_battery_update_status(data);
 	return 0;
@@ -888,10 +902,14 @@ static int max8903_suspend(struct platform_device *pdev,
 	if (data) {
 		struct max8903_pdata *pdata = data->pdata;
 		if (pdata) {
-			irq = gpio_to_irq(pdata->dok);
-			enable_irq_wake(irq);
-			irq = gpio_to_irq(pdata->uok);
-			enable_irq_wake(irq);
+			if (pdata->dc_valid) {
+				irq = gpio_to_irq(pdata->dok);
+				enable_irq_wake(irq);
+			}
+			if (pdata->usb_valid) {
+				irq = gpio_to_irq(pdata->uok);
+				enable_irq_wake(irq);
+				}
 			cancel_delayed_work(&data->work);
 		}
 	}
@@ -923,10 +941,14 @@ static int max8903_resume(struct platform_device *pdev)
 				max8903_charger_update_status(data);
 				power_supply_changed(&data->usb);
 			}
-			irq = gpio_to_irq(pdata->dok);
-			disable_irq_wake(irq);
-			irq = gpio_to_irq(pdata->uok);
-			disable_irq_wake(irq);
+			if (pdata->dc_valid) {
+				irq = gpio_to_irq(pdata->dok);
+				disable_irq_wake(irq);
+			}
+			if (pdata->usb_valid) {
+				irq = gpio_to_irq(pdata->uok);
+				disable_irq_wake(irq);
+			}
 			schedule_delayed_work(&data->work, BATTERY_UPDATE_INTERVAL);
 		}
 	}
diff --git a/drivers/regulator/max17135-regulator.c b/drivers/regulator/max17135-regulator.c
index 45d4501ef7a5..5ea70ef5868f 100644
--- a/drivers/regulator/max17135-regulator.c
+++ b/drivers/regulator/max17135-regulator.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2010-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2010-2012 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
@@ -284,12 +284,14 @@ static int max17135_vcom_is_enabled(struct regulator_dev *reg)
 
 static int max17135_is_power_good(struct max17135 *max17135)
 {
-	/*
-	 * XOR of polarity (starting value) and current
-	 * value yields whether power is good.
-	 */
-	return gpio_get_value(max17135->gpio_pmic_pwrgood) ^
-		max17135->pwrgood_polarity;
+    unsigned int reg_val;
+    unsigned int fld_val;
+
+    max17135_reg_read(REG_MAX17135_FAULT, &reg_val);
+    fld_val = (reg_val & BITFMASK(FAULT_POK)) >> FAULT_POK_LSH;
+
+    /* Check the POK bit */
+    return fld_val;
 }
 
 static int max17135_wait_power_good(struct max17135 *max17135)
@@ -302,6 +304,7 @@ static int max17135_wait_power_good(struct max17135 *max17135)
 
 		msleep(1);
 	}
+
 	return -ETIMEDOUT;
 }
 
@@ -622,9 +625,6 @@ int max17135_register_regulator(struct max17135 *max17135, int reg,
 		 */
 		max17135_setup_timings(max17135);
 
-		max17135->pwrgood_polarity =
-			gpio_get_value(max17135->gpio_pmic_pwrgood);
-
 		max17135->init_done = true;
 	}
 
diff --git a/drivers/tty/serial/imx.c b/drivers/tty/serial/imx.c
index 8a66f3eeb992..5e9594d744d9 100644
--- a/drivers/tty/serial/imx.c
+++ b/drivers/tty/serial/imx.c
@@ -1742,6 +1742,8 @@ static int serial_imx_resume(struct platform_device *dev)
 	return 0;
 }
 
+extern int uart_at_24;
+
 static int serial_imx_probe(struct platform_device *pdev)
 {
 	struct imx_port *sport;
@@ -1788,6 +1790,9 @@ static int serial_imx_probe(struct platform_device *pdev)
 		ret = PTR_ERR(sport->clk);
 		goto unmap;
 	}
+	if (uart_at_24)
+		clk_set_parent(sport->clk, clk_get(NULL, "osc"));
+
 	clk_enable(sport->clk);
 
 	sport->port.uartclk = clk_get_rate(sport->clk);
diff --git a/drivers/usb/gadget/arcotg_udc.c b/drivers/usb/gadget/arcotg_udc.c
index c3a48307c918..08118b3ba61e 100755
--- a/drivers/usb/gadget/arcotg_udc.c
+++ b/drivers/usb/gadget/arcotg_udc.c
@@ -1695,6 +1695,10 @@ static void setup_received_irq(struct fsl_udc *udc,
 				else if (setup->bRequest ==
 					 USB_DEVICE_A_ALT_HNP_SUPPORT)
 					udc->gadget.a_alt_hnp_support = 1;
+				else
+					break;
+			} else {
+				break;
 			}
 			rc = 0;
 		} else
@@ -3065,6 +3069,8 @@ static int __devinit fsl_udc_probe(struct platform_device *pdev)
 		goto err2a;
 	}
 
+	spin_lock_init(&pdata->lock);
+
 	/* Due to mx35/mx25's phy's bug */
 	reset_phy();
 
@@ -3210,7 +3216,6 @@ static int __devinit fsl_udc_probe(struct platform_device *pdev)
 	udc_controller->charger.enable = false;
 #endif
 
-	spin_lock_init(&pdata->lock);
 	return 0;
 
 err4:
@@ -3323,10 +3328,13 @@ static int udc_suspend(struct fsl_udc *udc)
 	 * charge using usb
 	 */
 	if (pdata->pmflags == 0) {
-		if (!udc_can_wakeup_system())
+		if (!udc_can_wakeup_system()) {
 			dr_wake_up_enable(udc, false);
-		else
+		} else {
+			if (pdata->platform_phy_power_on)
+				pdata->platform_phy_power_on();
 			dr_wake_up_enable(udc, true);
+		}
 	}
 
 	/*
diff --git a/drivers/usb/host/ehci-arc.c b/drivers/usb/host/ehci-arc.c
index b872d94b3fc3..35815869bb8d 100755
--- a/drivers/usb/host/ehci-arc.c
+++ b/drivers/usb/host/ehci-arc.c
@@ -260,6 +260,8 @@ int usb_hcd_fsl_probe(const struct hc_driver *driver,
 		goto err4;
 	}
 
+	spin_lock_init(&pdata->lock);
+
 	fsl_platform_set_host_mode(hcd);
 	hcd->power_budget = pdata->power_budget;
 	ehci = hcd_to_ehci(hcd);
@@ -308,7 +310,6 @@ int usb_hcd_fsl_probe(const struct hc_driver *driver,
 
 	ehci = hcd_to_ehci(hcd);
 	pdata->pm_command = ehci->command;
-	spin_lock_init(&pdata->lock);
 	return retval;
 err6:
 	free_irq(irq, (void *)pdev);
@@ -678,6 +679,9 @@ static int ehci_fsl_drv_suspend(struct platform_device *pdev,
 			usb_host_set_wakeup(hcd->self.controller, false);
 
 			fsl_usb_clk_gate(hcd->self.controller->platform_data, false);
+		} else {
+			if (pdata->platform_phy_power_on)
+				pdata->platform_phy_power_on();
 		}
 
 		printk(KERN_DEBUG "host suspend ends\n");
diff --git a/drivers/video/mxc/Kconfig b/drivers/video/mxc/Kconfig
index f0afddc95bd3..8a4a0792724f 100644
--- a/drivers/video/mxc/Kconfig
+++ b/drivers/video/mxc/Kconfig
@@ -1,6 +1,6 @@
 config FB_MXC
 	tristate "MXC Framebuffer support"
-	depends on FB && (MXC_IPU || ARCH_MX21 || ARCH_MX27 || ARCH_MX25)
+	depends on FB && (MXC_IPU || ARCH_MX21 || ARCH_MX27 || ARCH_MX25 || ARCH_MX6)
 	select FB_CFB_FILLRECT
 	select FB_CFB_COPYAREA
 	select FB_CFB_IMAGEBLIT
@@ -60,6 +60,10 @@ config FB_MXC_SII902X
 	depends on FB_MXC_SYNC_PANEL && I2C
 	tristate "Si Image SII9022 DVI/HDMI Interface Chip"
 
+config FB_MXC_SII902X_ELCDIF
+	depends on FB_MXC_SYNC_PANEL && I2C
+	tristate "Si Image SII9022 DVI/HDMI Interface Chip for ELCDIF FB"
+
 config FB_MXC_CH7026
 	depends on FB_MXC_SYNC_PANEL
 	tristate "Chrontel CH7026 VGA Interface Chip"
diff --git a/drivers/video/mxc/Makefile b/drivers/video/mxc/Makefile
index 21aef052ecd6..092d0aacba27 100644
--- a/drivers/video/mxc/Makefile
+++ b/drivers/video/mxc/Makefile
@@ -1,9 +1,10 @@
 obj-$(CONFIG_FB_MXC_TVOUT_TVE)             	+= tve.o
 obj-$(CONFIG_FB_MXC_SII902X)                    += mxcfb_sii902x.o
+obj-$(CONFIG_FB_MXC_SII902X_ELCDIF)           += mxcfb_sii902x_elcdif.o
 obj-$(CONFIG_FB_MXC_LDB)			+= ldb.o
 obj-$(CONFIG_FB_MXC_MIPI_DSI)			+= mipi_dsi.o
 obj-$(CONFIG_FB_MXC_TRULY_WVGA_SYNC_PANEL) 	+= mxcfb_hx8369_wvga.o
-obj-$(CONFIG_FB_MXC_EDID)			+= mxc_edid.o mxc_dvi.o
+obj-$(CONFIG_FB_MXC_EDID)			+= mxc_edid.o
 ifeq ($(CONFIG_ARCH_MX21)$(CONFIG_ARCH_MX27)$(CONFIG_ARCH_MX25),y)
 	obj-$(CONFIG_FB_MXC_TVOUT)              += fs453.o
 	obj-$(CONFIG_FB_MXC_SYNC_PANEL)         += mx2fb.o mxcfb_modedb.o
@@ -11,8 +12,10 @@ ifeq ($(CONFIG_ARCH_MX21)$(CONFIG_ARCH_MX27)$(CONFIG_ARCH_MX25),y)
 else
 ifeq ($(CONFIG_MXC_IPU_V1),y)
 	obj-$(CONFIG_FB_MXC_SYNC_PANEL)         += mxcfb.o mxcfb_modedb.o
-else
+endif
+ifeq ($(CONFIG_MXC_IPU_V3),y)
 	obj-$(CONFIG_FB_MXC_SYNC_PANEL)         += mxc_dispdrv.o mxc_lcdif.o mxc_ipuv3_fb.o
+	obj-$(CONFIG_FB_MXC_EDID)		+= mxc_dvi.o
 endif
 	obj-$(CONFIG_FB_MXC_EPSON_PANEL)        += mxcfb_epson.o
 	obj-$(CONFIG_FB_MXC_EPSON_QVGA_PANEL)   += mxcfb_epson_qvga.o
diff --git a/drivers/video/mxc/mxc_elcdif_fb.c b/drivers/video/mxc/mxc_elcdif_fb.c
index 9c3b7b99a411..7eef316b1afe 100644
--- a/drivers/video/mxc/mxc_elcdif_fb.c
+++ b/drivers/video/mxc/mxc_elcdif_fb.c
@@ -72,6 +72,7 @@ struct mxc_elcdif_fb_data {
 	bool wait4vsync;
 	bool wait4framedone;
 	bool panning;
+	bool running;
 	struct completion vsync_complete;
 	struct completion frame_done_complete;
 	struct semaphore flip_sem;
@@ -86,6 +87,12 @@ struct elcdif_signal_cfg {
 	unsigned Vsync_pol:1;	/* true = active high */
 };
 
+struct mxcfb_mode {
+	int dev_mode;
+	int num_modes;
+	struct fb_videomode *mode;
+};
+
 static int mxc_elcdif_fb_blank(int blank, struct fb_info *info);
 static int mxc_elcdif_fb_map_video_memory(struct fb_info *info);
 static int mxc_elcdif_fb_unmap_video_memory(struct fb_info *info);
@@ -97,6 +104,15 @@ static bool g_elcdif_axi_clk_enable;
 static bool g_elcdif_pix_clk_enable;
 static struct clk *g_elcdif_axi_clk;
 static struct clk *g_elcdif_pix_clk;
+static struct mxcfb_mode mxc_disp_mode;
+
+static void dump_fb_videomode(struct fb_videomode *m)
+{
+	pr_debug("fb_videomode = %d %d %d %d %d %d %d %d %d %d %d %d %d\n",
+		m->refresh, m->xres, m->yres, m->pixclock, m->left_margin,
+		m->right_margin, m->upper_margin, m->lower_margin,
+		m->hsync_len, m->vsync_len, m->sync, m->vmode, m->flag);
+}
 
 static inline void setup_dotclk_panel(u32 pixel_clk,
 				      u16 v_pulse_width,
@@ -113,16 +129,39 @@ static inline void setup_dotclk_panel(u32 pixel_clk,
 				      int enable_present)
 {
 	u32 val, rounded_pixel_clk;
+	u32 rounded_parent_clk;
+	struct clk *clk_parent;
 
 	if (!g_elcdif_axi_clk_enable) {
 		clk_enable(g_elcdif_axi_clk);
 		g_elcdif_axi_clk_enable = true;
 	}
 
+	/* Init clocking */
 	dev_dbg(g_elcdif_dev, "pixel clk = %d\n", pixel_clk);
-	rounded_pixel_clk = clk_round_rate(g_elcdif_pix_clk, pixel_clk);
+
+	clk_parent = clk_get_parent(g_elcdif_pix_clk);
+	if (clk_parent == NULL)
+		dev_err(g_elcdif_dev, "%s Failed get clk parent\n", __func__);
+
+	rounded_pixel_clk = pixel_clk * 2;
+
+	rounded_parent_clk = clk_round_rate(clk_parent,
+				rounded_pixel_clk);
+
+	while (rounded_pixel_clk < rounded_parent_clk) {
+		/* the max divider from parent to di is 8 */
+		if (rounded_parent_clk / pixel_clk < 8)
+			rounded_pixel_clk += pixel_clk * 2;
+	}
+
+	clk_set_rate(clk_parent, rounded_pixel_clk);
+	rounded_pixel_clk =
+		clk_round_rate(g_elcdif_pix_clk, pixel_clk);
 	clk_set_rate(g_elcdif_pix_clk, rounded_pixel_clk);
 
+	msleep(5);
+
 	__raw_writel(BM_ELCDIF_CTRL_DATA_SHIFT_DIR,
 		     elcdif_base + HW_ELCDIF_CTRL_CLR);
 
@@ -519,6 +558,35 @@ static inline void mxc_init_elcdif(void)
 	return;
 }
 
+void mxcfb_elcdif_register_mode(const struct fb_videomode *modedb,
+	int num_modes, int dev_mode)
+{
+	struct fb_videomode *mode;
+
+	mode = kzalloc(num_modes * sizeof(struct fb_videomode), GFP_KERNEL);
+
+	if (!mode) {
+		dev_err(g_elcdif_dev, "%s Failed to allocate mode data\n", __func__);
+		return;
+	}
+
+	if (mxc_disp_mode.num_modes)
+		memcpy(mode, mxc_disp_mode.mode,
+			mxc_disp_mode.num_modes * sizeof(struct fb_videomode));
+	if (modedb)
+		memcpy(mode + mxc_disp_mode.num_modes, modedb,
+			num_modes * sizeof(struct fb_videomode));
+
+	if (mxc_disp_mode.num_modes)
+		kfree(mxc_disp_mode.mode);
+
+	mxc_disp_mode.mode = mode;
+	mxc_disp_mode.num_modes += num_modes;
+	mxc_disp_mode.dev_mode = dev_mode;
+
+	return;
+}
+
 int mxc_elcdif_frame_addr_setup(dma_addr_t phys)
 {
 	int ret = 0;
@@ -548,7 +616,7 @@ static inline void mxc_elcdif_dma_release(void)
 	return;
 }
 
-static inline void mxc_elcdif_run(void)
+static inline void mxc_elcdif_run(struct mxc_elcdif_fb_data *data)
 {
 	if (!g_elcdif_axi_clk_enable) {
 		clk_enable(g_elcdif_axi_clk);
@@ -559,6 +627,9 @@ static inline void mxc_elcdif_run(void)
 		     elcdif_base + HW_ELCDIF_CTRL_SET);
 	__raw_writel(BM_ELCDIF_CTRL_RUN,
 		     elcdif_base + HW_ELCDIF_CTRL_SET);
+
+	data->running = true;
+
 	return;
 }
 
@@ -813,8 +884,8 @@ static int mxc_elcdif_fb_set_par(struct fb_info *fbi)
 	dev_dbg(fbi->device, "Reconfiguring framebuffer\n");
 
 	/* If parameter no change, don't reconfigure. */
-	if (mxc_elcdif_fb_par_equal(fbi, data))
-	    return 0;
+	if (mxc_elcdif_fb_par_equal(fbi, data) && (data->running == true))
+		return 0;
 
 	sema_init(&data->flip_sem, 1);
 
@@ -882,7 +953,7 @@ static int mxc_elcdif_fb_set_par(struct fb_info *fbi)
 			   sig_cfg,
 			   1);
 	mxc_elcdif_frame_addr_setup(fbi->fix.smem_start);
-	mxc_elcdif_run();
+	mxc_elcdif_run(data);
 	mxc_elcdif_blank_panel(FB_BLANK_UNBLANK);
 
 	fbi->mode = (struct fb_videomode *)fb_match_mode(&fbi->var,
@@ -1236,6 +1307,9 @@ static int mxc_elcdif_fb_probe(struct platform_device *pdev)
 	struct resource *res;
 	struct fb_info *fbi;
 	struct mxc_fb_platform_data *pdata = pdev->dev.platform_data;
+	const struct fb_videomode *mode;
+	struct fb_videomode m;
+	int num;
 
 	fbi = framebuffer_alloc(sizeof(struct mxc_elcdif_fb_data), &pdev->dev);
 	if (fbi == NULL) {
@@ -1296,20 +1370,63 @@ static int mxc_elcdif_fb_probe(struct platform_device *pdev)
 	if (pdata && !data->output_pix_fmt)
 		data->output_pix_fmt = pdata->interface_pix_fmt;
 
+	INIT_LIST_HEAD(&fbi->modelist);
+
 	if (pdata && pdata->mode && pdata->num_modes)
 		fb_videomode_to_modelist(pdata->mode, pdata->num_modes,
 				&fbi->modelist);
 
+	if (mxc_disp_mode.num_modes) {
+		int i;
+		mode = mxc_disp_mode.mode;
+		num = mxc_disp_mode.num_modes;
+
+		for (i = 0; i < num; i++) {
+			/*
+			 * FIXME now we do not support interlaced
+			 * mode for ddc mode
+			 */
+			if ((mxc_disp_mode.dev_mode
+				& MXC_DISP_DDC_DEV) &&
+				(mode[i].vmode & FB_VMODE_INTERLACED))
+				continue;
+			else {
+				dev_dbg(&pdev->dev, "Added mode %d:", i);
+				dev_dbg(&pdev->dev,
+					"xres = %d, yres = %d, freq = %d, vmode = %d, flag = %d\n",
+					mode[i].xres, mode[i].yres,	mode[i].refresh, mode[i].vmode,
+					mode[i].flag);
+				fb_add_videomode(&mode[i], &fbi->modelist);
+			}
+		}
+	}
+
 	if (!fb_mode && pdata && pdata->mode_str)
 		fb_mode = pdata->mode_str;
 
 	if (fb_mode) {
+		dev_dbg(&pdev->dev, "default video mode %s\n", fb_mode);
 		ret = fb_find_mode(&fbi->var, fbi, fb_mode, NULL, 0, NULL,
 				   default_bpp);
-		if ((!ret || (ret > 2)) && pdata && pdata->mode &&
-		    pdata->num_modes)
-			fb_find_mode(&fbi->var, fbi, fb_mode, pdata->mode,
+		if ((ret == 1) || (ret == 2)) {
+			fb_var_to_videomode(&m, &fbi->var);
+			dump_fb_videomode(&m);
+			mode = fb_find_nearest_mode(&m,
+				&fbi->modelist);
+			fb_videomode_to_var(&fbi->var, mode);
+		} else if (pdata && pdata->mode && pdata->num_modes) {
+			ret = fb_find_mode(&fbi->var, fbi, fb_mode, pdata->mode,
 					pdata->num_modes, NULL, default_bpp);
+			if (!ret) {
+				dev_err(fbi->device,
+					"No valid video mode found");
+				goto err2;
+			}
+		} else {
+			dev_err(fbi->device,
+				"No valid video mode found");
+			goto err2;
+		}
 	}
 
 	mxc_elcdif_fb_check_var(&fbi->var, fbi);
@@ -1343,6 +1460,19 @@ static int mxc_elcdif_fb_probe(struct platform_device *pdev)
 	 */
 	clk_set_rate(g_elcdif_pix_clk, 25000000);
 
+	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 (data->cur_blank == FB_BLANK_UNBLANK) {
+		console_lock();
+		fb_blank(fbi, FB_BLANK_UNBLANK);
+		console_unlock();
+	}
+
 	ret = register_framebuffer(fbi);
 	if (ret)
 		goto err3;
@@ -1424,6 +1554,7 @@ static int mxc_elcdif_fb_suspend(struct platform_device *pdev,
 		clk_disable(g_elcdif_axi_clk);
 		g_elcdif_axi_clk_enable = false;
 	}
+	data->running = false;
 	console_unlock();
 	return 0;
 }
diff --git a/drivers/video/mxc/mxc_epdc_fb.c b/drivers/video/mxc/mxc_epdc_fb.c
index 0b3923c2cea6..727c6ebc575c 100644
--- a/drivers/video/mxc/mxc_epdc_fb.c
+++ b/drivers/video/mxc/mxc_epdc_fb.c
@@ -50,6 +50,7 @@
 #include <linux/bitops.h>
 #include <mach/epdc.h>
 #include <mach/dma.h>
+#include <asm/cacheflush.h>
 
 #include "epdc_regs.h"
 
@@ -69,6 +70,10 @@
 #define INVALID_LUT		(-1)
 #define DRY_RUN_NO_LUT		100
 
+/* Maximum update buffer image width due to v2.0 and v2.1 errata ERR005313. */
+#define EPDC_V2_MAX_UPDATE_WIDTH	2047
+#define EPDC_V2_ROTATION_ALIGNMENT	8
+
 #define DEFAULT_TEMP_INDEX	0
 #define DEFAULT_TEMP		20 /* room temp in deg Celsius */
 
@@ -202,6 +207,8 @@ struct mxc_epdc_fb_data {
 	bool updates_active;
 	int pwrdown_delay;
 	unsigned long tce_prevent;
+	bool restrict_width; /* work around rev >=2.0 width and
+				stride restriction  */
 
 	/* FB elements related to PxP DMA */
 	struct completion pxp_tx_cmpl;
@@ -262,6 +269,16 @@ static int pxp_complete_update(struct mxc_epdc_fb_data *fb_data, u32 *hist_stat)
 static void draw_mode0(struct mxc_epdc_fb_data *fb_data);
 static bool is_free_list_full(struct mxc_epdc_fb_data *fb_data);
 
+static void do_dithering_processing_Y1_v1_0(
+		unsigned char *update_region_ptr,
+		struct mxcfb_rect *update_region,
+		unsigned long update_region_stride,
+		int *err_dist);
+static void do_dithering_processing_Y4_v1_0(
+		unsigned char *update_region_ptr,
+		struct mxcfb_rect *update_region,
+		unsigned long update_region_stride,
+		int *err_dist);
 
 #ifdef DEBUG
 static void dump_pxp_config(struct mxc_epdc_fb_data *fb_data,
@@ -718,18 +735,24 @@ static int epdc_choose_next_lut(int rev, int *next_lut)
 {
 	u64 luts_status, unprocessed_luts;
 	bool next_lut_found = false;
+	/* Available LUTs are reduced to 16 in 5-bit waveform mode */
+	u32 format_p5n = __raw_readl(EPDC_FORMAT) &
+		EPDC_FORMAT_BUF_PIXEL_FORMAT_P5N;
 
 	luts_status = __raw_readl(EPDC_STATUS_LUTS);
-	if (rev < 20)
+	if ((rev < 20) || format_p5n)
 		luts_status &= 0xFFFF;
 	else
 		luts_status |= ((u64)__raw_readl(EPDC_STATUS_LUTS2) << 32);
 
-	if (rev < 20)
+	if (rev < 20) {
 		unprocessed_luts = __raw_readl(EPDC_IRQ) & 0xFFFF;
-	else
+	} else {
 		unprocessed_luts = __raw_readl(EPDC_IRQ1) |
 			((u64)__raw_readl(EPDC_IRQ2) << 32);
+		if (format_p5n)
+			unprocessed_luts &= 0xFFFF;
+	}
 
 	while (!next_lut_found) {
 		/*
@@ -745,7 +768,7 @@ static int epdc_choose_next_lut(int rev, int *next_lut)
 		 */
 		*next_lut = fls64(luts_status);
 
-		if (rev < 20) {
+		if ((rev < 20) || format_p5n) {
 			if (*next_lut > 15)
 				*next_lut = ffz(luts_status);
 		} else {
@@ -1930,10 +1953,10 @@ static int epdc_process_update(struct update_data_list *upd_data_list,
 	 * to copybuf in addition to the PxP structures */
 	mutex_lock(&fb_data->pxp_mutex);
 
-	if ((((width_unaligned || height_unaligned || input_unaligned) &&
+	if (((((width_unaligned || height_unaligned || input_unaligned) &&
 		(upd_desc_list->upd_data.waveform_mode == WAVEFORM_MODE_AUTO))
-		|| line_overflow) && (fb_data->rev < 20)) {
-
+		|| line_overflow) && (fb_data->rev < 20)) ||
+		fb_data->restrict_width) {
 		dev_dbg(fb_data->dev, "Copying update before processing.\n");
 
 		/* Update to reflect what the new source buffer will be */
@@ -2137,7 +2160,8 @@ static int epdc_process_update(struct update_data_list *upd_data_list,
 }
 
 static int epdc_submit_merge(struct update_desc_list *upd_desc_list,
-				struct update_desc_list *update_to_merge)
+				struct update_desc_list *update_to_merge,
+				struct mxc_epdc_fb_data *fb_data)
 {
 	struct mxcfb_update_data *a, *b;
 	struct mxcfb_rect *arect, *brect;
@@ -2195,6 +2219,19 @@ static int epdc_submit_merge(struct update_desc_list *upd_desc_list,
 			(arect->top + arect->height - combine.top) :
 			(brect->top + brect->height - combine.top);
 
+	/* Don't merge if combined width exceeds max width */
+	if (fb_data->restrict_width) {
+		u32 max_width = EPDC_V2_MAX_UPDATE_WIDTH;
+		u32 combined_width = combine.width;
+		if (fb_data->epdc_fb_var.rotate != FB_ROTATE_UR)
+			max_width -= EPDC_V2_ROTATION_ALIGNMENT;
+		if ((fb_data->epdc_fb_var.rotate == FB_ROTATE_CW) ||
+			(fb_data->epdc_fb_var.rotate == FB_ROTATE_CCW))
+			combined_width = combine.height;
+		if (combined_width > max_width)
+			return MERGE_FAIL;
+	}
+
 	*arect = combine;
 
 	/* Use flags of the later update */
@@ -2226,6 +2263,7 @@ static void epdc_submit_work_func(struct work_struct *work)
 	bool end_merge = false;
 	bool is_transform;
 	u32 update_addr;
+	int *err_dist;
 	int ret;
 
 	/* Protect access to buffer queues and to update HW */
@@ -2260,7 +2298,8 @@ static void epdc_submit_work_func(struct work_struct *work)
 				break;
 		} else {
 			switch (epdc_submit_merge(upd_data_list->update_desc,
-						next_update->update_desc)) {
+						next_update->update_desc,
+						fb_data)) {
 			case MERGE_OK:
 				dev_dbg(fb_data->dev,
 					"Update merged [collision]\n");
@@ -2330,7 +2369,7 @@ static void epdc_submit_work_func(struct work_struct *work)
 					break;
 			} else {
 				switch (epdc_submit_merge(upd_data_list->update_desc,
-						next_desc)) {
+						next_desc, fb_data)) {
 				case MERGE_OK:
 					dev_dbg(fb_data->dev,
 						"Update merged [queue]\n");
@@ -2371,12 +2410,14 @@ static void epdc_submit_work_func(struct work_struct *work)
 	 * PxP in versions 2.0 and earlier of EPDC.
 	 */
 	is_transform = upd_data_list->update_desc->upd_data.flags &
-		(EPDC_FLAG_ENABLE_INVERSION |
-		EPDC_FLAG_FORCE_MONOCHROME | EPDC_FLAG_USE_CMAP) ?
-		true : false;
+		(EPDC_FLAG_ENABLE_INVERSION | EPDC_FLAG_USE_DITHERING_Y1 |
+		EPDC_FLAG_USE_DITHERING_Y4 | EPDC_FLAG_FORCE_MONOCHROME |
+		EPDC_FLAG_USE_CMAP) ? true : false;
+
 	if ((fb_data->epdc_fb_var.rotate == FB_ROTATE_UR) &&
 		(fb_data->epdc_fb_var.grayscale == GRAYSCALE_8BIT) &&
-		!is_transform && (fb_data->rev > 20)) {
+		!is_transform && (fb_data->rev > 20) &&
+		!fb_data->restrict_width) {
 
 		/* If needed, enable EPDC HW while ePxP is processing */
 		if ((fb_data->power_state == POWER_STATE_OFF)
@@ -2391,7 +2432,6 @@ static void epdc_submit_work_func(struct work_struct *work)
 		update_addr = fb_data->info.fix.smem_start +
 			((upd_region->top * fb_data->info.var.xres_virtual) +
 			upd_region->left) * fb_data->info.var.bits_per_pixel/8;
-
 		upd_data_list->update_desc->epdc_stride =
 					fb_data->info.var.xres_virtual *
 					fb_data->info.var.bits_per_pixel/8;
@@ -2457,6 +2497,45 @@ static void epdc_submit_work_func(struct work_struct *work)
 	}
 
 	/*
+	 * Dithering Processing (Version 1.0 - for i.MX508 and i.MX6SL)
+	 */
+	if (upd_data_list->update_desc->upd_data.flags &
+	    EPDC_FLAG_USE_DITHERING_Y1) {
+
+		err_dist = kzalloc((fb_data->info.var.xres_virtual + 3) * 3
+				* sizeof(int), GFP_KERNEL);
+
+		/* Dithering Y8 -> Y1 */
+		do_dithering_processing_Y1_v1_0(
+				(uint8_t *)(upd_data_list->virt_addr +
+				upd_data_list->update_desc->epdc_offs),
+				&adj_update_region,
+				(fb_data->rev < 20) ?
+				ALIGN(adj_update_region.width, 8) :
+				adj_update_region.width,
+				err_dist);
+
+		kfree(err_dist);
+	} else if (upd_data_list->update_desc->upd_data.flags &
+		EPDC_FLAG_USE_DITHERING_Y4) {
+
+		err_dist = kzalloc((fb_data->info.var.xres_virtual + 3) * 3
+				* sizeof(int), GFP_KERNEL);
+
+		/* Dithering Y8 -> Y1 */
+		do_dithering_processing_Y4_v1_0(
+				(uint8_t *)(upd_data_list->virt_addr +
+				upd_data_list->update_desc->epdc_offs),
+				&adj_update_region,
+				(fb_data->rev < 20) ?
+				ALIGN(adj_update_region.width, 8) :
+				adj_update_region.width,
+				err_dist);
+
+		kfree(err_dist);
+	}
+
+	/*
 	 * If there are no LUTs available,
 	 * then we must wait for the resource to become free.
 	 * The IST will signal this event.
@@ -2572,8 +2651,7 @@ static void epdc_submit_work_func(struct work_struct *work)
 	mutex_unlock(&fb_data->queue_mutex);
 }
 
-
-int mxc_epdc_fb_send_update(struct mxcfb_update_data *upd_data,
+static int mxc_epdc_fb_send_single_update(struct mxcfb_update_data *upd_data,
 				   struct fb_info *info)
 {
 	struct mxc_epdc_fb_data *fb_data = info ?
@@ -2868,6 +2946,63 @@ int mxc_epdc_fb_send_update(struct mxcfb_update_data *upd_data,
 	mutex_unlock(&fb_data->queue_mutex);
 	return 0;
 }
+
+int mxc_epdc_fb_send_update(struct mxcfb_update_data *upd_data,
+				   struct fb_info *info)
+{
+	struct mxc_epdc_fb_data *fb_data = info ?
+		(struct mxc_epdc_fb_data *)info:g_fb_data;
+
+	if (!fb_data->restrict_width) {
+		/* No width restriction, send entire update region */
+		return mxc_epdc_fb_send_single_update(upd_data, info);
+	} else {
+		int ret;
+		__u32 width, left;
+		__u32 marker;
+		__u32 *region_width, *region_left;
+		u32 max_upd_width = EPDC_V2_MAX_UPDATE_WIDTH;
+
+		/* Further restrict max width due to pxp rotation
+		  * alignment requirement
+		  */
+		if (fb_data->epdc_fb_var.rotate != FB_ROTATE_UR)
+			max_upd_width -= EPDC_V2_ROTATION_ALIGNMENT;
+
+		/* Select split of width or height based on rotation */
+		if ((fb_data->epdc_fb_var.rotate == FB_ROTATE_UR) ||
+			(fb_data->epdc_fb_var.rotate == FB_ROTATE_UD)) {
+			region_width = &upd_data->update_region.width;
+			region_left = &upd_data->update_region.left;
+		} else {
+			region_width = &upd_data->update_region.height;
+			region_left = &upd_data->update_region.top;
+		}
+
+		if (*region_width <= max_upd_width)
+			return mxc_epdc_fb_send_single_update(upd_data,	info);
+
+		width = *region_width;
+		left = *region_left;
+		marker = upd_data->update_marker;
+		upd_data->update_marker = 0;
+
+		do {
+			*region_width = max_upd_width;
+			*region_left = left;
+			ret = mxc_epdc_fb_send_single_update(upd_data, info);
+			if (ret)
+				return ret;
+			width -= max_upd_width;
+			left += max_upd_width;
+		} while (width > max_upd_width);
+
+		*region_width = width;
+		*region_left = left;
+		upd_data->update_marker = marker;
+		return mxc_epdc_fb_send_single_update(upd_data, info);
+	}
+}
 EXPORT_SYMBOL(mxc_epdc_fb_send_update);
 
 int mxc_epdc_fb_wait_update_complete(struct mxcfb_update_marker_data *marker_data,
@@ -3041,6 +3176,25 @@ static int mxc_epdc_fb_ioctl(struct fb_info *info, unsigned int cmd,
 			ret = 0;
 			break;
 		}
+
+	case MXCFB_GET_WORK_BUFFER:
+		{
+			/* copy the epdc working buffer to the user space */
+			struct mxc_epdc_fb_data *fb_data = info ?
+				(struct mxc_epdc_fb_data *)info:g_fb_data;
+			flush_cache_all();
+			outer_flush_all();
+			if (copy_to_user((void __user *)arg,
+				(const void *) fb_data->working_buffer_virt,
+				fb_data->working_buffer_size))
+				ret = -EFAULT;
+			else
+				ret = 0;
+			flush_cache_all();
+			outer_flush_all();
+			break;
+		}
+
 	default:
 		break;
 	}
@@ -3546,7 +3700,13 @@ static void epdc_intr_work_func(struct work_struct *work)
 					next_marker->update_marker);
 				complete(&next_marker->update_completion);
 			}
-		} else if (epdc_lut_cancelled) {
+		} else if (epdc_lut_cancelled && !epdc_collision) {
+			/*
+			* Note: The update may be cancelled (void) if all
+			* pixels collided. In that case we handle it as a
+			* collision, not a cancel.
+			*/
+
 			/* Clear LUT status (might be set if no AUTOWV used) */
 
 			/*
@@ -4076,7 +4236,8 @@ static void mxc_epdc_fb_fw_handler(const struct firmware *fw,
 		&fb_data->info);
 	if (ret < 0)
 		dev_err(fb_data->dev,
-			"Wait for update complete failed.  Error = 0x%x", ret);
+			"Wait for initial update complete failed."
+			" Error = 0x%x", ret);
 }
 
 static int mxc_epdc_fb_init_hw(struct fb_info *info)
@@ -4417,6 +4578,8 @@ int __devinit mxc_epdc_fb_probe(struct platform_device *pdev)
 	} else {
 		fb_data->num_luts = EPDC_V2_NUM_LUTS;
 		fb_data->max_num_updates = EPDC_V2_MAX_NUM_UPDATES;
+		if (vmode->xres > EPDC_V2_MAX_UPDATE_WIDTH)
+			fb_data->restrict_width = true;
 	}
 	fb_data->max_num_buffers = EPDC_MAX_NUM_BUFFERS;
 
@@ -4454,8 +4617,9 @@ int __devinit mxc_epdc_fb_probe(struct platform_device *pdev)
 		 * be as big as the full-screen frame buffer
 		 */
 		fb_data->virt_addr_updbuf[i] =
-		    dma_alloc_coherent(fb_data->info.device, fb_data->max_pix_size,
-				       &fb_data->phys_addr_updbuf[i], GFP_DMA);
+			kmalloc(fb_data->max_pix_size, GFP_KERNEL);
+		fb_data->phys_addr_updbuf[i] =
+			virt_to_phys(fb_data->virt_addr_updbuf[i]);
 		if (fb_data->virt_addr_updbuf[i] == NULL) {
 			ret = -ENOMEM;
 			goto out_upd_buffers;
@@ -4737,10 +4901,7 @@ out_copybuffer:
 out_upd_buffers:
 	for (i = 0; i < fb_data->max_num_buffers; i++)
 		if (fb_data->virt_addr_updbuf[i] != NULL)
-			dma_free_writecombine(&pdev->dev,
-				fb_data->max_pix_size,
-				fb_data->virt_addr_updbuf[i],
-				fb_data->phys_addr_updbuf[i]);
+			kfree(fb_data->virt_addr_updbuf[i]);
 	if (fb_data->virt_addr_updbuf != NULL)
 		kfree(fb_data->virt_addr_updbuf);
 	if (fb_data->phys_addr_updbuf != NULL)
@@ -4785,9 +4946,7 @@ static int mxc_epdc_fb_remove(struct platform_device *pdev)
 
 	for (i = 0; i < fb_data->max_num_buffers; i++)
 		if (fb_data->virt_addr_updbuf[i] != NULL)
-			dma_free_writecombine(&pdev->dev, fb_data->max_pix_size,
-				fb_data->virt_addr_updbuf[i],
-				fb_data->phys_addr_updbuf[i]);
+			kfree(fb_data->virt_addr_updbuf[i]);
 	if (fb_data->virt_addr_updbuf != NULL)
 		kfree(fb_data->virt_addr_updbuf);
 	if (fb_data->phys_addr_updbuf != NULL)
@@ -5082,6 +5241,124 @@ static int pxp_complete_update(struct mxc_epdc_fb_data *fb_data, u32 *hist_stat)
 	return 0;
 }
 
+/*
+ * Different dithering algorithm can be used. We chose
+ * to implement Bill Atkinson's algorithm as an example
+ * Thanks Bill Atkinson for his dithering algorithm.
+ */
+
+/*
+ * Dithering algorithm implementation - Y8->Y1 version 1.0 for i.MX
+ */
+static void do_dithering_processing_Y1_v1_0(
+		unsigned char *update_region_ptr,
+		struct mxcfb_rect *update_region,
+		unsigned long update_region_stride,
+		int *err_dist)
+{
+
+	/* create a temp error distribution array */
+	int bwPix;
+	int y;
+	int col;
+	int *err_dist_l0, *err_dist_l1, *err_dist_l2, distrib_error;
+	int width_3 = update_region->width + 3;
+	char *y8buf;
+	int x_offset = 0;
+
+	/* prime a few elements the error distribution array */
+	for (y = 0; y < update_region->height; y++) {
+		/* Dithering the Y8 in sbuf to BW suitable for A2 waveform */
+		err_dist_l0 = err_dist + (width_3) * (y % 3);
+		err_dist_l1 = err_dist + (width_3) * ((y + 1) % 3);
+		err_dist_l2 = err_dist + (width_3) * ((y + 2) % 3);
+
+		y8buf = update_region_ptr + x_offset;
+
+		/* scan the line and convert the Y8 to BW */
+		for (col = 1; col <= update_region->width; col++) {
+			bwPix = *(err_dist_l0 + col) + *y8buf;
+
+			if (bwPix >= 128) {
+				*y8buf++ = 0xff;
+				distrib_error = (bwPix - 255) >> 3;
+			} else {
+				*y8buf++ = 0;
+				distrib_error = bwPix >> 3;
+			}
+
+			/* modify the error distribution buffer */
+			*(err_dist_l0 + col + 2) += distrib_error;
+			*(err_dist_l1 + col + 1) += distrib_error;
+			*(err_dist_l0 + col + 1) += distrib_error;
+			*(err_dist_l1 + col - 1) += distrib_error;
+			*(err_dist_l1 + col) += distrib_error;
+			*(err_dist_l2 + col) = distrib_error;
+		}
+		x_offset += update_region_stride;
+	}
+
+	flush_cache_all();
+	outer_flush_all();
+}
+
+/*
+ * Dithering algorithm implementation - Y8->Y4 version 1.0 for i.MX
+ */
+
+static void do_dithering_processing_Y4_v1_0(
+		unsigned char *update_region_ptr,
+		struct mxcfb_rect *update_region,
+		unsigned long update_region_stride,
+		int *err_dist)
+{
+
+	/* create a temp error distribution array */
+	int gcPix;
+	int y;
+	int col;
+	int *err_dist_l0, *err_dist_l1, *err_dist_l2, distrib_error;
+	int width_3 = update_region->width + 3;
+	char *y8buf;
+	int x_offset = 0;
+
+	/* prime a few elements the error distribution array */
+	for (y = 0; y < update_region->height; y++) {
+		/* Dithering the Y8 in sbuf to Y4 */
+		err_dist_l0 = err_dist + (width_3) * (y % 3);
+		err_dist_l1 = err_dist + (width_3) * ((y + 1) % 3);
+		err_dist_l2 = err_dist + (width_3) * ((y + 2) % 3);
+
+		y8buf = update_region_ptr + x_offset;
+
+		/* scan the line and convert the Y8 to Y4 */
+		for (col = 1; col <= update_region->width; col++) {
+			gcPix = *(err_dist_l0 + col) + *y8buf;
+
+			if (gcPix > 255)
+				gcPix = 255;
+			else if (gcPix < 0)
+				gcPix = 0;
+
+			distrib_error = (*y8buf - (gcPix & 0xf0)) >> 3;
+
+			*y8buf++ = gcPix & 0xf0;
+
+			/* modify the error distribution buffer */
+			*(err_dist_l0 + col + 2) += distrib_error;
+			*(err_dist_l1 + col + 1) += distrib_error;
+			*(err_dist_l0 + col + 1) += distrib_error;
+			*(err_dist_l1 + col - 1) += distrib_error;
+			*(err_dist_l1 + col) += distrib_error;
+			*(err_dist_l2 + col) = distrib_error;
+		}
+		x_offset += update_region_stride;
+	}
+
+	flush_cache_all();
+	outer_flush_all();
+}
+
 static int __init mxc_epdc_fb_init(void)
 {
 	return platform_driver_register(&mxc_epdc_fb_driver);
diff --git a/drivers/video/mxc/mxc_ipuv3_fb.c b/drivers/video/mxc/mxc_ipuv3_fb.c
index b38f97d246a5..a41001c834c6 100644
--- a/drivers/video/mxc/mxc_ipuv3_fb.c
+++ b/drivers/video/mxc/mxc_ipuv3_fb.c
@@ -113,6 +113,25 @@ struct mxcfb_info {
 #endif
 };
 
+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;
@@ -135,14 +154,12 @@ static void mxcfb_early_suspend(struct early_suspend *h);
 static void mxcfb_later_resume(struct early_suspend *h);
 #endif
 
-static uint32_t bpp_to_pixfmt(struct fb_info *fbi)
+/* Return default standard(RGB) pixel format */
+static uint32_t bpp_to_pixfmt(int bpp)
 {
 	uint32_t pixfmt = 0;
 
-	if (fbi->var.nonstd)
-		return fbi->var.nonstd;
-
-	switch (fbi->var.bits_per_pixel) {
+	switch (bpp) {
 	case 24:
 		pixfmt = IPU_PIX_FMT_BGR24;
 		break;
@@ -156,6 +173,82 @@ static uint32_t bpp_to_pixfmt(struct fb_info *fbi)
 	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;
@@ -216,13 +309,13 @@ static int _setup_disp_channel1(struct fb_info *fbi)
 		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 = bpp_to_pixfmt(fbi);
+		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 = bpp_to_pixfmt(fbi);
+		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;
 	}
@@ -239,7 +332,7 @@ static int _setup_disp_channel2(struct fb_info *fbi)
 	unsigned long base;
 	unsigned int fr_xoff, fr_yoff, fr_w, fr_h;
 
-	switch (bpp_to_pixfmt(fbi)) {
+	switch (fbi_to_pixfmt(fbi)) {
 	case IPU_PIX_FMT_YUV420P2:
 	case IPU_PIX_FMT_YVU420P:
 	case IPU_PIX_FMT_NV12:
@@ -287,7 +380,7 @@ static int _setup_disp_channel2(struct fb_info *fbi)
 
 	retval = ipu_init_channel_buffer(mxc_fbi->ipu,
 					 mxc_fbi->ipu_ch, IPU_INPUT_BUFFER,
-					 bpp_to_pixfmt(fbi),
+					 fbi_to_pixfmt(fbi),
 					 fbi->var.xres, fbi->var.yres,
 					 fb_stride,
 					 fbi->var.rotate,
@@ -305,7 +398,7 @@ static int _setup_disp_channel2(struct fb_info *fbi)
 	/* update u/v offset */
 	ipu_update_channel_offset(mxc_fbi->ipu, mxc_fbi->ipu_ch,
 			IPU_INPUT_BUFFER,
-			bpp_to_pixfmt(fbi),
+			fbi_to_pixfmt(fbi),
 			fr_w,
 			fr_h,
 			fr_w,
@@ -786,76 +879,9 @@ static int mxcfb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 	    (var->bits_per_pixel != 8))
 		var->bits_per_pixel = 16;
 
-	switch (var->bits_per_pixel) {
-	case 8:
-		var->red.length = 3;
-		var->red.offset = 5;
-		var->red.msb_right = 0;
-
-		var->green.length = 3;
-		var->green.offset = 2;
-		var->green.msb_right = 0;
-
-		var->blue.length = 2;
-		var->blue.offset = 0;
-		var->blue.msb_right = 0;
-
-		var->transp.length = 0;
-		var->transp.offset = 0;
-		var->transp.msb_right = 0;
-		break;
-	case 16:
-		var->red.length = 5;
-		var->red.offset = 11;
-		var->red.msb_right = 0;
-
-		var->green.length = 6;
-		var->green.offset = 5;
-		var->green.msb_right = 0;
-
-		var->blue.length = 5;
-		var->blue.offset = 0;
-		var->blue.msb_right = 0;
-
-		var->transp.length = 0;
-		var->transp.offset = 0;
-		var->transp.msb_right = 0;
-		break;
-	case 24:
-		var->red.length = 8;
-		var->red.offset = 16;
-		var->red.msb_right = 0;
-
-		var->green.length = 8;
-		var->green.offset = 8;
-		var->green.msb_right = 0;
-
-		var->blue.length = 8;
-		var->blue.offset = 0;
-		var->blue.msb_right = 0;
-
-		var->transp.length = 0;
-		var->transp.offset = 0;
-		var->transp.msb_right = 0;
-		break;
-	case 32:
-		var->red.length = 8;
-		var->red.offset = 16;
-		var->red.msb_right = 0;
-
-		var->green.length = 8;
-		var->green.offset = 8;
-		var->green.msb_right = 0;
-
-		var->blue.length = 8;
-		var->blue.offset = 0;
-		var->blue.msb_right = 0;
-
-		var->transp.length = 8;
-		var->transp.offset = 24;
-		var->transp.msb_right = 0;
-		break;
-	}
+	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 +
@@ -1443,7 +1469,7 @@ mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 	if (y_bottom > info->var.yres_virtual)
 		return -EINVAL;
 
-	switch (bpp_to_pixfmt(info)) {
+	switch (fbi_to_pixfmt(info)) {
 	case IPU_PIX_FMT_YUV420P2:
 	case IPU_PIX_FMT_YVU420P:
 	case IPU_PIX_FMT_NV12:
@@ -1530,7 +1556,7 @@ mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 		/* update u/v offset */
 		ipu_update_channel_offset(mxc_fbi->ipu, mxc_fbi->ipu_ch,
 				IPU_INPUT_BUFFER,
-				bpp_to_pixfmt(info),
+				fbi_to_pixfmt(info),
 				fr_w,
 				fr_h,
 				fr_w,
@@ -1982,12 +2008,14 @@ static int mxcfb_dispdrv_init(struct platform_device *pdev,
  * 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)
+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)) {
@@ -2005,61 +2033,63 @@ static int mxcfb_option_setup(struct platform_device *pdev)
 		if (!strncmp(opt, "dev=", 4)) {
 			memcpy(pdata->disp_dev, opt + 4, strlen(opt) - 4);
 			pdata->disp_dev[strlen(opt) - 4] = '\0';
-			continue;
-		}
-		if (!strncmp(opt, "if=", 3)) {
-			if (!strncmp(opt+3, "RGB24", 5)) {
+		} else if (!strncmp(opt, "if=", 3)) {
+			if (!strncmp(opt+3, "RGB24", 5))
 				pdata->interface_pix_fmt = IPU_PIX_FMT_RGB24;
-				continue;
-			} else if (!strncmp(opt+6, "BGR24", 5)) {
+			else if (!strncmp(opt+3, "BGR24", 5))
 				pdata->interface_pix_fmt = IPU_PIX_FMT_BGR24;
-				continue;
-			}
-			if (!strncmp(opt+3, "GBR24", 5)) {
+			else if (!strncmp(opt+3, "GBR24", 5))
 				pdata->interface_pix_fmt = IPU_PIX_FMT_GBR24;
-				continue;
-			}
-			if (!strncmp(opt+3, "RGB565", 6)) {
+			else if (!strncmp(opt+3, "RGB565", 6))
 				pdata->interface_pix_fmt = IPU_PIX_FMT_RGB565;
-				continue;
-			}
-			if (!strncmp(opt+3, "RGB666", 6)) {
+			else if (!strncmp(opt+3, "RGB666", 6))
 				pdata->interface_pix_fmt = IPU_PIX_FMT_RGB666;
-				continue;
-			}
-			if (!strncmp(opt+3, "YUV444", 6)) {
+			else if (!strncmp(opt+3, "YUV444", 6))
 				pdata->interface_pix_fmt = IPU_PIX_FMT_YUV444;
-				continue;
-			}
-			if (!strncmp(opt+3, "LVDS666", 7)) {
+			else if (!strncmp(opt+3, "LVDS666", 7))
 				pdata->interface_pix_fmt = IPU_PIX_FMT_LVDS666;
-				continue;
-			}
-			if (!strncmp(opt+3, "YUYV16", 6)) {
+			else if (!strncmp(opt+3, "YUYV16", 6))
 				pdata->interface_pix_fmt = IPU_PIX_FMT_YUYV;
-				continue;
-			}
-			if (!strncmp(opt+3, "UYVY16", 6)) {
+			else if (!strncmp(opt+3, "UYVY16", 6))
 				pdata->interface_pix_fmt = IPU_PIX_FMT_UYVY;
-				continue;
-			}
-			if (!strncmp(opt+3, "YVYU16", 6)) {
+			else if (!strncmp(opt+3, "YVYU16", 6))
 				pdata->interface_pix_fmt = IPU_PIX_FMT_YVYU;
-				continue;
-			}
-			if (!strncmp(opt+3, "VYUY16", 6)) {
+			else if (!strncmp(opt+3, "VYUY16", 6))
 				pdata->interface_pix_fmt = IPU_PIX_FMT_VYUY;
-				continue;
+		} 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;
 			}
-		}
-		if (!strncmp(opt, "int_clk", 7)) {
+		} else if (!strncmp(opt, "int_clk", 7)) {
 			pdata->int_clk = true;
 			continue;
-		}
-		if (!strncmp(opt, "bpp=", 4))
+		} 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);
-		else
+
+			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;
 	}
 
@@ -2320,10 +2350,6 @@ static int mxcfb_probe(struct platform_device *pdev)
 	char buf[32];
 	int ret = 0;
 
-	ret = mxcfb_option_setup(pdev);
-	if (ret)
-		goto get_fb_option_failed;
-
 	/* Initialize FB structures */
 	fbi = mxcfb_init_fbinfo(&pdev->dev, &mxcfb_ops);
 	if (!fbi) {
@@ -2331,6 +2357,10 @@ static int mxcfb_probe(struct platform_device *pdev)
 		goto init_fbinfo_failed;
 	}
 
+	ret = mxcfb_option_setup(pdev, fbi);
+	if (ret)
+		goto get_fb_option_failed;
+
 	mxcfbi = (struct mxcfb_info *)fbi->par;
 	spin_lock_init(&mxcfbi->lock);
 	mxcfbi->fbi = fbi;
@@ -2481,8 +2511,8 @@ ipu_in_busy:
 init_dispdrv_failed:
 	fb_dealloc_cmap(&fbi->cmap);
 	framebuffer_release(fbi);
-init_fbinfo_failed:
 get_fb_option_failed:
+init_fbinfo_failed:
 	return ret;
 }
 
diff --git a/drivers/video/mxc/mxcfb_sii902x_elcdif.c b/drivers/video/mxc/mxcfb_sii902x_elcdif.c
new file mode 100644
index 000000000000..ceab3e66e91c
--- /dev/null
+++ b/drivers/video/mxc/mxcfb_sii902x_elcdif.c
@@ -0,0 +1,560 @@
+/*
+ * Copyright (C) 2010-2012 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.
+ */
+
+/*!
+ * @defgroup Framebuffer Framebuffer Driver for SDC and ADC.
+ */
+
+/*!
+ * @file mxcfb_sii902x_elcdif.c
+ *
+ * @brief MXC ELCDIF Frame buffer driver for SII902x
+ *
+ * @ingroup Framebuffer
+ */
+
+/*!
+ * Include files
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/console.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/i2c.h>
+#include <linux/mxcfb.h>
+#include <linux/fsl_devices.h>
+#include <linux/interrupt.h>
+#include <asm/mach-types.h>
+#include <mach/hardware.h>
+#include <mach/mxc_edid.h>
+
+#define SII_EDID_LEN	512
+#define MXC_ENABLE	1
+#define MXC_DISABLE	2
+
+struct sii902x_data {
+	struct platform_device *pdev;
+	struct i2c_client *client;
+	struct delayed_work det_work;
+	struct fb_info *fbi;
+	struct mxc_edid_cfg edid_cfg;
+	u8 cable_plugin;
+	u8 edid[SII_EDID_LEN];
+	bool waiting_for_fb;
+} sii902x;
+
+static void sii902x_poweron(void);
+static void sii902x_poweroff(void);
+static void (*sii902x_reset) (void);
+
+#ifdef DEBUG
+static void dump_fb_videomode(struct fb_videomode *m)
+{
+	pr_debug("fb_videomode = %d %d %d %d %d %d %d %d %d %d %d %d %d\n",
+		m->refresh, m->xres, m->yres, m->pixclock, m->left_margin,
+		m->right_margin, m->upper_margin, m->lower_margin,
+		m->hsync_len, m->vsync_len, m->sync, m->vmode, m->flag);
+}
+#else
+static void dump_fb_videomode(struct fb_videomode *m)
+{}
+#endif
+
+static __attribute__ ((unused)) void dump_regs(u8 reg, int len)
+{
+	u8 buf[50];
+	int i;
+
+	i2c_smbus_read_i2c_block_data(sii902x.client, reg, len, buf);
+	for (i = 0; i < len; i++)
+		dev_dbg(&sii902x.client->dev, "reg[0x%02X]: 0x%02X\n",
+				i+reg, buf[i]);
+}
+
+static ssize_t sii902x_show_name(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	strcpy(buf, sii902x.fbi->fix.id);
+	sprintf(buf+strlen(buf), "\n");
+
+	return strlen(buf);
+}
+
+static DEVICE_ATTR(fb_name, S_IRUGO, sii902x_show_name, NULL);
+
+static ssize_t sii902x_show_state(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	if (sii902x.cable_plugin == 0)
+		strcpy(buf, "plugout\n");
+	else
+		strcpy(buf, "plugin\n");
+
+	return strlen(buf);
+}
+
+static DEVICE_ATTR(cable_state, S_IRUGO, sii902x_show_state, NULL);
+
+static ssize_t sii902x_show_edid(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int i, j, len = 0;
+
+	for (j = 0; j < SII_EDID_LEN/16; j++) {
+		for (i = 0; i < 16; i++)
+			len += sprintf(buf+len, "0x%02X ",
+					sii902x.edid[j*16 + i]);
+		len += sprintf(buf+len, "\n");
+	}
+
+	return len;
+}
+
+static DEVICE_ATTR(edid, S_IRUGO, sii902x_show_edid, NULL);
+
+static void sii902x_setup(struct fb_info *fbi)
+{
+	u16 data[4];
+	u32 refresh;
+	u8 *tmp;
+	int i;
+
+	dev_dbg(&sii902x.client->dev, "Sii902x: setup..\n");
+
+	/* Power up */
+	i2c_smbus_write_byte_data(sii902x.client, 0x1E, 0x00);
+
+	/* set TPI video mode */
+	data[0] = PICOS2KHZ(fbi->var.pixclock) / 10;
+	data[2] = fbi->var.hsync_len + fbi->var.left_margin +
+		  fbi->var.xres + fbi->var.right_margin;
+	data[3] = fbi->var.vsync_len + fbi->var.upper_margin +
+		  fbi->var.yres + fbi->var.lower_margin;
+	refresh = data[2] * data[3];
+	refresh = (PICOS2KHZ(fbi->var.pixclock) * 1000) / refresh;
+	data[1] = refresh * 100;
+	tmp = (u8 *)data;
+	for (i = 0; i < 8; i++)
+		i2c_smbus_write_byte_data(sii902x.client, i, tmp[i]);
+
+	/* input bus/pixel: full pixel wide (24bit), rising edge */
+	i2c_smbus_write_byte_data(sii902x.client, 0x08, 0x70);
+	/* Set input format to RGB */
+	i2c_smbus_write_byte_data(sii902x.client, 0x09, 0x00);
+	/* set output format to RGB */
+	i2c_smbus_write_byte_data(sii902x.client, 0x0A, 0x00);
+	/* audio setup */
+	i2c_smbus_write_byte_data(sii902x.client, 0x25, 0x00);
+	i2c_smbus_write_byte_data(sii902x.client, 0x26, 0x40);
+	i2c_smbus_write_byte_data(sii902x.client, 0x27, 0x00);
+}
+
+#ifdef CONFIG_FB_MODE_HELPERS
+static int sii902x_read_edid(struct fb_info *fbi)
+{
+	int old, dat, ret, cnt = 100;
+	unsigned short addr = 0x50;
+
+	dev_dbg(&sii902x.pdev->dev, "%s\n", __func__);
+
+	old = i2c_smbus_read_byte_data(sii902x.client, 0x1A);
+
+	i2c_smbus_write_byte_data(sii902x.client, 0x1A, old | 0x4);
+	do {
+		cnt--;
+		msleep(10);
+		dat = i2c_smbus_read_byte_data(sii902x.client, 0x1A);
+	} while ((!(dat & 0x2)) && cnt);
+
+	if (!cnt) {
+		ret = -1;
+		goto done;
+	}
+
+	i2c_smbus_write_byte_data(sii902x.client, 0x1A, old | 0x06);
+
+	/* edid reading */
+	ret = mxc_edid_read(sii902x.client->adapter, addr,
+				sii902x.edid, &sii902x.edid_cfg, fbi);
+
+	cnt = 100;
+	do {
+		cnt--;
+		i2c_smbus_write_byte_data(sii902x.client, 0x1A, old & ~0x6);
+		msleep(10);
+		dat = i2c_smbus_read_byte_data(sii902x.client, 0x1A);
+	} while ((dat & 0x6) && cnt);
+
+	if (!cnt)
+		ret = -1;
+
+done:
+
+	i2c_smbus_write_byte_data(sii902x.client, 0x1A, old);
+	return ret;
+}
+#else
+static int sii902x_read_edid(struct fb_info *fbi)
+{
+	return -1;
+}
+#endif
+
+static void det_worker(struct work_struct *work)
+{
+	int dat;
+	char event_string[16];
+	char *envp[] = { event_string, NULL };
+
+	dev_dbg(&sii902x.pdev->dev, "%s\n", __func__);
+
+	dat = i2c_smbus_read_byte_data(sii902x.client, 0x3D);
+	if (dat & 0x1) {
+		/* cable connection changes */
+		if (dat & 0x4) {
+			sii902x.cable_plugin = 1;
+			dev_dbg(&sii902x.pdev->dev, "EVENT=plugin\n");
+			sprintf(event_string, "EVENT=plugin");
+
+			/* make sure fb is powerdown */
+			console_lock();
+			fb_blank(sii902x.fbi, FB_BLANK_POWERDOWN);
+			console_unlock();
+
+			if (sii902x_read_edid(sii902x.fbi) < 0)
+				dev_err(&sii902x.client->dev,
+					"Sii902x: read edid fail\n");
+			else {
+				if (sii902x.fbi->monspecs.modedb_len > 0) {
+
+					int i;
+					const struct fb_videomode *mode;
+					struct fb_videomode m;
+
+					fb_destroy_modelist(&sii902x.fbi->modelist);
+
+					for (i = 0; i < sii902x.fbi->monspecs.modedb_len; i++) {
+						/*FIXME now we do not support interlaced mode */
+						mode = &sii902x.fbi->monspecs.modedb[i];
+
+						if (!(mode->vmode & FB_VMODE_INTERLACED)) {
+
+							dev_dbg(&sii902x.pdev->dev, "Added mode %d:", i);
+							dev_dbg(&sii902x.pdev->dev,
+								"xres = %d, yres = %d, freq = %d, vmode = %d, flag = %d\n",
+								mode->xres, mode->yres, mode->refresh,
+								mode->vmode, mode->flag);
+
+							fb_add_videomode(mode, &sii902x.fbi->modelist);
+						}
+					}
+
+					fb_var_to_videomode(&m, &sii902x.fbi->var);
+					dump_fb_videomode(&m);
+
+					mode = fb_find_nearest_mode(&m,
+							&sii902x.fbi->modelist);
+
+					fb_videomode_to_var(&sii902x.fbi->var, mode);
+
+					sii902x.fbi->var.activate |= FB_ACTIVATE_FORCE;
+					console_lock();
+					sii902x.fbi->flags |= FBINFO_MISC_USEREVENT;
+					fb_set_var(sii902x.fbi, &sii902x.fbi->var);
+					sii902x.fbi->flags &= ~FBINFO_MISC_USEREVENT;
+					console_unlock();
+				}
+
+				console_lock();
+				fb_blank(sii902x.fbi, FB_BLANK_UNBLANK);
+				console_unlock();
+			}
+		} else {
+			sii902x.cable_plugin = 0;
+			dev_dbg(&sii902x.pdev->dev, "EVENT=plugout\n");
+			sprintf(event_string, "EVENT=plugout");
+			console_lock();
+			fb_blank(sii902x.fbi, FB_BLANK_POWERDOWN);
+			console_unlock();
+		}
+		kobject_uevent_env(&sii902x.pdev->dev.kobj, KOBJ_CHANGE, envp);
+	}
+	i2c_smbus_write_byte_data(sii902x.client, 0x3D, dat);
+
+	dev_dbg(&sii902x.pdev->dev, "exit %s\n", __func__);
+
+}
+
+static irqreturn_t sii902x_detect_handler(int irq, void *data)
+{
+	if (sii902x.fbi)
+		schedule_delayed_work(&(sii902x.det_work), msecs_to_jiffies(20));
+	else
+		sii902x.waiting_for_fb = true;
+
+	return IRQ_HANDLED;
+}
+
+static int sii902x_fb_event(struct notifier_block *nb, unsigned long val, void *v)
+{
+	struct fb_event *event = v;
+	struct fb_info *fbi = event->info;
+
+	dev_dbg(&sii902x.pdev->dev, "%s\n", __func__);
+
+	switch (val) {
+	case FB_EVENT_FB_REGISTERED:
+		if (sii902x.fbi == NULL) {
+			sii902x.fbi = fbi;
+			if (sii902x.waiting_for_fb)
+				det_worker(NULL);
+		}
+		fb_show_logo(fbi, 0);
+		break;
+	case FB_EVENT_MODE_CHANGE:
+		sii902x_setup(fbi);
+		break;
+	case FB_EVENT_BLANK:
+		if (*((int *)event->data) == FB_BLANK_UNBLANK) {
+			dev_dbg(&sii902x.pdev->dev, "FB_BLANK_UNBLANK\n");
+			sii902x_poweron();
+		} else {
+			dev_dbg(&sii902x.pdev->dev, "FB_BLANK_BLANK\n");
+			sii902x_poweroff();
+		}
+		break;
+	}
+	return 0;
+}
+
+static struct notifier_block nb = {
+	.notifier_call = sii902x_fb_event,
+};
+
+static int __devinit sii902x_probe(struct i2c_client *client,
+		const struct i2c_device_id *id)
+{
+	int i, dat, ret;
+	struct fsl_mxc_lcd_platform_data *plat = client->dev.platform_data;
+	struct fb_info edid_fbi;
+
+	dev_dbg(&sii902x.pdev->dev, "%s\n", __func__);;
+
+	sii902x.client = client;
+
+	/* Claim HDMI pins */
+	if (plat->get_pins)
+		if (!plat->get_pins())
+			return -EACCES;
+
+	if (plat->reset) {
+		sii902x_reset = plat->reset;
+		sii902x_reset();
+	}
+
+	/* Set 902x in hardware TPI mode on and jump out of D3 state */
+	if (i2c_smbus_write_byte_data(sii902x.client, 0xc7, 0x00) < 0) {
+		dev_err(&sii902x.client->dev,
+			"Sii902x: cound not find device\n");
+		return -ENODEV;
+	}
+
+	/* read device ID */
+	for (i = 10; i > 0; i--) {
+		dat = i2c_smbus_read_byte_data(sii902x.client, 0x1B);
+		printk(KERN_DEBUG "Sii902x: read id = 0x%02X", dat);
+		if (dat == 0xb0) {
+			dat = i2c_smbus_read_byte_data(sii902x.client, 0x1C);
+			printk(KERN_DEBUG "-0x%02X", dat);
+			dat = i2c_smbus_read_byte_data(sii902x.client, 0x1D);
+			printk(KERN_DEBUG "-0x%02X", dat);
+			dat = i2c_smbus_read_byte_data(sii902x.client, 0x30);
+			printk(KERN_DEBUG "-0x%02X\n", dat);
+			break;
+		}
+	}
+	if (i == 0) {
+		dev_err(&sii902x.client->dev,
+			"Sii902x: cound not find device\n");
+		return -ENODEV;
+	}
+
+	/* try to read edid */
+	ret = sii902x_read_edid(&edid_fbi);
+	if (ret < 0)
+		dev_warn(&sii902x.client->dev, "Can not read edid\n");
+
+	if (ret >= 0)
+		mxcfb_elcdif_register_mode(edid_fbi.monspecs.modedb,
+				edid_fbi.monspecs.modedb_len, MXC_DISP_DDC_DEV);
+
+	if (sii902x.client->irq) {
+		ret = request_irq(sii902x.client->irq, sii902x_detect_handler,
+				IRQF_TRIGGER_FALLING,
+				"SII902x_det", &sii902x);
+		if (ret < 0)
+			dev_warn(&sii902x.client->dev,
+				"Sii902x: cound not request det irq %d\n",
+				sii902x.client->irq);
+		else {
+			/*enable cable hot plug irq*/
+			i2c_smbus_write_byte_data(sii902x.client, 0x3c, 0x01);
+			INIT_DELAYED_WORK(&(sii902x.det_work), det_worker);
+		}
+		ret = device_create_file(&sii902x.pdev->dev, &dev_attr_fb_name);
+		if (ret < 0)
+			dev_warn(&sii902x.client->dev,
+				"Sii902x: cound not create sys node for fb name\n");
+		ret = device_create_file(&sii902x.pdev->dev, &dev_attr_cable_state);
+		if (ret < 0)
+			dev_warn(&sii902x.client->dev,
+				"Sii902x: cound not create sys node for cable state\n");
+		ret = device_create_file(&sii902x.pdev->dev, &dev_attr_edid);
+		if (ret < 0)
+			dev_warn(&sii902x.client->dev,
+				"Sii902x: cound not create sys node for edid\n");
+
+	}
+
+	fb_register_client(&nb);
+
+	dev_dbg(&sii902x.pdev->dev, "%s exit\n", __func__);;
+
+	return 0;
+}
+
+static int __devexit sii902x_remove(struct i2c_client *client)
+{
+	struct fsl_mxc_lcd_platform_data *plat = sii902x.client->dev.platform_data;
+
+	dev_dbg(&sii902x.pdev->dev, "%s\n", __func__);
+
+	fb_unregister_client(&nb);
+	sii902x_poweroff();
+
+	/* Release HDMI pins */
+	if (plat->put_pins)
+		plat->put_pins();
+
+	return 0;
+}
+
+static int sii902x_suspend(struct i2c_client *client, pm_message_t message)
+{
+	/*TODO*/
+	return 0;
+}
+
+static int sii902x_resume(struct i2c_client *client)
+{
+	/*TODO*/
+	return 0;
+}
+
+static void sii902x_poweron(void)
+{
+	struct fsl_mxc_lcd_platform_data *plat = sii902x.client->dev.platform_data;
+
+	dev_dbg(&sii902x.pdev->dev, "%s\n", __func__);
+
+	/* Enable pins to HDMI */
+	if (plat->enable_pins)
+		plat->enable_pins();
+
+	/* Turn on DVI or HDMI */
+	if (sii902x.edid_cfg.hdmi_cap)
+		i2c_smbus_write_byte_data(sii902x.client, 0x1A, 0x01);
+	else
+		i2c_smbus_write_byte_data(sii902x.client, 0x1A, 0x00);
+	return;
+}
+
+static void sii902x_poweroff(void)
+{
+	struct fsl_mxc_lcd_platform_data *plat = sii902x.client->dev.platform_data;
+
+	dev_dbg(&sii902x.pdev->dev, "%s\n", __func__);
+
+	/* disable tmds before changing resolution */
+	if (sii902x.edid_cfg.hdmi_cap)
+		i2c_smbus_write_byte_data(sii902x.client, 0x1A, 0x11);
+	else
+		i2c_smbus_write_byte_data(sii902x.client, 0x1A, 0x10);
+
+	/* Disable pins to HDMI */
+	if (plat->disable_pins)
+		plat->disable_pins();
+
+	return;
+}
+
+static const struct i2c_device_id sii902x_id[] = {
+	{ "sii902x", 0 },
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, sii902x_id);
+
+static struct i2c_driver sii902x_i2c_driver = {
+	.driver = {
+		   .name = "sii902x",
+		   },
+	.probe = sii902x_probe,
+	.remove = sii902x_remove,
+	.suspend = sii902x_suspend,
+	.resume = sii902x_resume,
+	.id_table = sii902x_id,
+};
+
+static int __init sii902x_init(void)
+{
+	int ret;
+
+	memset(&sii902x, 0, sizeof(sii902x));
+
+	sii902x.pdev = platform_device_register_simple("sii902x", 0, NULL, 0);
+	if (IS_ERR(sii902x.pdev)) {
+		printk(KERN_ERR
+				"Unable to register Sii902x as a platform device\n");
+		ret = PTR_ERR(sii902x.pdev);
+		goto err;
+	}
+
+	return i2c_add_driver(&sii902x_i2c_driver);
+err:
+	return ret;
+}
+
+static void __exit sii902x_exit(void)
+{
+	i2c_del_driver(&sii902x_i2c_driver);
+	platform_device_unregister(sii902x.pdev);
+}
+
+module_init(sii902x_init);
+module_exit(sii902x_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("SII902x DVI/HDMI driver");
+MODULE_LICENSE("GPL");